Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/Robot…

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 6, 2024 · d16d9af · d16d9af
2 parents e33037d + 7a22712
commit d16d9af
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -172,9 +172,10 @@ public void autoCancelArmCommand() {
     private void driveArm() {
         setpoint = armProfile.calculate(kDt, setpoint, goalState);
         double armFeedVolts = armFeed.calculate(getArmPos(), setpoint.velocity);
-        if ((getArmPos() < LOWER_ANGLE_LIMIT_RAD && getCurrentArmGoal().velocity > 0)
-                || (getArmPos() > UPPER_ANGLE_LIMIT_RAD && getCurrentArmGoal().velocity > 0)) {
-            armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);
+        if ((getArmPos() < LOWER_ANGLE_LIMIT_RAD)
+                || (getArmPos() > UPPER_ANGLE_LIMIT_RAD)) {
+            armFeedVolts = kG * (COM_ARM_LENGTH_METERS) * Math.cos(getArmPos()) + armFeed.calculate(getCurrentArmGoal().position, 0);
+            //kg * cos(arm angle) * arm_COM_length
         }
         armPIDMaster.setReference(setpoint.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts);
     }