feat(arm): Add new home sequence.

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

@@ -185,10 +185,6 @@ private void setSetpoint(ArmState setpoint) {
     wristMotor.setSetpoint(setpoint.wrist().position, setpoint.wrist().velocity);
   }
 
-  public Command home() {
-    return run(() -> shoulderMotor.setVoltage(-1)).until(() -> limitSwitchValues.isPressed);
-  }
-
   private MoveShoulderCommand moveShoulder(ArmState goal) {
     return new MoveShoulderCommand(goal);
   }
@@ -205,11 +201,18 @@ public Command moveWristThenShoulder(ArmState goal) {
     return moveWrist(goal).andThen(moveShoulder(goal));
   }
 
-  public Command stowFromUp() {
-    return moveWristThenShoulder(ArmState.STOW);
-  }
-
   public Command amp() {
     return moveShoulderThenWrist(ArmState.AMP);
   }
+
+  public Command home() {
+    return moveWrist(ArmState.STOW)
+        .andThen(moveShoulder(ArmState.STOW).until(() -> limitSwitchValues.isPressed))
+        .finallyDo(
+            interrupted -> {
+              if (!interrupted) {
+                setPosition(ArmState.STOW);
+              }
+            });
+  }
 }

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

@@ -29,7 +29,7 @@ public static class ShoulderMotorConstants {
             1);
 
     /** Minimum angle of the shoulder joint. */
-    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(27.5);
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(29.5);
 
     /** Maximum angle of the shoulder joint. */
     public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90);
@@ -45,11 +45,11 @@ public static class ShoulderMotorConstants {
     public static final double KP = 36.0;
 
     /** Maximum speed of the shoulder joint in rotations per second. */
-    public static final double MAXIMUM_SPEED = 1.0;
+    public static final double MAXIMUM_SPEED = 0.5;
 
     /** Maximum acceleration of the shoulder joint in rotations per second per second. */
     public static final double MAXIMUM_ACCELERATION =
-        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.5);
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1);
 
     /** Maximum speed and acceleration of the shoulder joint. */
     public static final TrapezoidProfile.Constraints CONSTRAINTS =

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

@@ -11,6 +11,9 @@
 /** State of the arm. */
 public record ArmState(State shoulder, State wrist) {
 
+  public static final ArmState INIT =
+      new ArmState(Rotation2d.fromDegrees(52.5), Rotation2d.fromDegrees(-35));
+
   public static final ArmState STOW =
       new ArmState(ShoulderMotorConstants.MINIMUM_ANGLE, WristMotorConstants.MAXIMUM_ANGLE);
   public static final ArmState SHOOT = STOW.withWrist(Rotation2d.fromDegrees(23.265));