From fd749df53f9e37e4e0870c4d32f25e36678534d9 Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Thu, 16 Apr 2026 20:36:22 -0700 Subject: [PATCH] ok I added second motor and voltage control --- src/main/java/frc/robot/RobotContainer.java | 2 - .../java/frc/robot/constants/IdConstants.java | 6 +- .../robot/subsystems/Climb/LinearClimb.java | 2 +- .../robot/subsystems/spindexer/Spindexer.java | 68 ++++++------------- .../spindexer/SpindexerConstants.java | 6 +- .../subsystems/spindexer/SpindexerIO.java | 6 +- 6 files changed, 32 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15c3fec..9530c11 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -251,8 +251,6 @@ public class RobotContainer { NamedCommands.registerCommand("Start Spindexer", new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer))); NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel())); - NamedCommands.registerCommand("Reset Spindexer", new InstantCommand(() -> spindexer.resetSpindexer())); - NamedCommands.registerCommand("Reset Reset Angle", new InstantCommand(() -> spindexer.resetResetAngle())); } if (hood != null) { diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index 160c8da..a662da5 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -32,10 +32,8 @@ public class IdConstants { public static final int HOOD_ID = 11; // Spindexer - public static final int SPINDEXER_ID = 4; - - // Climb - public static final int CLIMB_MOTOR_ID = 8; + public static final int SPINDEXER_ONE_ID = 4; + public static final int SPINDEXER_TWO_ID = 8; // Intake public static final int RIGHT_MOTOR_ID = 1; diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java index 80389af..5eccdc1 100644 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java @@ -45,7 +45,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO { ClimbConstants.PID_D); public LinearClimb() { - motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB); + motor = new TalonFX(IdConstants.SPINDEXER_TWO_ID, Constants.CANIVORE_SUB); pid.setTolerance(ClimbConstants.PID_TOLERANCE); motor.setNeutralMode(NeutralModeValue.Brake); diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index e9cb913..d9822c5 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -2,6 +2,7 @@ package frc.robot.subsystems.spindexer; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import org.littletonrobotics.junction.Logger; @@ -14,7 +15,8 @@ import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; public class Spindexer extends SubsystemBase implements SpindexerIO { - private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.CANIVORE_SUB); + 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; @@ -35,22 +37,20 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { limitConfig.StatorCurrentLimitEnable = true; limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit; limitConfig.SupplyCurrentLowerTime = 1.5; - motor.getConfigurator().apply(limitConfig); + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); 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())); } - - resetPID.setTolerance(0.05); } public enum SpindexerState { MAX, REVERSE, STOPPED, - RESET, CUSTOM, } @@ -59,49 +59,36 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { updateInputs(); Logger.processInputs("Spindexer", inputs); - if (resetPos == null) { - motor.setPosition(0.1 * gearRatio); - resetPos = (motor.getPosition().getValueAsDouble() / gearRatio) % 1.0; - resetPID.reset(); - } - if (state == SpindexerState.MAX) { - motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true)); + setMotorVoltages(SpindexerConstants.spindexerForwardVoltage); reversing = false; } else if (state == SpindexerState.REVERSE) { - motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true)); + setMotorVoltages(SpindexerConstants.spindexerReverseVoltage); reversing = true; } else if (state == SpindexerState.STOPPED) { - motor.setControl(new DutyCycleOut(0.0).withEnableFOC(true)); + setMotorVoltages(0.0); reversing = false; - } else if (state == SpindexerState.RESET && resetPos != null) { - motor.setControl(new DutyCycleOut(resetPID.calculate((motor.getPosition().getValueAsDouble() / gearRatio) % 1.0, resetPos)).withEnableFOC(true)); } else { - motor.setControl(new DutyCycleOut(power).withEnableFOC(true)); + setMotorVoltages(power); reversing = false; } - // scale threshold based on power - double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power; if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Spindexer Ball Count", ballCount); - SmartDashboard.putBoolean("Spindexer Running", state == SpindexerState.MAX || state == SpindexerState.CUSTOM); SmartDashboard.putBoolean("Spindexer Has Ball", ballCount > 0); } - boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold; - if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) { - ballCount++; - } - wasSpindexerSlow = isSpindexerSlow; - if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE); } } + public void setMotorVoltages(double voltage) { + motorOne.setControl(new VoltageOut(voltage).withEnableFOC(true)); + motorTwo.setControl(new VoltageOut(voltage).withEnableFOC(true)); + } + public void maxSpindexer() { state = SpindexerState.MAX; } @@ -119,16 +106,8 @@ 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; + return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent; } public void setNewCurrentLimit(double newCurrentLimit) { @@ -137,20 +116,15 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { limitConfig.StatorCurrentLimitEnable = true; limitConfig.SupplyCurrentLowerLimit = newCurrentLimit; limitConfig.SupplyCurrentLowerTime = 1.5; - motor.getConfigurator().apply(limitConfig); + motorOne.getConfigurator().apply(limitConfig); + motorTwo.getConfigurator().apply(limitConfig); } @Override public void updateInputs() { - inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio; - inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble(); + inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); + inputs.spindexerOneCurrent = motorOne.getStatorCurrent().getValueAsDouble(); + inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble(); + inputs.spindexerTwoCurrent = motorTwo.getStatorCurrent().getValueAsDouble(); } - - private Double resetPos; - private PIDController resetPID = new PIDController(4.0, 0.0, 0); - - private final double gearRatio = 27.0 / 1.0; //spindexer spins once for every 27 motor spins - - - } diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java index a644b98..1855440 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@ -3,8 +3,10 @@ package frc.robot.subsystems.spindexer; public class SpindexerConstants { public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls) public static final double currentLimit = 40; // A - public static final double spindexerMaxPower = 1.00; - public static final double spindexerReversePower = -1.00; + public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing) + public static final double spindexerReverseVoltage = -1.00; // Volts + public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing + public static final double CURRENT_SPIKE_LIMIT = 150; public static final double CURRENT_TIME_LIMIT = 1.0; //s public static final double JAM_CURRENT_THRESHOLD = 75.0; // A diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java index 1cf6740..e1a154f 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java @@ -5,8 +5,10 @@ import org.littletonrobotics.junction.AutoLog; public interface SpindexerIO { @AutoLog public static class SpindexerIOInputs { - public double spindexerVelocity = 0.0; - public double spindexerCurrent = 0.0; + public double spindexerOneVelocity = 0.0; + public double spindexerOneCurrent = 0.0; + public double spindexerTwoVelocity = 0.0; + public double spindexerTwoCurrent = 0.0; } public void updateInputs(); -- 2.39.5