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

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 3, 2024 · f55615e · f55615e
2 parents e00d3c1 + 3ecde74
commit f55615e
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -65,7 +65,7 @@ public class Arm extends SubsystemBase {
     private static double kDt = 0.02;
 
     //PID, feedforward, trap profile
-    private final ArmFeedforward armFeed = new ArmFeedforward(kS, kV, kA);
+    private final ArmFeedforward armFeed = new ArmFeedforward(kS, kG, kV, kA);
     private final SparkPIDController armPID1 = armMotorMaster.getPIDController();
     private final SparkPIDController armPID2 = armMotorFollower.getPIDController();
     private TrapezoidProfile armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
@@ -95,6 +95,8 @@ public Arm() {
         SmartDashboard.putData("Arm", this);
 
         //armProfileTimer.start(); <-- don't neeed timer anymore
+
+        goalState = getCurrentArmState();
 
         setArmTarget(goalState.position);
     }
@@ -234,7 +236,8 @@ public TrapezoidProfile.State getCurrentArmGoal() {
 
 
     public boolean armAtSetpoint() {
-        return armProfile.isFinished(armProfileTimer.get());
+        return Math.abs(getArmPos() - goalState.position) < POS_TOLERANCE_RAD &&
+                Math.abs(getArmVel() - goalState.velocity) < VEL_TOLERANCE_RAD_P_SEC;
     }