From 987a02d35d83bdc7ad7c3fc26ea77499fe04dc63 Mon Sep 17 00:00:00 2001
From: stwiggy <144397102+stwiggy@users.noreply.github.com>
Date: Sat, 2 Mar 2024 22:52:32 -0800
Subject: [PATCH] wrapped arm pids

---
 src/main/java/org/carlmontrobotics/Constants.java    |  1 +
 .../java/org/carlmontrobotics/subsystems/Arm.java    | 12 +++++++++++-
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
index f2e26bdb..54048406 100644
--- 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
index 03f3e140..836a6b85 100644
--- 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);