changed ot use setFF

DeepBlueRobotics · Feb 28, 2024 · 6acb48c · 6acb48c
1 parent 20fab44
commit 6acb48c
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -62,6 +62,7 @@ public Arm() {
       armPID.setP(Constants.Arm.kP);
       armPID.setI(Constants.Arm.kI);
       armPID.setD(Constants.Arm.kD);
+      armPID.setFF(kV,0);
       SmartDashboard.putNumber("kp", kP);
       SmartDashboard.putNumber("kd", kD);
       SmartDashboard.putNumber("ki", kI);
@@ -143,6 +144,7 @@ public void driveArm(TrapezoidProfile.State state) {
   */
       double targetRPS = SmartDashboard.getNumber("Shooter RPS", 0);
       double feed = armFeed.calculate(targetRPS); 
+
       //double armFeedVolts = 0; //<-- similar math to above to get this
       //double armPIDVolts = 
       armPID.setReference(targetRPS * 60, CANSparkBase.ControlType.kVelocity, 0, feed);
@@ -151,7 +153,7 @@ public void driveArm(TrapezoidProfile.State state) {
 
       SmartDashboard.putNumber("ArmFeedVolts", armFeedVolts);
       SmartDashboard.putNumber("ArmPIDVolts", armPIDVolts);
-      double volts = armFeedVolts + armPIDVolts;
+      double volts =feed + armPIDVolts;
       SmartDashboard.putNumber("ArmTotalVolts", volts);
       armMotor1.setVoltage(volts);
     }