add gear ratio so it doesn't rotate a bunch when resetting

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index e5d113a94a074db7a3afbc5708fd94cae7b55118..50aa904c89173c63f054a2065de87b556200b19b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -53,7 +53,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         Logger.processInputs("Spindexer", inputs);
 
         if (resetPos == null) {
-            resetPos = motor.getPosition().getValueAsDouble();
+            resetPos = (motor.getPosition().getValueAsDouble() % gearRatio);
             resetPID.reset();
         }
 
@@ -67,7 +67,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(), resetPos));
+            motor.set(resetPID.calculate((motor.getPosition().getValueAsDouble() % gearRatio), resetPos));
         } else {
             motor.set(power);
             reversing = false;
@@ -137,6 +137,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     private Double resetPos;
     private PIDController resetPID = new PIDController(0.5, 0.1, 0);
 
+    private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins
+
     
 
 }