Merge branch 'main' into taran-cleanout

diff --cc src/main/java/frc/robot/RobotContainer.java
Simple merge

diff --cc src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index ae572adf40c8dc298a9d4d19ab67cdde60f0a4a5,9b2c40e81555f7bfcc165df9c0d8d9a9510a8a99..4567eaed9e3d6ffe7b83cee86ed2c164ded5ee15
--- 1/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
--- 2/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@@ -12,9 -20,7 +12,8 @@@ public class Spindexer extends Subsyste
  
      private double power = 0.0;
      public int ballCount = 0;
 +    private boolean wasSpindexerSlow = false;
      private SpindexerState state = SpindexerState.STOPPED;
-     private boolean reversing = false;
      private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
  
      public boolean noIndexing = false;
@@@ -39,29 -55,17 +38,25 @@@
  
      @Override
      public void periodic() {
 -        updateInputs();
 +        io.updateInputs(inputs);
          Logger.processInputs("Spindexer", inputs);
  
-             resetPos = (inputs.spindexerPosition / gearRatio) % 1.0;
 +        if (resetPos == null) {
 +            io.setPositionRaw(0.1 * gearRatio);
++            resetPos = (inputs.spindexerOneVelocity / gearRatio) % 1.0;
 +            resetPID.reset();
 +        }
 +
          if (state == SpindexerState.MAX) {
-             io.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true));
-             reversing = false;
 -            setMotorVoltages(SpindexerConstants.spindexerForwardVoltage);
++            io.setControl(new DutyCycleOut(SpindexerConstants.spindexerForwardVoltage / 12.0).withEnableFOC(true));
          } else if (state == SpindexerState.REVERSE) {
-             io.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true));
-             reversing = true;
 -            setMotorVoltages(SpindexerConstants.spindexerReverseVoltage);
++            io.setControl(new DutyCycleOut(SpindexerConstants.spindexerReverseVoltage / 12.0).withEnableFOC(true));
          } else if (state == SpindexerState.STOPPED) {
 -            setMotorVoltages(0.0);
 +            io.setControl(new DutyCycleOut(0.0).withEnableFOC(true));
-             reversing = false;
 +        } else if (state == SpindexerState.RESET && resetPos != null) {
-             io.setControl(new DutyCycleOut(resetPID.calculate((inputs.spindexerPosition / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
++            io.setControl(new DutyCycleOut(resetPID.calculate((inputs.spindexerOneVelocity / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
          } else {
 -            setMotorVoltages(power);
 +            io.setControl(new DutyCycleOut(power).withEnableFOC(true));
-             reversing = false;
          }
  
  
@@@ -74,12 -74,6 +69,12 @@@
              SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0);
          }
  
-         boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;
++        boolean isSpindexerSlow = (inputs.spindexerOneVelocity + inputs.spindexerTwoVelocity) / 2.0 < velocityThreshold;
 +        if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) {
 +            ballCount++;
 +        }
 +        wasSpindexerSlow = isSpindexerSlow;
 +
          if (!Constants.DISABLE_SMART_DASHBOARD) {
              SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE);
          }
@@@ -102,16 -101,8 +97,16 @@@
          state = SpindexerState.CUSTOM;
      }
  
 +    public void resetSpindexer() {
 +        state = SpindexerState.RESET;
 +    }
 +
 +    public void resetResetAngle() {
 +        resetPos = null;
 +    }
 +
      public double getStatorCurrent() {
-         return inputs.spindexerCurrent;
+         return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent;
      }
  
      public void setNewCurrentLimit(double newCurrentLimit) {

diff --cc src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
index 321cb1d84d99c483d2e0445f81007187da9d6e01,e1a154f6fe6f38d2e47a0e744b20393a58335842..0c8ab05ab0edf30356d082662e6b6b1a6dd219cf
--- 1/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
--- 2/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java
@@@ -2,21 -2,14 +2,22 @@@ package frc.robot.subsystems.spindexer
  
  import org.littletonrobotics.junction.AutoLog;
  
 +import com.ctre.phoenix6.controls.ControlRequest;
 +
  public interface SpindexerIO {
-   @AutoLog
-   public static class SpindexerIOInputs {
-     public double spindexerVelocity = 0.0;
-     public double spindexerCurrent = 0.0;
-     public double spindexerPosition = 0.0;
-   }
+     @AutoLog
+     public static class SpindexerIOInputs {
+         public double spindexerOneVelocity = 0.0;
+         public double spindexerOneCurrent = 0.0;
+         public double spindexerTwoVelocity = 0.0;
+         public double spindexerTwoCurrent = 0.0;
+     }
  
 -    public void updateInputs();
 +  public void updateInputs(SpindexerIOInputs inputs);
 +
 +  public void setControl(ControlRequest request);
 +
 +  public void setPositionRaw(double pos);
 +
 +  public void setNewCurrentLimit(double newCurrentLimit);
  }