Fix Logic for Backlash Removal

Team-501-The-PowerKnights · Mar 29, 2024 · 0c47c9d · 0c47c9d
1 parent d108b58
commit 0c47c9d
Showing 1 changed file with 24 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Mast.java b/src/main/java/frc/robot/subsystems/Mast.java
@@ -84,7 +84,7 @@ public double getAngle() {
   CommandXboxController operPad;
 
   LoggedDashboardNumber testingAngleNumber;
-  private static double counts = 0; // used for encoder sync
+  private int counts = 0; // used for encoder sync
 
   private static Task currentTask;
   private static final double gearRatio = (48.0 / 32.0) * 125.0;
@@ -188,14 +188,30 @@ public void periodic() {
     double error = relEncoderDeg - absEncoderDeg;
 
     // Remove backlash from Launcher by syncing constantly
-    if (++counts == 30 && Math.abs(error) > .4) {
-      // double AbsToRel = getAbsoluteEncoderDegrees() * gearRatio / 360;
-      double AbsToRel =
-          new BigDecimal(absEncoderDeg * gearRatio / 360)
-              .setScale(3, RoundingMode.DOWN)
-              .doubleValue();
-      relMastLeftEncoder.setPosition(AbsToRel);
+    // if (++counts == 30 && Math.abs(error) > .4) {
+    // // double AbsToRel = getAbsoluteEncoderDegrees() * gearRatio / 360;
+    // double AbsToRel = new BigDecimal(absEncoderDeg * gearRatio / 360)
+    // .setScale(3, RoundingMode.DOWN)
+    // .doubleValue();
+    // relMastLeftEncoder.setPosition(AbsToRel);
+    // counts = 0;
+    // }
+
+    if (++counts >= 30) {
+      if (Math.abs(error) > .4) {
+        // System.out.println("*** fixing at " + counts);
+        // double AbsToRel = getAbsoluteEncoderDegrees() * gearRatio / 360;
+        double AbsToRel =
+            new BigDecimal(absEncoderDeg * gearRatio / 360)
+                .setScale(3, RoundingMode.DOWN)
+                .doubleValue();
+        relMastLeftEncoder.setPosition(AbsToRel);
+      } else {
+        // System.out.println("*** not fixing at " + counts);
+      }
       counts = 0;
+    } else {
+      // System.out.println("skipping at " + counts);
     }
 
     // SmartDashboard.putNumber("Mast/Left_Enc", leftEncAngle);