Proper arm state machine logic

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

@@ -27,6 +27,8 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.units.Unit;
+import frc.robot.subsystems.arm.ArmPose;
+import frc.robot.subsystems.arm.ArmSubsystem;
 import frc.robot.subsystems.drive.module.ModuleConstants;
 
 /**
@@ -221,11 +223,16 @@ private ArmConstants() {}
   }
 
   public static class ArmSetpoints {
-    public record ArmSetpoint(double armPoseDegs, double wristPoseDegs) {}
+    private ArmSetpoints() {
+      throw new IllegalStateException("Static classes should not be constructed");
+    }
 
-    public static final ArmSetpoint STOW_SETPOINT = new ArmSetpoint(0.0, 45.0);
-    public static final ArmSetpoint INTAKE_SETPOINT = new ArmSetpoint(0.0, 35.0);
-    public static final ArmSetpoint AMP_SETPOINT = new ArmSetpoint(90.0, 135.0);
+    public static final ArmPose STOW_SETPOINT = new
+        ArmPose("ArmPoses/Stow", false, 0.0, 45.0);
+    public static final ArmPose INTAKE_SETPOINT =
+        new ArmPose("ArmPoses/Intake", false, 0.0, 35.0);
+    public static final ArmPose AMP_SETPOINT =
+        new ArmPose("ArmPoses/Amp", false, 90.0, 135.0);
   }
 
   public static class ShooterConstants {

src/main/java/frc/robot/subsystems/arm/ArmPose.java

@@ -0,0 +1,42 @@
+package frc.robot.subsystems.arm;
+
+import com.gos.lib.properties.GosDoubleProperty;
+
+public class ArmPose {
+  private final GosDoubleProperty m_wristAngleProperty;
+  private final GosDoubleProperty m_armAngleProperty;
+
+  private final double m_armAngle;
+  private final double m_wristAngle;
+
+  public ArmPose(String name, boolean isConstant, double armAngle, double wristAngle) {
+    m_armAngleProperty = new GosDoubleProperty(isConstant, name + "Arm Angle", armAngle);
+    m_wristAngleProperty = new GosDoubleProperty(isConstant, name + "Wrist Angle", wristAngle);
+
+    m_armAngle = 0.0;
+    m_wristAngle = 0.0;
+  }
+
+  public ArmPose(double armAngle, double wristAngle) {
+    m_armAngleProperty = null;
+    m_wristAngleProperty = null;
+
+    m_armAngle = armAngle;
+    m_wristAngle = wristAngle;
+  }
+
+  public double armAngle() {
+    if (m_armAngleProperty != null) {
+      return m_armAngleProperty.getValue();
+    }
+    return m_armAngle;
+  }
+
+  public double wristAngle() {
+    if (m_wristAngleProperty != null) {
+      return m_wristAngleProperty.getValue();
+    }
+
+    return m_wristAngle;
+  }
+}

src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -77,16 +78,19 @@ public void periodic() {
             ArmConstants.ARM_UPPER_LIMIT.getValue())
         : m_desiredArmPoseDegs;
 
+    // if we're disabled go back to hold pose
+    if (DriverStation.isDisabled()) {
+      m_desiredState = ArmState.DISABLED;
+    }
+
     // check to make sure we're not in manual control
-    m_io.enableBrakeMode(m_desiredArmPoseDegs <= Double.NEGATIVE_INFINITY
-            || m_desiredWristPoseDegs <= Double.NEGATIVE_INFINITY);
+    m_io.enableBrakeMode(m_desiredState != ArmState.DISABLED);
 
-    if (m_desiredArmPoseDegs > Double.NEGATIVE_INFINITY) {
+    if (m_desiredState != ArmState.DISABLED) {
+      // set the arms angle
       m_io.setArmAngle(m_desiredArmPoseDegs);
       Logger.recordOutput("Arm/Arm Setpoint Degs", m_desiredArmPoseDegs);
-    }
 
-    if (m_desiredWristPoseDegs > Double.NEGATIVE_INFINITY) {
       // check to see if the wrist is currently too close to the rest of the arm
       double wristGap = m_inputs.wristPositionDegs + m_inputs.armPositionDegs;
       if (wristGap < ArmConstants.WRIST_ARM_GAP.getValue()) {
@@ -109,23 +113,23 @@ public void periodic() {
   public void handleState() {
     switch(m_desiredState) {
       case STOW -> {
-        m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armPoseDegs();
-        m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristPoseDegs();
+        m_desiredArmPoseDegs = ArmSetpoints.STOW_SETPOINT.armAngle();
+        m_desiredWristPoseDegs = ArmSetpoints.STOW_SETPOINT.wristAngle();
       }
       case AUTO_AIM -> {
-        ArmSetpoints.ArmSetpoint aimSetpoint = AimbotUtils.aimbotCalculate(
+        ArmPose aimSetpoint = AimbotUtils.aimbotCalculate(
                 new Pose3d(m_poseSupplier.get()), m_inputs.armPositionDegs);
 
-        m_desiredArmPoseDegs = aimSetpoint.armPoseDegs();
-        m_desiredWristPoseDegs = aimSetpoint.wristPoseDegs();
+        m_desiredArmPoseDegs = aimSetpoint.armAngle();
+        m_desiredWristPoseDegs = aimSetpoint.wristAngle();
       }
       case INTAKE -> {
-        m_desiredArmPoseDegs = ArmSetpoints.INTAKE_SETPOINT.armPoseDegs();
-        m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristPoseDegs();
+        m_desiredArmPoseDegs = ArmSetpoints.INTAKE_SETPOINT.armAngle();
+        m_desiredWristPoseDegs = ArmSetpoints.INTAKE_SETPOINT.wristAngle();
       }
       case AMP -> {
-        m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armPoseDegs();
-        m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristPoseDegs();
+        m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle();
+        m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle();
       }
       default -> {
         m_desiredArmPoseDegs = m_inputs.armPositionDegs;
@@ -134,34 +138,11 @@ public void handleState() {
     }
   }
 
-
-
   /* Command Factories */
 
   public Command setDesiredState(ArmState state) {
-    return runOnce(() -> m_desiredState = state);
-  }
-
-  public Command setArmDesiredPose(double armPose, double wristPose) {
-    return run(() -> {
-      m_desiredArmPoseDegs = armPose;
-
-      double estWristGap = armPose + wristPose;
-      if (estWristGap < ArmConstants.WRIST_ARM_GAP.getValue()) {
-        double underGap = ArmConstants.WRIST_ARM_GAP.getValue() - estWristGap;
-        m_desiredWristPoseDegs = wristPose + underGap;
-      } else {
-        m_desiredWristPoseDegs = wristPose;
-      }
-    });
-  }
-
-  public Command setArmPositionFactory(double degrees) {
-    return run(() -> m_desiredArmPoseDegs = degrees);
-  }
-
-  public Command setWristPositionFactory(double degrees) {
-    return run(() -> m_desiredWristPoseDegs = degrees);
+    return runEnd(() -> m_desiredState = state,
+        () -> m_desiredState = ArmState.STOW);
   }
 
   public Command enableBrakeMode(boolean enabled) {

src/main/java/lib/utils/AimbotUtils.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.ArmSetpoints;
+import frc.robot.subsystems.arm.ArmPose;
 import org.littletonrobotics.junction.Logger;
 
 public class AimbotUtils {
@@ -30,7 +31,7 @@ public static Transform3d getShooterTransformation(double armAngle) {
     );
   }
 
-  public static ArmSetpoints.ArmSetpoint aimbotCalculate(Pose3d robotPose, double armAngle) {
+  public static ArmPose aimbotCalculate(Pose3d robotPose, double armAngle) {
     // the position to target
     Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d());
     Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d();
@@ -50,6 +51,6 @@ public static ArmSetpoints.ArmSetpoint aimbotCalculate(Pose3d robotPose, double
     double desiredWristAngle = Units.radiansToDegrees(Math.atan(robotToSpeaker.getZ()/groundDistance));
     Logger.recordOutput("Arm/ Wrist Aimbot Raw", desiredWristAngle);
     double safeArmAngle = ArmConstants.WRIST_ARM_GAP.getValue() - desiredWristAngle;
-    return new ArmSetpoints.ArmSetpoint(safeArmAngle, desiredWristAngle);
+    return new ArmPose(safeArmAngle, desiredWristAngle);
   }
 }