added clamp to arm (#43)

* added clamp to arm

* spotless

Author: ryanknj5

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

@@ -118,49 +118,75 @@ protected void afterTransition(ArmState newState) {
 
       case CLIMBING_1_LINEUP -> {
         leftMotor.setControl(
-            positionRequest.withPosition(ArmAngle.CLIMBING_1_LINEUP.getRotations()));
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP.getDegrees()))));
         rightMotor.setControl(
-            positionRequest.withPosition(ArmAngle.CLIMBING_1_LINEUP.getRotations()));
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP.getDegrees()))));
       }
       case CLIMBING_2_HANGING -> {
         leftMotor.setControl(
-            positionRequest.withPosition(ArmAngle.CLIMBING_2_HANGING.getRotations()));
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING.getDegrees()))));
         rightMotor.setControl(
-            positionRequest.withPosition(ArmAngle.CLIMBING_2_HANGING.getRotations()));
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING.getDegrees()))));
       }
 
       case DROP -> {
-        leftMotor.setControl(positionRequest.withPosition(ArmAngle.DROP.getRotations()));
-        rightMotor.setControl(positionRequest.withPosition(ArmAngle.DROP.getRotations()));
+        leftMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.DROP.getDegrees()))));
+        rightMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.DROP.getDegrees()))));
       }
 
       case PODIUM_SHOT -> {
-        leftMotor.setControl(positionRequest.withPosition(ArmAngle.PODIUM.getRotations()));
-        rightMotor.setControl(positionRequest.withPosition(ArmAngle.PODIUM.getRotations()));
+        leftMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.PODIUM.getDegrees()))));
+        rightMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.PODIUM.getDegrees()))));
       }
       case SUBWOOFER_SHOT -> {
-        leftMotor.setControl(positionRequest.withPosition(ArmAngle.SUBWOOFER.getRotations()));
-        rightMotor.setControl(positionRequest.withPosition(ArmAngle.SUBWOOFER.getRotations()));
+        leftMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER.getDegrees()))));
+        rightMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER.getDegrees()))));
       }
 
       case FEEDING -> {
-        double newAngle = Units.degreesToRotations(feedSpotDistanceToAngle.get(distanceToFeedSpot));
+        double newAngle =
+            Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot)));
         leftMotor.setControl(positionRequest.withPosition(newAngle));
         rightMotor.setControl(positionRequest.withPosition(newAngle));
       }
       case SPEAKER_SHOT -> {
-        var newAngle = Units.degreesToRotations(speakerDistanceToAngle.get(distanceToSpeaker));
+        var newAngle =
+            Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker)));
 
         leftMotor.setControl(positionRequest.withPosition(newAngle));
         rightMotor.setControl(positionRequest.withPosition(newAngle));
       }
       case AMP -> {
-        leftMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations()));
-        rightMotor.setControl(positionRequest.withPosition(ArmAngle.AMP.getRotations()));
+        leftMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.AMP.getDegrees()))));
+        rightMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.AMP.getDegrees()))));
       }
       case PASS -> {
-        leftMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations()));
-        rightMotor.setControl(positionRequest.withPosition(ArmAngle.PASS.getRotations()));
+        leftMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.PASS.getDegrees()))));
+        rightMotor.setControl(
+            positionRequest.withPosition(
+                Units.degreesToRotations(clamp(ArmAngle.PASS.getDegrees()))));
       }
     }
   }
@@ -193,4 +219,9 @@ public void robotPeriodic() {
       setStateFromRequest(ArmState.IDLE);
     }
   }
+
+  private static double clamp(double armAngle) {
+    return MathUtil.clamp(
+        armAngle, RobotConfig.get().arm().minAngle(), RobotConfig.get().arm().maxAngle());
+  }
 }