reseting encoder

DeepBlueRobotics · Aug 19, 2024 · e56a17d · e56a17d
1 parent a7fd7b9
commit e56a17d
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -120,7 +120,8 @@ public static final class Armc {
 		public static final boolean ENCODER_INVERTED = true;
 
 		// difference between CoG and arm is .328 rad
-		public static final double ENCODER_OFFSET_RAD = -0.08 + .328; // - 0.6095;
+		public static final double ENCODER_OFFSET_RAD = -0.145;
+		// 0.08 + .328 - 0.404; // - 0.6095;
 
 		// 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
@@ -183,6 +183,7 @@ public void setBooleanDrive(boolean climb) {
     public void periodic() {
 
         SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing
+        SmartDashboard.putNumber("no math arm angle", getNoMathPos());
 
         babyMode = SmartDashboard.getBoolean("babymode", false);
 
@@ -396,12 +397,17 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) {
     // #region Getters
 
     public double getArmPos() {
+
         return MathUtil.inputModulus(armMasterEncoder.getPosition(),
                 ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI);// armMasterEncoder.getPosition();//MathUtil.inputModulus(armMasterEncoder.getPosition(),
                                                                 // ARM_DISCONT_RAD,
         // ARM_DISCONT_RAD + 2 * Math.PI);
     }
 
+    public double getNoMathPos() {
+            return (armMasterEncoder.getPosition());
+    }
+
     public double getArmVel() {
         return armMasterEncoder.getVelocity();
     }