Feature disable motion magic

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -222,10 +222,7 @@ private void configureButtonBindings() {
     // Shoot
     Trigger readyToShoot =
         new Trigger(
-            () ->
-                drive.isAutoAimGoalCompleted()
-                    && superstructure.atShootingSetpoint()
-                    && flywheels.atGoal());
+            () -> drive.isAutoAimGoalCompleted() && superstructure.atGoal() && flywheels.atGoal());
     readyToShoot
         .whileTrue(
             Commands.run(

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java

@@ -62,14 +62,9 @@ public void periodic() {
     Logger.recordOutput("Superstructure/CurrentState", currentGoal);
   }
 
-  @AutoLogOutput(key = "Superstructure/ReadyToShoot")
-  public boolean atShootingSetpoint() {
-    return currentGoal == Goal.AIM && arm.atSetpoint();
-  }
-
   @AutoLogOutput(key = "Superstructure/CompletedGoal")
   public boolean atGoal() {
-    return currentGoal == desiredGoal && arm.atSetpoint();
+    return currentGoal == desiredGoal && arm.atGoal();
   }
 
   public void stow() {

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java

@@ -71,6 +71,7 @@ private double getRads() {
   private final ArmIO io;
   private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
   private TrapezoidProfile motionProfile;
+  private TrapezoidProfile.State setpointState = new TrapezoidProfile.State();
   private ArmFeedforward ff;
 
   private final ArmVisualizer measuredVisualizer;
@@ -118,7 +119,7 @@ public void periodic() {
 
     if (!characterizing) {
       // Run closed loop
-      var output =
+      setpointState =
           motionProfile.calculate(
               0.02,
               new TrapezoidProfile.State(inputs.armPositionRads, inputs.armVelocityRadsPerSec),
@@ -129,20 +130,20 @@ public void periodic() {
                       Units.degreesToRadians(upperLimitDegrees.get())),
                   0.0));
 
-      io.runSetpoint(output.position, ff.calculate(output.position, output.velocity));
-
-      // Logs
-      Logger.recordOutput("Arm/SetpointAngle", output.position);
-      Logger.recordOutput("Arm/SetpointVelocity", output.velocity);
-      setpointVisualizer.update(Rotation2d.fromRadians(output.position));
-      goalVisualizer.update(Rotation2d.fromRadians(goal.getRads()));
+      io.runSetpoint(
+          setpointState.position, ff.calculate(setpointState.position, setpointState.velocity));
     }
 
     if (DriverStation.isDisabled()) {
       io.stop();
     }
 
-    measuredVisualizer.update(Rotation2d.fromRadians(inputs.armPositionRads));
+    // Logs
+    measuredVisualizer.update(inputs.armPositionRads);
+    setpointVisualizer.update(setpointState.position);
+    goalVisualizer.update(goal.getRads());
+    Logger.recordOutput("Arm/SetpointAngle", setpointState.position);
+    Logger.recordOutput("Arm/SetpointVelocity", setpointState.velocity);
     Logger.recordOutput("Arm/Goal", goal);
   }
 
@@ -155,9 +156,9 @@ public Rotation2d getSetpoint() {
     return Rotation2d.fromRadians(goal.getRads());
   }
 
-  @AutoLogOutput(key = "Arm/AtSetpoint")
-  public boolean atSetpoint() {
-    return Math.abs(inputs.armPositionRads - goal.getRads())
+  @AutoLogOutput(key = "Arm/AtGoal")
+  public boolean atGoal() {
+    return Math.abs(setpointState.position - goal.getRads())
         <= Units.degreesToRadians(toleranceDegrees.get());
   }
 

src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java

@@ -21,27 +21,25 @@
 
 public class ArmVisualizer {
   private final Mechanism2d mechanism;
-  private final MechanismRoot2d root;
   private final MechanismLigament2d arm;
   private final String key;
 
   public ArmVisualizer(String key, Color color) {
     this.key = key;
     mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kWhite));
-    root = mechanism.getRoot("pivot", 1.0, 0.4);
+    MechanismRoot2d root = mechanism.getRoot("pivot", 1.0, 0.4);
     arm = new MechanismLigament2d("arm", armLength, 20.0, 6, new Color8Bit(color));
     root.append(arm);
   }
 
   /** Update arm visualizer with current arm angle */
-  public void update(Rotation2d angle) {
-    arm.setAngle(angle);
+  public void update(double angleRads) {
+    arm.setAngle(Rotation2d.fromRadians(angleRads));
     Logger.recordOutput("Arm/" + key + "Mechanism2d", mechanism);
 
     // Log 3d poses
     Pose3d pivot =
-        new Pose3d(
-            armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angle.getRadians(), 0.0));
+        new Pose3d(armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angleRads, 0.0));
     Logger.recordOutput("Arm/" + key + "3d", pivot);
   }
 }