diff --git a/src/main/java/frc/robot/shooter/ShooterRpms.java b/src/main/java/frc/robot/shooter/ShooterRpms.java new file mode 100644 index 0000000..7faa506 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterRpms.java @@ -0,0 +1,8 @@ +package frc.robot.shooter; + +public class ShooterRpms { + public static final double SUBWOOFER = 3000.0; + public static final double DROP = 500.0; + public static final double PODIUM = 4000.0; + public static final double IDLE_WITH_GP = 1000.0; +} diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java new file mode 100644 index 0000000..15c57c0 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -0,0 +1,11 @@ +package frc.robot.shooter; + +public enum ShooterState { + IDLE_WITH_GP, + IDLE_NO_GP, + SPEAKER_SHOT, + DROP, + SUBWOOFER_SHOT, + FEEDING, + PODIUM_SHOT; +} diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java new file mode 100644 index 0000000..43e6d39 --- /dev/null +++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java @@ -0,0 +1,84 @@ +package frc.robot.shooter; + +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import frc.robot.util.scheduling.SubsystemPriority; +import frc.robot.util.state_machines.StateMachine; + +public class ShooterSubsystem extends StateMachine { + private final TalonFX motor; + private double shooterRPM; + private final VelocityVoltage velocityRequest = + new VelocityVoltage(0).withEnableFOC(false).withLimitReverseMotion(true); + private double distanceToSpeaker; + private double distanceToFeedSpot; + private InterpolatingDoubleTreeMap speakerDistanceToRpm = new InterpolatingDoubleTreeMap(); + private InterpolatingDoubleTreeMap feedSpotDistanceToRpm = new InterpolatingDoubleTreeMap(); + + public void setDistanceToSpeaker(double distance) { + distanceToSpeaker = distance; + } + + public void setDistanceToFeedSpot(double distance) { + distanceToFeedSpot = distance; + } + + public ShooterSubsystem(TalonFX motor) { + super(SubsystemPriority.SHOOTER, ShooterState.IDLE_NO_GP); + this.motor = motor; + + // TODO: Tune lookup table + speakerDistanceToRpm.put(123.0, 321.0); + + feedSpotDistanceToRpm.put(123.0, 321.0); + } + + public boolean atGoal() { + return switch (getState()) { + case SUBWOOFER_SHOT -> MathUtil.isNear(ShooterRpms.SUBWOOFER, shooterRPM, 50); + case IDLE_WITH_GP, IDLE_NO_GP->true; + case PODIUM_SHOT-> MathUtil.isNear(ShooterRpms.PODIUM,shooterRPM,50); + case DROP->MathUtil.isNear(ShooterRpms.DROP,shooterRPM,50); + case FEEDING->MathUtil.isNear(feedSpotDistanceToRpm.get(distanceToFeedSpot),shooterRPM,50); + case SPEAKER_SHOT->MathUtil.isNear(speakerDistanceToRpm.get(distanceToSpeaker), shooterRPM, 50); + }; + } + + public void setState(ShooterState newState) { + setStateFromRequest(newState); + } + + @Override + protected void collectInputs() { + shooterRPM = motor.getVelocity().getValueAsDouble() * 60.0; + } + + @Override + protected ShooterState getNextState(ShooterState currentState) { + // The shooter has no sensor, so some other manager will need to set shooter state + return currentState; + } + + @Override + protected void afterTransition(ShooterState newState) { + switch (newState) { + case IDLE_NO_GP -> motor.disable(); + case IDLE_WITH_GP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WITH_GP / 60.0)); + case SPEAKER_SHOT -> + motor.setControl( + velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToSpeaker) / 60.0)); + case SUBWOOFER_SHOT -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.SUBWOOFER / 60.0)); + case FEEDING -> + motor.setControl( + velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToFeedSpot) / 60.0)); + case DROP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); + case PODIUM_SHOT -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); + } + } + + @Override + public void robotPeriodic() {} +}