wrapped arm pids

DeepBlueRobotics · Mar 3, 2024 · 987a02d · 987a02d
1 parent 9dc6e76
commit 987a02d
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -44,6 +44,7 @@ public static final class Arm {
 		public static final double POS_TOLERANCE_RAD = 0;
 		public static final double VEL_TOLERANCE_RAD_P_SEC = 0;
 		public static final int MAX_VOLTAGE = 12;
+		public static final double ENCODER_OFFSET_RAD = 0;
 
 		//TODO: finish understand why this is broken public static final Measure<Angle> INTAKE_ANGLE = Degrees.to(-1);
 

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -83,10 +83,20 @@ public Arm() {
         armPID2.setI(kI);
         armPID2.setD(kD);
 
-        //armEncoder1.setZeroOffset(offsetRad);
+        armEncoder.setZeroOffset(ENCODER_OFFSET_RAD);
 
         //armPID.setTolerance(posToleranceRad, velToleranceRadPSec);
 
+        armPID1.setFeedbackDevice(armMotorMaster.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle));
+        armPID1.setPositionPIDWrappingEnabled(true);
+        armPID1.setPositionPIDWrappingMinInput(ARM_LOWER_LIMIT_RAD);
+        armPID1.setPositionPIDWrappingMaxInput(ARM_UPPER_LIMIT_RAD);
+        //two PIDs?
+        armPID2.setFeedbackDevice(armMotorFollower.getEncoder());
+        armPID2.setPositionPIDWrappingEnabled(true);
+        armPID2.setPositionPIDWrappingMinInput(ARM_LOWER_LIMIT_RAD);
+        armPID2.setPositionPIDWrappingMaxInput(ARM_UPPER_LIMIT_RAD);
+
         SmartDashboard.putData("Arm", this);