diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65a5b34..9ea7489 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -103,7 +103,7 @@ public class Ports { } public final class ShooterConstants { - // Constants for the Shooter + // Constants for the Shooter public static final Angle ANGLE_TOLERANCE = Rotations.of(0.01); public static final AngularVelocity ANGLE_VELOCITY_TOLERANCE = RotationsPerSecond.of(0.01); public static final AngularVelocity CRUISE_VELOCITY = RotationsPerSecond.of(204); @@ -116,10 +116,15 @@ public final class ShooterConstants { public static final Angle MAX_ANGLE = Rotations.of(10.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); + public static final RotaryMechCharacteristics CONSTANTS = + RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + public static final double IDLE_SPEED_RPM = (1.0); + public static final double HUB_SPEED_RPM = (1.0); + public static final double TOWER_SPEED_RPM = (1.0); + public static final double DEFAULT_SPEED_RPM = (1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; - public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } - + public static final int CANDLE_ID = 50; public class IntakeConstants { @@ -150,6 +155,12 @@ public class ClimberConstants { public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); - public static final LinearMechCharacteristics CHARACTERISTICS = new LinearMechCharacteristics(new Translation3d(0.0, 0.0, 0.0), MIN_DISTANCE, MAX_DISTANCE, STARTING_DISTANCE, CONVERTER); + public static final LinearMechCharacteristics CHARACTERISTICS = + new LinearMechCharacteristics( + new Translation3d(0.0, 0.0, 0.0), + MIN_DISTANCE, + MAX_DISTANCE, + STARTING_DISTANCE, + CONVERTER); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index f3170b0..a9d351e 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -14,19 +14,14 @@ public class Hopper extends SubsystemBase { private FlywheelMechanism _io; public Hopper(FlywheelMechanism io) { - _io = io; + _io = io; } public void setGoal(double position) { Distance positionInches = Inches.of(position); - _io.runPosition(HopperConstants.CONVERTER.toAngle(positionInches), HopperConstants.ANGULAR_VELOCITY, HopperConstants.ANGULAR_ACCELERATION, null, PIDSlot.SLOT_0); + _io.runPosition(HopperConstants.CONVERTER.toAngle(positionInches), PIDSlot.SLOT_0); } - - public void runHopper() { - _io.runVoltage(HopperConstants.VOLTAGE); - } @Override public void periodic() {} - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ba3067c..bcb6880 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,6 +8,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; +import frc.robot.Constants.ShooterConstants; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Second; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.FeederConstants; import frc.robot.Constants.ShooterConstants; @@ -36,6 +47,20 @@ public void setFlywheelVelocity(double velocity) { _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } + public enum State { + OFF(Units.RevolutionsPerSecond.of(0.0)), + IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM/60)), + SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM/60)), + SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM/60)), + SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM/60)), + SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM/60)); + + private final AngularVelocity stateVelocity; + State(AngularVelocity stateVelocity) { + this.stateVelocity = stateVelocity; + } + } + // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond))