Truncate to Avoid Tweaking the PID

Team-501-The-PowerKnights · Mar 29, 2024 · 4dff328 · 4dff328
1 parent 15738d8
commit 4dff328
Showing 1 changed file with 9 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Mast.java b/src/main/java/frc/robot/subsystems/Mast.java
@@ -13,6 +13,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import java.math.BigDecimal;
+import java.math.RoundingMode;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -207,9 +209,13 @@ public void periodic() {
 
     // Remove backlash from Launcher by syncing constantly
     if (counts == 30) {
-      double ABStoRel = getAbsoluteEncoderDegrees() * gearRatio / 360;
-      relmastLeftEncoder.setPosition(ABStoRel);
-      // relmastRightEncoder.setPosition(-ABStoRel);
+      // double AbsToRel = getAbsoluteEncoderDegrees() * gearRatio / 360;
+      double AbsToRel =
+          new BigDecimal(getAbsoluteEncoderDegrees() * gearRatio / 360)
+              .setScale(3, RoundingMode.DOWN)
+              .doubleValue();
+      relmastLeftEncoder.setPosition(AbsToRel);
+      // relmastRightEncoder.setPosition(-AbsToRel);
       counts = 0;
     }
     counts++;