reset spindexer stuf, untested

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index fc209815c48c3b1b4581bb994f706b6cc70cd340..e5d113a94a074db7a3afbc5708fd94cae7b55118 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -5,6 +5,8 @@ import com.ctre.phoenix6.hardware.TalonFX;
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,6 +22,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     private SpindexerState state = SpindexerState.STOPPED;
     private boolean reversing = false;
     private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
+
     public Spindexer() {
         updateInputs();
 
@@ -40,6 +43,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         MAX,
         REVERSE,
         STOPPED,
+        RESET,
         CUSTOM,
     }
 
@@ -48,6 +52,11 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         updateInputs();
         Logger.processInputs("Spindexer", inputs);
 
+        if (resetPos == null) {
+            resetPos = motor.getPosition().getValueAsDouble();
+            resetPID.reset();
+        }
+
         if (state == SpindexerState.MAX) {
             motor.set(SpindexerConstants.spindexerMaxPower);
             reversing = false;
@@ -57,6 +66,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         } else if (state == SpindexerState.STOPPED) {
             motor.set(0.0);
             reversing = false;
+        } else if (state == SpindexerState.RESET && resetPos != null) {
+            motor.set(resetPID.calculate(motor.getPosition().getValueAsDouble(), resetPos));
         } else {
             motor.set(power);
             reversing = false;
@@ -95,6 +106,14 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         state = SpindexerState.CUSTOM;
     }
 
+    public void resetSpindexer() {
+        state = SpindexerState.RESET;
+    }
+
+    public void resetResetAngle() {
+        resetPos = null;
+    }
+
     public double getStatorCurrent() {
         return inputs.spindexerCurrent;
     }
@@ -115,4 +134,9 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         Logger.processInputs("Spindexer", inputs);
     }
 
+    private Double resetPos;
+    private PIDController resetPID = new PIDController(0.5, 0.1, 0);
+
+    
+
 }