From 16da0866b822962606e5fc040e2188870035b801 Mon Sep 17 00:00:00 2001 From: moo Date: Sun, 30 Aug 2026 10:27:15 -0700 Subject: [PATCH] spindexer magic + formatting sry --- .../robot/subsystems/spindexer/Spindexer.java | 346 ++++++++++-------- .../subsystems/spindexer/SpindexerTuning.java | 163 +++++++++ 2 files changed, 365 insertions(+), 144 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 03bb574..762552a 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -2,162 +2,220 @@ package frc.robot.subsystems.spindexer; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; public class Spindexer extends SubsystemBase implements SpindexerIO { - private TalonFX motorOne = new TalonFX(IdConstants.SPINDEXER_ONE_ID, Constants.CANIVORE_SUB); - private TalonFX motorTwo = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB); - - private double power = 0.0; - public int ballCount = 0; - private SpindexerState state = SpindexerState.STOPPED; - private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); - - public boolean noIndexing = false; - - - public Spindexer() { - updateInputs(); - - // configure current limit - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT; - limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.SUPPLY_CURRENT_LIMIT; - limitConfig.SupplyCurrentLimitEnable = true; - motorOne.getConfigurator().apply(limitConfig); - motorTwo.getConfigurator().apply(limitConfig); - motorTwo.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer())); - SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer())); - SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer())); - } - } - - public enum SpindexerState { - MAX, - REVERSE, - STOPPED, - CUSTOM, - } - - private SpindexerState pastState = SpindexerState.STOPPED; - - @Override - public void periodic() { - updateInputs(); - Logger.processInputs("Spindexer", inputs); - - if (state == SpindexerState.MAX) { - setMotorVoltages(SpindexerConstants.spindexerForwardVoltage); - } else if (state == SpindexerState.REVERSE) { - setMotorVoltages(SpindexerConstants.spindexerReverseVoltage); - } else if (state == SpindexerState.STOPPED) { - setMotorVoltages(0.0); - } else { - setMotorVoltages(power); - } - - if (state != pastState) { - if (state == SpindexerState.REVERSE) { - setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_REVERSE_STATOR_LIMIT); - } else { - setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT); - } - pastState = state; - } - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); - SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); - } - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE); - } - - Logger.recordOutput("HasBalls", spinningAir()); - } - - public void setMotorVoltages(double voltage) { - motorOne.setControl(new VoltageOut(voltage * 12).withEnableFOC(true)); - motorTwo.setControl(new VoltageOut(voltage * 12).withEnableFOC(true)); - } - - public void maxSpindexer() { - state = SpindexerState.MAX; - } - - public void reverseSpindexer(){ - state = SpindexerState.REVERSE; - } - - public void stopSpindexer() { - state = SpindexerState.STOPPED; - } - - public void setSpindexer(double power) { - this.power = power; - state = SpindexerState.CUSTOM; - } - - public void setNewCurrentLimit(double stator, double supply) { - CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = stator; - limitConfig.StatorCurrentLimitEnable = true; - limitConfig.SupplyCurrentLimit = supply; - limitConfig.SupplyCurrentLimitEnable = true; - motorOne.getConfigurator().apply(limitConfig); - motorTwo.getConfigurator().apply(limitConfig); - } - - public double getSubsystemStatorCurrent() { - return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent; - } - - public double getMotorOneStatorCurrent() { - return inputs.spindexerOneStatorCurrent; - } - - public double getMotorTwoStatorCurrent() { - return inputs.spindexerTwoStatorCurrent; - } - - public double getSubsystemSupplyCurrent() { - return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent; - } - - public boolean spinningAir() { - return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0; - } - - public double getMotorOneVelocity() { - return inputs.spindexerOneVelocity; - } - - public double getMotorTwoVelocity() { - return inputs.spindexerTwoVelocity; - } + private TalonFX motorOne = new TalonFX(IdConstants.SPINDEXER_ONE_ID, Constants.CANIVORE_SUB); + private TalonFX motorTwo = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB); + + private MotionMagicVelocityTorqueCurrentFOC velocityRequest = new MotionMagicVelocityTorqueCurrentFOC(0); + + private double power = 0.0; + public int ballCount = 0; + private SpindexerState state = SpindexerState.STOPPED; + private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); + + public boolean noIndexing = false; + + public Spindexer() { + updateInputs(); + + // configure current limit + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT; + limitConfig.StatorCurrentLimitEnable = true; + limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.SUPPLY_CURRENT_LIMIT; + limitConfig.SupplyCurrentLimitEnable = true; + + // configure motion magic + TalonFXConfiguration configs = new TalonFXConfiguration(); + // units = amperes + Slot0Configs slot0 = configs.Slot0; + slot0.kP = 2.0; + slot0.kI = 0.0; + slot0.kD = 0.0; + + // start with feedforward at zero for tuning + slot0.kS = 0.0; + slot0.kV = 0.0; + slot0.kA = 0.0; + + motorOne.getConfigurator().apply(configs); + motorTwo.getConfigurator().apply(configs); + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer())); + SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer())); + SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer())); + } + } + + public enum SpindexerState { + MAX, + REVERSE, + STOPPED, + CUSTOM, + } + + private SpindexerState pastState = SpindexerState.STOPPED; + + public void runIndexer(double velocityRPS) { + motorOne.setControl( + velocityRequest.withVelocity(velocityRPS)); + motorTwo.setControl( + velocityRequest.withVelocity(velocityRPS)); + } + + public void stopIndexing() { + motorOne.setControl(velocityRequest.withVelocity(0)); + motorTwo.setControl(velocityRequest.withVelocity(0)); + } + + @AutoLogOutput + public double getVelocityRPS() { + return motorOne.getVelocity().getValueAsDouble(); + } + + @AutoLogOutput + public double getTorqueCurrent() { + return motorOne.getTorqueCurrent().getValueAsDouble(); + } + + private final SpindexerTuning tuner = new SpindexerTuning(motorOne, motorTwo); + + @Override + public void periodic() { + motorTwo.setControl(new Follower(IdConstants.SPINDEXER_ONE_ID, MotorAlignmentValue.Opposed)); + tuner.periodic(); + } + + public void periodicDEATH() { + updateInputs(); + Logger.processInputs("Spindexer", inputs); + + if (state == SpindexerState.MAX) { + setMotorVoltages(SpindexerConstants.spindexerForwardVoltage); + } else if (state == SpindexerState.REVERSE) { + setMotorVoltages(SpindexerConstants.spindexerReverseVoltage); + } else if (state == SpindexerState.STOPPED) { + setMotorVoltages(0.0); + } else { + setMotorVoltages(power); + } + + if (state != pastState) { + if (state == SpindexerState.REVERSE) { + setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_REVERSE_STATOR_LIMIT); + } else { + setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT); + } + pastState = state; + } + + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); + SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); + } + + if (!Constants.DISABLE_SMART_DASHBOARD) { + SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE); + } + + Logger.recordOutput("HasBalls", spinningAir()); + } + + public void setMotorVoltages(double voltage) { + motorOne.setControl(new VoltageOut(voltage * 12).withEnableFOC(true)); + motorTwo.setControl(new VoltageOut(voltage * 12).withEnableFOC(true)); + } + + public void maxSpindexer() { + state = SpindexerState.MAX; + } + + public void reverseSpindexer() { + state = SpindexerState.REVERSE; + } + + public void stopSpindexer() { + state = SpindexerState.STOPPED; + } + + public void setSpindexer(double power) { + this.power = power; + state = SpindexerState.CUSTOM; + } + + public void setNewCurrentLimit(double stator, double supply) { + CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); + limitConfig.StatorCurrentLimit = stator; + limitConfig.StatorCurrentLimitEnable = true; + limitConfig.SupplyCurrentLimit = supply; + limitConfig.SupplyCurrentLimitEnable = true; + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); + } + + public double getSubsystemStatorCurrent() { + return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent; + } + + public double getMotorOneStatorCurrent() { + return inputs.spindexerOneStatorCurrent; + } + + public double getMotorTwoStatorCurrent() { + return inputs.spindexerTwoStatorCurrent; + } + + public double getSubsystemSupplyCurrent() { + return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent; + } + + public boolean spinningAir() { + return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0; + } + + public double getMotorOneVelocity() { + return inputs.spindexerOneVelocity; + } + + public double getMotorTwoVelocity() { + return inputs.spindexerTwoVelocity; + } + + @Override + public void updateInputs() { + inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); + inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble(); + inputs.spindexerOneSupplyCurrent = motorOne.getSupplyCurrent().getValueAsDouble(); + inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble(); + inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); + inputs.spindexerTwoSupplyCurrent = motorTwo.getSupplyCurrent().getValueAsDouble(); + } - @Override - public void updateInputs() { - inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); - inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble(); - inputs.spindexerOneSupplyCurrent = motorOne.getSupplyCurrent().getValueAsDouble(); - inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble(); - inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); - inputs.spindexerTwoSupplyCurrent = motorTwo.getSupplyCurrent().getValueAsDouble(); - } } diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java new file mode 100644 index 0000000..b638575 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java @@ -0,0 +1,163 @@ +package frc.robot.subsystems.spindexer; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SpindexerTuning { + + private final TalonFX motor1; + private final TalonFX motor2; + + private final MotionMagicVelocityTorqueCurrentFOC request = new MotionMagicVelocityTorqueCurrentFOC(0); + + private final NeutralOut neutralOut = new NeutralOut(); + + private double kS = 0.0; + private double kV = 0.0; + private double kA = 0.0; + private double kP = 0.0; + private double kI = 0.0; + private double kD = 0.0; + + private double targetVelocity = 0.0; + private double maxAccel = 0.0; + private double maxJerk = 0.0; + + public SpindexerTuning(TalonFX motor1, TalonFX motor2) { + this.motor1 = motor1; + this.motor2 = motor2; + + SmartDashboard.putNumber("Indexer Tuning/kS", 0.0); + SmartDashboard.putNumber("Indexer Tuning/kV", 0.0); + SmartDashboard.putNumber("Indexer Tuning/kA", 0.0); + SmartDashboard.putNumber("Indexer Tuning/kP", 1.0); + SmartDashboard.putNumber("Indexer Tuning/kI", 0.0); + SmartDashboard.putNumber("Indexer Tuning/kD", 0.0); + SmartDashboard.putNumber("Indexer Tuning/maxAccel", 0.0); + SmartDashboard.putNumber("Indexer Tuning/maxJerk", 0.0); + + SmartDashboard.putNumber( + "Indexer Tuning/Target Velocity RPS", + 0.0); + + SmartDashboard.putBoolean( + "Indexer Tuning/Enabled", + false); + + SmartDashboard.putData("Indexer Tuning/update", new InstantCommand(() -> applyGains())); + } + + public void periodic() { + + boolean enabled = SmartDashboard.getBoolean( + "Indexer Tuning/Enabled", + false); + + if (!enabled) { + motor1.setControl(neutralOut); + return; + } + + kS = SmartDashboard.getNumber( + "Indexer Tuning/kS", + kS); + + kV = SmartDashboard.getNumber( + "Indexer Tuning/kV", + kV); + + kA = SmartDashboard.getNumber( + "Indexer Tuning/kA", + kA); + + kP = SmartDashboard.getNumber( + "Indexer Tuning/kP", + kP); + + kI = SmartDashboard.getNumber( + "Indexer Tuning/kI", + kI); + + kD = SmartDashboard.getNumber( + "Indexer Tuning/kD", + kD); + + maxAccel = SmartDashboard.getNumber( + "Indexer Tuning/maxAccel", + maxAccel); + + maxJerk = SmartDashboard.getNumber( + "Indexer Tuning/maxJerk", + maxJerk); + + // low = 6-7 rps, high: 60-67 rps for tuning: https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/device-specific/talonfx/manual-pid-tuning.html#flywheel-tuning-with-torquecurrentfoc + targetVelocity = SmartDashboard.getNumber( + "Indexer Tuning/Target Velocity RPS", + targetVelocity); + + // applyGains(); + + motor1.setControl( + request.withVelocity(targetVelocity)); + motor2.setControl( + request.withVelocity(targetVelocity)); + + Logger.recordOutput("Indexer Tuning/Target Velocity RPS", targetVelocity); + + Logger.recordOutput( + "Indexer Tuning/Actual Velocity RPS", + motor1.getVelocity().getValueAsDouble()); + + Logger.recordOutput( + "Indexer Tuning/Velocity Error RPS", + motor1.getClosedLoopError().getValueAsDouble()); + + Logger.recordOutput( + "Indexer Tuning/Torque Current Amps", + motor1.getTorqueCurrent().getValueAsDouble()); + + Logger.recordOutput( + "Indexer Tuning/Closed Loop Reference", + motor1.getClosedLoopReference().getValueAsDouble()); + + Logger.recordOutput("Indexer Tuning/Max Accel", maxAccel); + Logger.recordOutput("Indexer Tuning/Max Jerk", maxJerk); + + Logger.recordOutput("Indexer Tuning/Actual Acceleration r/s^2", motor1.getAcceleration().getValueAsDouble()); + + Logger.recordOutput("Indexer Tuning/Acceleration Cap", motor1.isSafetyEnabled()); + } + + private void applyGains() { + + TalonFXConfiguration config = new TalonFXConfiguration(); + // acceleration = rotations/sec^2 + // jerk = rotations/sec^3 + MotionMagicConfigs motionMagic = config.MotionMagic; + + motionMagic.MotionMagicAcceleration = maxAccel; // 0 means no lim + motionMagic.MotionMagicJerk = maxJerk; + + Slot0Configs slot0 = config.Slot0; + + slot0.kS = kS; + slot0.kV = kV; + slot0.kA = kA; + + slot0.kP = kP; + slot0.kI = kI; + slot0.kD = kD; + + motor1.getConfigurator().apply(config); + motor2.getConfigurator().apply(config); + } +} -- 2.47.3