Update Spindexer.java

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 17d72a7000869a382ab5cfbe15564d61e9b9bca1..05bf846bd1713cf34d50183ecbea9235ac6c0b8d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -1,54 +1,77 @@
 package frc.robot.subsystems.spindexer;
 
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
+import frc.robot.subsystems.spindexer.SpindexerIO;
 
 public class Spindexer extends SubsystemBase implements SpindexerIO {
-    TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID);
+    private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.CANIVORE_SUB);
 
     private double power = 0.0;
-    private int ballCount = 0;
+    public int ballCount = 0;
+    private boolean wasSpindexerSlow = false;
     private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
-    private boolean wasAboveThreshold = false;
 
     public Spindexer() {
-        // SmartDashboard.putData("Turn on Spindexer", new InstantCommand(()->
-        // turnOnSpindexer()));
+        updateInputs();
+
+        // configure current limit
+        CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+        limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT;
+        limitConfig.StatorCurrentLimitEnable = true;
+        limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit;
+        limitConfig.SupplyCurrentLowerTime = 1.5;
+        motor.getConfigurator().apply(limitConfig);
+
+        SmartDashboard.putData("Max speed spindexer", new InstantCommand(() -> maxSpindexer()));
+        SmartDashboard.putData("Turn off spindexer", new InstantCommand(() -> stopSpindexer()));
+        SmartDashboard.putData("Spindexer 50%", new InstantCommand(() -> setSpindexer(0.5)));
     }
 
     @Override
     public void periodic() {
-        power = SmartDashboard.getNumber("Spindexer Power", power);
-        SmartDashboard.putNumber("Spindexer Power", power);
+        updateInputs();
+        Logger.processInputs("Spindexer", inputs);
 
         motor.set(power);
-        updateInputs();
+        
+        // scale threshold based on power
+        double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
+        SmartDashboard.putNumber("Spindexer Velocity Threshold", velocityThreshold);
+        SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
 
-        boolean isAboveThreshold = inputs.spindexerVelocity >= SpindexerConstants.spindexerVelocityWithBall;
-        if (wasAboveThreshold && !isAboveThreshold && power > 0.1) {
+        boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;
+        if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) {
             ballCount++;
         }
-        wasAboveThreshold = isAboveThreshold;
+        wasSpindexerSlow = isSpindexerSlow;
     }
 
-    /**
-     * @return
-     */
     public void maxSpindexer() {
-        power = 0.5;
+        power = SpindexerConstants.spindexerMaxPower;
     }
 
     public void stopSpindexer() {
         power = 0.0;
     }
 
+    public void setSpindexer(double power) {
+        this.power = power;
+    }
+
     @Override
     public void updateInputs() {
-        inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble();
+        inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;
         inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
+        Logger.processInputs("Spindexer", inputs);
     }
 
 }