woops

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index df6bb830914179c049b781b5fd45bd852253f882..f33331bda2e8beba6a2b97a15c567c56d70dafd8 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -37,6 +37,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer()));
         SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer()));
         SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer()));
+
+        resetPID.setTolerance(0.05);
     }
 
     public enum SpindexerState {
@@ -53,7 +55,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         Logger.processInputs("Spindexer", inputs);
 
         if (resetPos == null) {
-            resetPos = (motor.getPosition().getValueAsDouble() % gearRatio) % 1.0;
+            motor.setPosition(0.5 * gearRatio);
+            resetPos = (motor.getPosition().getValueAsDouble() / gearRatio) % 1.0;
             resetPID.reset();
         }
 
@@ -67,7 +70,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
             motor.set(0.0);
             reversing = false;
         } else if (state == SpindexerState.RESET && resetPos != null) {
-            motor.set(resetPID.calculate((motor.getPosition().getValueAsDouble() % gearRatio) % 1.0, resetPos));
+            motor.set(resetPID.calculate((motor.getPosition().getValueAsDouble() / gearRatio) % 1.0, resetPos));
         } else {
             motor.set(power);
             reversing = false;
@@ -135,7 +138,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     }
 
     private Double resetPos;
-    private PIDController resetPID = new PIDController(0.5, 0.1, 0);
+    private PIDController resetPID = new PIDController(1.0, 0.0, 0);
 
     private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins