offboard shooter control

src/main/java/frc/robot/Autos.java

@@ -2,9 +2,8 @@
 
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoControlFunction;
-import com.pathplanner.lib.util.GeometryUtil;
+import com.choreo.lib.ChoreoTrajectory;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -97,13 +96,13 @@ private Command getPathFollowingCommand(String trajectoryName) {
   }
 
   private Command resetPose(String trajectoryName) {
-    Pose2d startingPose = Choreo.getTrajectory(trajectoryName).getInitialPose();
+    ChoreoTrajectory trajectory = Choreo.getTrajectory(trajectoryName);
     return Commands.runOnce(
         () ->
             m_drivetrain.resetPose.accept(
                 Constants.onRedAllianceSupplier.getAsBoolean()
-                    ? GeometryUtil.flipFieldPose(startingPose)
-                    : startingPose));
+                    ? trajectory.getFlippedInitialPose()
+                    : trajectory.getInitialPose()));
   }
 
   private Command intake() {

src/main/java/frc/robot/Constants.java

@@ -12,7 +12,7 @@
 import java.util.function.BooleanSupplier;
 
 public final class Constants {
-  public static final Mode kCurrentMode = Mode.kReal;
+  public static final Mode kCurrentMode = Mode.kSim;
   public static final RobotName kRobot = RobotName.kViper;
   public static final boolean kIsViper = kRobot.equals(RobotName.kViper);
   public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim);

src/main/java/frc/robot/Robot.java

@@ -232,7 +232,7 @@ public Robot() {
     // m_driverController.x().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap));
     m_driverController.y().whileTrue(Commands.parallel(m_climber.windWinch()));
 
-    m_driverController.povRight().onTrue(m_shooter.trap());
+    m_driverController.povRight().onTrue(m_shooter.trapShot());
 
     m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake));
     m_operatorController

src/main/java/frc/robot/arm/Arm.java

@@ -46,8 +46,9 @@ public class Arm extends SubsystemBase {
       new LoggedTunableNumber("Arm/Wrist/MaxAcceleration");
 
   private Vector<N2> m_initialAngles;
-  private boolean m_atGoal = false;
-  public Trigger atGoal = new Trigger(this::getJointsAtGoal).debounce(0.1, DebounceType.kBoth);
+  private Rotation2d m_lastElbowSetpoint;
+  private Rotation2d m_lastWristSetpoint;
+  public Trigger atGoal = new Trigger(this::jointsAtGoal).debounce(0.1, DebounceType.kBoth);
   public Supplier<Pose3d> wristPoseSupplier;
   private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist;
 
@@ -135,6 +136,8 @@ private void runSetpoint(
       throw new IllegalArgumentException("Only one joint can be delayed at a time.");
     if (elbowDelay < 0.0 || wristDelay < 0.0)
       throw new IllegalArgumentException("Percent delay can't be negative.");
+    m_lastElbowSetpoint = new Rotation2d(elbowAngleRad);
+    m_lastWristSetpoint = new Rotation2d(wristAngleRad);
     Logger.recordOutput("Arm/Goal Elbow Angle", Units.radiansToDegrees(elbowAngleRad));
     Logger.recordOutput("Arm/Goal Wrist Angle", Units.radiansToDegrees(wristAngleRad));
     m_goalVisualizer.update(elbowAngleRad, wristAngleRad);
@@ -146,30 +149,29 @@ private void runSetpoint(
             / Math.abs(Units.degreesToRotations(wristAngleRad) - m_initialAngles.get(1, 0));
     Logger.recordOutput("Arm/Elbow Progress", elbowProgress);
     Logger.recordOutput("Arm/Wrist Progress", wristProgress);
-    m_atGoal = anglesAtGoal(new Rotation2d(elbowAngleRad), new Rotation2d(wristAngleRad));
 
     if (wristProgress < elbowDelay) m_io.setElbowRotations(m_inputs.elbowPositionRot);
     else m_io.setElbowRotations(Units.radiansToRotations(elbowAngleRad));
     if (elbowProgress < wristDelay) m_io.setWristRotations(m_inputs.wristPositionRot);
     else m_io.setWristRotations(Units.radiansToRotations(wristAngleRad));
   }
 
-  private boolean anglesAtGoal(Rotation2d elbowGoal, Rotation2d wristGoal) {
+  @AutoLogOutput(key = "Arm/JointsAtGoal")
+  private boolean jointsAtGoal() {
     return (MathUtil.isNear(
             0.0,
-            elbowGoal.minus(Rotation2d.fromRotations(m_inputs.elbowPositionRot)).getDegrees(),
+            m_lastElbowSetpoint
+                .minus(Rotation2d.fromRotations(m_inputs.elbowPositionRot))
+                .getDegrees(),
             kJointTolerenceDegrees)
         && MathUtil.isNear(
             0.0,
-            wristGoal.minus(Rotation2d.fromRotations(m_inputs.wristPositionRot)).getDegrees(),
+            m_lastWristSetpoint
+                .minus(Rotation2d.fromRotations(m_inputs.wristPositionRot))
+                .getDegrees(),
             kJointTolerenceDegrees));
   }
 
-  @AutoLogOutput(key = "Arm/JointsAtGoal")
-  private boolean getJointsAtGoal() {
-    return m_atGoal;
-  }
-
   public Command storeInitialAngles() {
     return this.runOnce(
         () ->
@@ -191,7 +193,6 @@ else if (jointIndexSupplier.getAsInt() == 1)
                         ArmSetpoints.kStowed.elbowAngle, angleSupplier.getAsDouble(), 0.0, 0.0);
                   else throw new IllegalArgumentException("Invalid joint index.");
                 }))
-        .finallyDo(() -> m_atGoal = false)
         .withName("Aim (Arm)");
   }
 

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -52,7 +52,6 @@ public ArmIOTalonFX() {
     currentLimitsConfig.StatorCurrentLimit = 60.0;
     currentLimitsConfig.StatorCurrentLimitEnable = true;
     final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration();
-    m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig);
     elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
     elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
@@ -72,7 +71,6 @@ public ArmIOTalonFX() {
     m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0);
 
     final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration();
-    m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig);
     wristLeftMotorConfig.CurrentLimits = currentLimitsConfig;
     wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
     wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
@@ -93,14 +91,12 @@ public ArmIOTalonFX() {
     m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0);
 
     final TalonFXConfiguration elbowRightMotorConfig = new TalonFXConfiguration();
-    m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig);
     elbowRightMotorConfig.CurrentLimits = currentLimitsConfig;
     elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig, 1.0);
     m_elbowRightFollowerMotor.setControl(new Follower(m_elbowLeftMotor.getDeviceID(), true));
 
     final TalonFXConfiguration wristRightMotorConfig = new TalonFXConfiguration();
-    m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig);
     wristRightMotorConfig.CurrentLimits = currentLimitsConfig;
     wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig, 1.0);

src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java

@@ -94,7 +94,6 @@ public ModuleIOTalonFX(int index) {
     }
 
     final CANcoderConfiguration azimuthEncoderConfig = new CANcoderConfiguration();
-    m_azimuthEncoder.getConfigurator().apply(azimuthEncoderConfig); // Factory Default
     azimuthEncoderConfig.MagnetSensor.AbsoluteSensorRange =
         AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
     azimuthEncoderConfig.MagnetSensor.SensorDirection =
@@ -103,7 +102,6 @@ public ModuleIOTalonFX(int index) {
     m_azimuthEncoder.getConfigurator().apply(azimuthEncoderConfig);
 
     final TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration();
-    m_driveMotor.getConfigurator().apply(driveMotorConfig); // Factory Default
     driveMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     driveMotorConfig.Feedback.SensorToMechanismRatio = Module.kDriveRatio;
     driveMotorConfig.Slot0.kP = kDrivekP;
@@ -116,7 +114,6 @@ public ModuleIOTalonFX(int index) {
     m_driveMotor.setPosition(0.0);
 
     final TalonFXConfiguration steerMotorConfig = new TalonFXConfiguration();
-    m_steerMotor.getConfigurator().apply(steerMotorConfig); // Factory Default
     steerMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
     steerMotorConfig.Feedback.FeedbackRemoteSensorID = m_azimuthEncoder.getDeviceID();
     steerMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;

src/main/java/frc/robot/shooter/Shooter.java

@@ -2,8 +2,7 @@
 
 import static edu.wpi.first.units.Units.Volts;
 
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -16,8 +15,9 @@
 
 public class Shooter extends SubsystemBase {
 
-  private final double kFarShotVelocityRpm = 5600.0;
-  private final double kAmpshot = 5000.0;
+  public static final double kFarShotVelocityRpm = 5600.0;
+  private final double kTrapShot = 400.0;
+  private final double kAmpShot = 5000.0;
   private final double kReadyToShootToleranceRps = 3.0;
 
   // Denominator for gains here are in rotations
@@ -28,30 +28,21 @@ public class Shooter extends SubsystemBase {
   public static final double bottomRollerkV = 0.12041;
   public static final double bottomRollerkA = 0.0071461;
 
-  private final double topRollerkP = 0.13085;
-  private final double bottomRollerkP = 0.11992;
+  public static final double topRollerkP = 0.13085;
+  public static final double bottomRollerkP = 0.11992;
 
-  private final PIDController m_topRollerController, m_bottomRollerController;
-  private final SimpleMotorFeedforward m_topRollerFeedforward, m_bottomRollerFeedforward;
   private final SysIdRoutine m_sysIdRoutine;
-
   private final ShooterIO m_io;
   private final ShooterIOInputsAutoLogged m_inputs = new ShooterIOInputsAutoLogged();
 
   public final Trigger readyToShoot =
-      new Trigger(() -> getRollersAtSetpoint()).debounce(0.1, DebounceType.kBoth);
+      new Trigger(this::getRollersAtSetpoint).debounce(0.1, DebounceType.kBoth);
+
+  private double m_lastSetpoint;
 
   public Shooter(ShooterIO io) {
     m_io = io;
 
-    m_topRollerFeedforward = new SimpleMotorFeedforward(topRollerkS, topRollerkV, topRollerkA);
-    m_topRollerController = new PIDController(topRollerkP, 0.0, 0.0);
-    m_bottomRollerFeedforward =
-        new SimpleMotorFeedforward(bottomRollerkS, bottomRollerkV, bottomRollerkA);
-    m_bottomRollerController = new PIDController(bottomRollerkP, 0.0, 0.0);
-
-    m_topRollerController.setTolerance(kReadyToShootToleranceRps);
-    m_bottomRollerController.setTolerance(kReadyToShootToleranceRps);
     m_sysIdRoutine =
         new SysIdRoutine(
             new SysIdRoutine.Config(
@@ -78,19 +69,17 @@ public void periodic() {
   private void setRollersSetpointRpm(double velocityRpm) {
     double goalVelocityRps = velocityRpm / 60.0;
     Logger.recordOutput("Shooter/Goal Roller RPS", goalVelocityRps);
-    double topRollerFeedforwardOutput = m_topRollerFeedforward.calculate(goalVelocityRps);
-    double bottomRollerFeedforwardOutput = m_bottomRollerFeedforward.calculate(goalVelocityRps);
-    double topRollerFeedbackOutput =
-        m_topRollerController.calculate(m_inputs.topRollerVelocityRps, goalVelocityRps);
-    double bottomRollerFeedbackOutput =
-        m_bottomRollerController.calculate(m_inputs.bottomRollerVelocityRps, goalVelocityRps);
-    m_io.setTopRollerVoltage(topRollerFeedforwardOutput + topRollerFeedbackOutput);
-    m_io.setBottomRollerVoltage(bottomRollerFeedforwardOutput + bottomRollerFeedbackOutput);
+    m_lastSetpoint = goalVelocityRps;
+    m_io.setTopRollerVelocity(goalVelocityRps);
+    m_io.setBottomRollerVelocity(goalVelocityRps);
   }
 
   @AutoLogOutput(key = "Shooter/RollersAtSetpoint")
   private boolean getRollersAtSetpoint() {
-    return m_bottomRollerController.atSetpoint() && m_topRollerController.atSetpoint();
+    return MathUtil.isNear(
+            m_lastSetpoint, m_inputs.bottomRollerVelocityRps, kReadyToShootToleranceRps)
+        && MathUtil.isNear(
+            m_lastSetpoint, m_inputs.topRollerVelocityRps, kReadyToShootToleranceRps);
   }
 
   public Command sysId() {
@@ -118,35 +107,19 @@ public Command stop() {
   }
 
   public Command runShooter() {
-    return this.run(
-            () -> {
-              setRollersSetpointRpm(kFarShotVelocityRpm);
-            })
-        .finallyDo(
-            () -> {
-              Logger.recordOutput("Shooter/Goal Roller RPS", 0.0);
-              m_topRollerController.reset();
-              m_bottomRollerController.reset();
-            })
-        .withName("Spin Up (Shooter)");
+    return this.run(() -> setRollersSetpointRpm(kFarShotVelocityRpm))
+        .finallyDo(() -> Logger.recordOutput("Shooter/Goal Roller RPS", 0.0))
+        .withName("Run Shooter (Shooter)");
   }
 
-  public Command trap() {
-    return this.run(
-            () -> {
-              setRollersSetpointRpm(2800);
-            })
-        .finallyDo(
-            () -> {
-              Logger.recordOutput("Shooter/Goal Roller RPS", 0.0);
-              m_topRollerController.reset();
-              m_bottomRollerController.reset();
-            })
-        .withName("Spin Up (Shooter)");
+  public Command trapShot() {
+    return this.run(() -> setRollersSetpointRpm(kTrapShot))
+        .finallyDo(() -> Logger.recordOutput("Shooter/Goal Roller RPS", 0.0))
+        .withName("Trap Shot (Shooter)");
   }
 
   public Command ampShot() {
-    return this.run(() -> setRollersSetpointRpm(kAmpshot));
+    return this.run(() -> setRollersSetpointRpm(kAmpShot));
   }
 
   public Command catchNote() {
@@ -166,10 +139,4 @@ public Command spit() {
             })
         .withName("Spit (Shooter)");
   }
-
-  public Command shootWithFeederDelay() {
-    return this.run(() -> setRollersSetpointRpm(kFarShotVelocityRpm))
-        .until(() -> getRollersAtSetpoint())
-        .andThen(runShooter());
-  }
 }

src/main/java/frc/robot/shooter/ShooterIOSim.java

@@ -1,6 +1,8 @@
 package frc.robot.shooter;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
@@ -20,13 +22,23 @@ public class ShooterIOSim implements ShooterIO {
   private final DCMotorSim m_bottomRollerSim =
       new DCMotorSim(m_bottomRollerPlant, m_rollerGearbox, 1.0);
 
+  private final SimpleMotorFeedforward m_topRollerFeedforward =
+      new SimpleMotorFeedforward(Shooter.topRollerkS, Shooter.topRollerkV, Shooter.topRollerkA);
+  private final PIDController m_topRollerController =
+      new PIDController(Shooter.topRollerkP, 0.0, 0.0);
+  private final SimpleMotorFeedforward m_bottomRollerFeedforward =
+      new SimpleMotorFeedforward(
+          Shooter.bottomRollerkS, Shooter.bottomRollerkV, Shooter.bottomRollerkA);
+  private final PIDController m_bottomRollerController =
+      new PIDController(Shooter.bottomRollerkP, 0.0, 0.0);
+
   private double m_topRollerAppliedVolts = 0.0;
   private double m_bottomRollerAppliedVolts = 0.0;
 
+  public ShooterIOSim() {}
+
   @Override
   public void updateInputs(ShooterIOInputs inputs) {
-    m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts);
-    m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts);
     m_topRollerSim.update(0.02);
     m_bottomRollerSim.update(0.02);
 
@@ -57,11 +69,29 @@ public void updateInputs(ShooterIOInputs inputs) {
   @Override
   public void setTopRollerVoltage(double volts) {
     m_topRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+    m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts);
   }
 
   @Override
   public void setBottomRollerVoltage(double volts) {
     m_bottomRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
+    m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts);
+  }
+
+  @Override
+  public void setTopRollerVelocity(double velocityRps) {
+    setTopRollerVoltage(
+        m_topRollerFeedforward.calculate(velocityRps)
+            + m_topRollerController.calculate(
+                m_topRollerSim.getAngularVelocityRadPerSec(), velocityRps));
+  }
+
+  @Override
+  public void setBottomRollerVelocity(double velocityRps) {
+    setBottomRollerVoltage(
+        m_bottomRollerFeedforward.calculate(velocityRps)
+            + m_bottomRollerController.calculate(
+                m_bottomRollerSim.getAngularVelocityRadPerSec(), velocityRps));
   }
 
   @Override