absolute encoder wrapping, im not sure it this works tho

DeepBlueRobotics · Mar 3, 2024 · d14677e · d14677e
1 parent 2ac4b07
commit d14677e
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 7 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -76,8 +76,8 @@ public static final class Arm {
 		public static final double MARGIN_OF_ERROR = Math.PI/18;
 		//Boundaries
 
-		public static final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70); 
-		public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0);
+		public static final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70) - MARGIN_OF_ERROR; 
+		public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0) + MARGIN_OF_ERROR;
 		public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
 		public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; //placeholder
 		public static final double POS_TOLERANCE_RAD = 0; //placeholder

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -78,9 +78,10 @@ public Arm() {
         armMotorMaster.setIdleMode(IdleMode.kBrake);
         armMotorFollower.setInverted(MOTOR_INVERTED_FOLLOWER);
         armMotorFollower.setIdleMode(IdleMode.kBrake);
-
+        // Comment out when running sysid
         armMasterEncoder.setPositionConversionFactor(ROTATION_TO_RAD);
         armMasterEncoder.setVelocityConversionFactor(ROTATION_TO_RAD);
+        // ------------------------------------------------------------
         armMasterEncoder.setInverted(ENCODER_INVERTED);
 
         armMotorFollower.follow(armMotorMaster);
@@ -97,10 +98,10 @@ public Arm() {
 
         //armPID.setTolerance(posToleranceRad, velToleranceRadPSec);
 
-        armPIDMaster.setFeedbackDevice(armMotorMaster.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle));
+        armPIDMaster.setFeedbackDevice(armMasterEncoder);
         armPIDMaster.setPositionPIDWrappingEnabled(true);
-        armPIDMaster.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT);
-        armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT);
+        armPIDMaster.setPositionPIDWrappingMinInput(MathUtil.angleModulus(LOWER_ANGLE_LIMIT));
+        armPIDMaster.setPositionPIDWrappingMaxInput(MathUtil.angleModulus(LOWER_ANGLE_LIMIT));
         //two PIDs?
         armPIDFollower.setFeedbackDevice(armMotorFollower.getEncoder());
         armPIDFollower.setPositionPIDWrappingEnabled(true);
@@ -109,7 +110,7 @@ public Arm() {
 
         SmartDashboard.putData("Arm", this);
 
-
+        setPoint = getCurrentArmState();
 
         goalState = getCurrentArmState();