From 519029c72360ccafd3e4ee46f25fcf05240bb073 Mon Sep 17 00:00:00 2001
From: Sofie Budman <105175854+sofiebudman@users.noreply.github.com>
Date: Tue, 5 Mar 2024 16:56:53 -0800
Subject: [PATCH] fixed feedforward stuff

---
 src/main/java/org/carlmontrobotics/subsystems/Arm.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
index ca5de1de..568e2a29 100644
--- 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);