PR Feedback on earlier Arm updates

src/main/java/competition/auto_programs/FromMidShootCollectShoot.java

@@ -96,10 +96,6 @@ public FromMidShootCollectShoot(
     }
 
     private void queueMessageToAutoSelector(String message) {
-        this.addCommands(
-            new InstantCommand(() -> {
-                autoSelector.setAutonomousState(message);
-            })
-        );
+        this.addCommands(autoSelector.createAutonomousStateMessageCommand(message));
     }
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -11,6 +11,8 @@
 import competition.subsystems.arm.commands.ArmMaintainerCommand;
 import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand;
 import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand;
+import competition.subsystems.arm.commands.DisengageBrakeCommand;
+import competition.subsystems.arm.commands.EngageBrakeCommand;
 import competition.subsystems.arm.commands.ManipulateArmBrakeCommand;
 import competition.subsystems.arm.commands.SetArmAngleCommand;
 import competition.subsystems.arm.commands.SetArmExtensionCommand;
@@ -66,8 +68,8 @@ public void setupFundamentalCommands(
             SetArmAngleCommand armAngle,
             ArmMaintainerCommand armMaintainer,
             WarmUpShooterRPMCommand warmUpShooterDifferentialRPM,
-            ManipulateArmBrakeCommand engageBrake,
-            ManipulateArmBrakeCommand disengageBrake
+            EngageBrakeCommand engageBrake,
+            DisengageBrakeCommand disengageBrake
     ) {
         // Scooch
         oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get());
@@ -302,7 +304,7 @@ public void setupAdvancedOperatorCommands(
     }
 
     @Inject
-    public void setupAutonomous(OperatorInterface oi,
+    public void setupAutonomousForTesting(OperatorInterface oi,
                                 FromMidShootCollectShoot fromMidShootCollectShoot) {
         oi.operatorGamepadAdvanced.getPovIfAvailable(0).whileTrue(fromMidShootCollectShoot);
     }

src/main/java/competition/simulation/Simulator2024.java

@@ -135,7 +135,7 @@ public void update() {
             initializeNewFiredNote();
         }
         lastCollectorPower = currentCollectorPower;
-        moveNote();
+        interpolateNotePosition();
     }
 
     double timeNoteFired = 0;
@@ -167,7 +167,10 @@ private void initializeNewFiredNote() {
         );
     }
 
-    private void moveNote() {
+    /**
+     * Interpolates the position of the note based on the time since it was fired.
+     */
+    private void interpolateNotePosition() {
         double timeSinceNoteFired = XTimer.getFPGATimestamp() - timeNoteFired;
         if (noteStartPoint != null && noteFinishPoint != null) {
             var interpolatedPosition = noteStartPoint.interpolate(noteFinishPoint, timeSinceNoteFired);

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -73,6 +73,7 @@ public class ArmSubsystem extends BaseSetpointSubsystem<Double> implements DataF
     private double timeSinceNewTarget = -Double.MAX_VALUE;
     private final DoubleProperty powerRampDurationSec;
     private boolean powerRampingEnabled = true;
+    private boolean dynamicBrakingEnabled = false;
 
     public enum ArmState {
         EXTENDING,
@@ -333,6 +334,14 @@ else if (distanceLeftAhead < 0) {
             rightPower = 0;
         }
 
+        // Engage brake if no power commanded
+        if (leftPower == 0 && rightPower == 0) {
+            setBrakeEnabled(true);
+        } else {
+            // Disengage brake if any power commanded.
+            setBrakeEnabled(false);
+        }
+
         if (contract.isArmReady()) {
             armMotorLeft.set(leftPower);
             armMotorRight.set(rightPower);
@@ -369,6 +378,7 @@ public void setPower(Double power) {
     }
 
     public void dangerousManualSetPowerToBothArms(double power) {
+        setBrakeEnabled(false);
         armMotorLeft.set(power);
         armMotorRight.set(power);
     }

src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java

@@ -25,8 +25,6 @@ public class ArmMaintainerCommand extends BaseMaintainerCommand<Double> {
     TimeStableValidator calibrationValidator;
     final double calibrationStallDurationSec = 0.5;
 
-    private boolean dynamicBrakingEnabled = false;
-
     @Inject
     public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf,
                                 HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory,
@@ -56,15 +54,8 @@ protected void calibratedMachineControlAction() {
         // The arms can draw huge currents when trying to move small values, so if we are on target
         // then we need to kill power.
         if (isMaintainerAtGoal()) {
-            if (dynamicBrakingEnabled) {
-                // Engage the brake so we don't backdrive away from this point
-                // (vibrations from the shooter can cause that)
-                arm.setBrakeEnabled(true);
-            }
             arm.setPower(0.0);
         } else {
-            // If we need to move, disengage the brake
-            arm.setBrakeEnabled(false);
             double power = positionPid.calculate(arm.getTargetValue(), arm.getCurrentValue());
             arm.setPower(power);
         }
@@ -77,8 +68,6 @@ protected void uncalibratedMachineControlAction() {
         aKitLog.record("Started Calibration", startedCalibration);
         aKitLog.record("Given Up On Calibration", givenUpOnCalibration);
 
-        arm.setBrakeEnabled(false);
-
         // Try to auto-calibrate.
         if (!startedCalibration) {
             calibrationStartTime = XTimer.getFPGATimestamp();
@@ -120,15 +109,6 @@ protected void uncalibratedMachineControlAction() {
         }
     }
 
-    @Override
-    protected void humanControlAction() {
-        // Disable the brake before letting the human take over.
-        if (dynamicBrakingEnabled) {
-            arm.setBrakeEnabled(false);
-        }
-        super.humanControlAction();
-    }
-
     @Override
     protected double getErrorMagnitude() {
             double current = arm.getCurrentValue();

src/main/java/competition/subsystems/arm/commands/DisengageBrakeCommand.java

@@ -0,0 +1,14 @@
+package competition.subsystems.arm.commands;
+
+import competition.subsystems.arm.ArmSubsystem;
+
+import javax.inject.Inject;
+
+public class DisengageBrakeCommand extends ManipulateArmBrakeCommand {
+
+    @Inject
+    public DisengageBrakeCommand(ArmSubsystem arm) {
+        super(arm);
+        this.setBrakeMode(false);
+    }
+}

src/main/java/competition/subsystems/arm/commands/EngageBrakeCommand.java

@@ -0,0 +1,14 @@
+package competition.subsystems.arm.commands;
+
+import competition.subsystems.arm.ArmSubsystem;
+
+import javax.inject.Inject;
+
+public class EngageBrakeCommand extends ManipulateArmBrakeCommand {
+
+    @Inject
+    public EngageBrakeCommand(ArmSubsystem arm) {
+        super(arm);
+        this.setBrakeMode(true);
+    }
+}