added clamp to arm

team581 · Oct 2, 2024 · 4ef471d · 4ef471d
1 parent ae8728e
commit 4ef471d
Showing 1 changed file with 20 additions and 16 deletions.
diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java
@@ -4,6 +4,7 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import dev.doglog.DogLog;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -118,49 +119,49 @@ 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 +194,7 @@ public void robotPeriodic() {
       setStateFromRequest(ArmState.IDLE);
     }
   }
+  private static double clamp(double armAngle){
+    return MathUtil.clamp(armAngle,RobotConfig.get().arm().minAngle(),RobotConfig.get().arm().maxAngle());
+  }
 }