From 4ef471d256cd8393026cbafe23e89ca66b9a426b Mon Sep 17 00:00:00 2001
From: ryanknj5 <140574269+ryanknj5@users.noreply.github.com>
Date: Wed, 2 Oct 2024 11:55:09 -0700
Subject: [PATCH] added clamp to arm

---
 src/main/java/frc/robot/arm/ArmSubsystem.java | 36 ++++++++++---------
 1 file changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java
index f4767c9..b825669 100644
--- 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());
+  }
 }