fixed feedforward stuff

DeepBlueRobotics · Mar 6, 2024 · 519029c · 519029c
1 parent ec3ea78
commit 519029c
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -174,7 +174,7 @@ private void driveArm() {
         double armFeedVolts = armFeed.calculate(getArmPos(), setpoint.velocity);
         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);
+            armFeedVolts = armFeed.calculate(getArmPos(), 0);
             //kg * cos(arm angle) * arm_COM_length
         }
         armPIDMaster.setReference(setpoint.position, CANSparkBase.ControlType.kPosition, 0, armFeedVolts);