From 0c47c9d7165798ef47e3f61b4a0bb3c493caffee Mon Sep 17 00:00:00 2001
From: Stu-PID <java4first@thelewins.net>
Date: Fri, 29 Mar 2024 19:03:18 -0400
Subject: [PATCH] Fix Logic for Backlash Removal

---
 src/main/java/frc/robot/subsystems/Mast.java | 32 +++++++++++++++-----
 1 file changed, 24 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Mast.java b/src/main/java/frc/robot/subsystems/Mast.java
index b0e2f79..12b730b 100644
--- 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);