From dd1ac710b7b11497230712ef66affe98e5259710 Mon Sep 17 00:00:00 2001
From: Brandon <83319404+BrandonS09@users.noreply.github.com>
Date: Mon, 4 Mar 2024 23:17:53 +0000
Subject: [PATCH] rad for lower upper angle

---
 .../java/org/carlmontrobotics/commands/ArmTeleop.java     | 2 +-
 src/main/java/org/carlmontrobotics/subsystems/Arm.java    | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
index cf92232e..bf99af6a 100644
--- a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
+++ b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -50,7 +50,7 @@ public void execute() {
     double deltaT = currTime - lastTime;
 
     double goalArmRad = goalState.position + speeds * deltaT; 
-    goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT, LOWER_ANGLE_LIMIT);
+    goalArmRad = MathUtil.clamp(goalArmRad, UPPER_ANGLE_LIMIT_RAD, LOWER_ANGLE_LIMIT_RAD);
     goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos()+ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getArmPos()-ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
     goalState = new TrapezoidProfile.State(goalArmRad, 0);
     armSubsystem.setArmTarget(goalState.position);
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index b3def1b0..71d69b59 100644
--- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java
+++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -94,8 +94,8 @@ public Arm() {
 
         armPIDMaster.setFeedbackDevice(armMasterEncoder);
         armPIDMaster.setPositionPIDWrappingEnabled(true);
-        armPIDMaster.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT);
-        armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT);
+        armPIDMaster.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT_RAD);
+        armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT_RAD);
         //two PIDs?
         
         SmartDashboard.putData("Arm", this);
@@ -150,7 +150,7 @@ private void driveArm(){
       
       setPoint = armProfile.calculate(kDt, setPoint, goalState);
       double armFeedVolts = armFeed.calculate(goalState.position, goalState.velocity);
-      if ((getArmPos() < LOWER_ANGLE_LIMIT && getCurrentArmGoal().velocity > 0) || (getArmPos() > UPPER_ANGLE_LIMIT && getCurrentArmGoal().velocity > 0)){
+      if ((getArmPos() < LOWER_ANGLE_LIMIT_RAD && getCurrentArmGoal().velocity > 0) || (getArmPos() > UPPER_ANGLE_LIMIT_RAD && getCurrentArmGoal().velocity > 0)){
         armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
       }
       armPIDMaster.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts);
@@ -227,6 +227,6 @@ public boolean armAtSetpoint() {
     }
 
     public double getArmClampedGoal(double goal) {
-        return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI), LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
+        return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI), LOWER_ANGLE_LIMIT_RAD, UPPER_ANGLE_LIMIT_RAD);
     } 
 }
\ No newline at end of file