diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java index 22b1341..bf46cbe 100644 --- a/src/main/java/frc/robot/constants/SpindexerConstants.java +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -4,29 +4,44 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; public class SpindexerConstants { // TODO Real values - public static final int kMotorCANID = 0; + public static final int kSpindexerMotorCANID = 0; + public static final int kFeederMotorCANID = 0; - public static final int kStatorCurrentLimit = 80; - public static final int kSupplyCurrentLimit = 30; + public static final int kSpindexerStatorCurrentLimit = 80; + public static final int kSpindexerSupplyCurrentLimit = 30; + public static final int kFeederCurrentLimit = 30; - public static final InvertedValue kInversionState = InvertedValue.Clockwise_Positive; - public static final NeutralModeValue kIdleMode = NeutralModeValue.Brake; + public static final boolean kFeederMotorInverted = false; + + public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake; + + public static final IdleMode kFeederIdleMode = IdleMode.kBrake; // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM - public static final CurrentLimitsConfigs kCurrentLimitConfig = new CurrentLimitsConfigs(); - public static final MotorOutputConfigs kMotorConfig = new MotorOutputConfigs(); + public static final SparkMaxConfig kFeederConfig = new SparkMaxConfig(); + + public static final CurrentLimitsConfigs kSpindexerCurrentLimitConfig = new CurrentLimitsConfigs(); + public static final MotorOutputConfigs kSpindexerMotorConfig = new MotorOutputConfigs(); static { - kCurrentLimitConfig.StatorCurrentLimitEnable = true; - kCurrentLimitConfig.SupplyCurrentLimitEnable = true; - kCurrentLimitConfig.StatorCurrentLimit = kStatorCurrentLimit; - kCurrentLimitConfig.SupplyCurrentLimit = kSupplyCurrentLimit; + kSpindexerCurrentLimitConfig.StatorCurrentLimitEnable = true; + kSpindexerCurrentLimitConfig.SupplyCurrentLimitEnable = true; + kSpindexerCurrentLimitConfig.StatorCurrentLimit = kSpindexerStatorCurrentLimit; + kSpindexerCurrentLimitConfig.SupplyCurrentLimit = kSpindexerSupplyCurrentLimit; - kMotorConfig.Inverted = kInversionState; - kMotorConfig.NeutralMode = kIdleMode; + kSpindexerMotorConfig.Inverted = kSpindexerInversionState; + kSpindexerMotorConfig.NeutralMode = kSpindexerIdleMode; + + kFeederConfig + .inverted(kFeederMotorInverted) + .smartCurrentLimit(kFeederCurrentLimit) + .idleMode(kFeederIdleMode); } } diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index 71c9af3..34180bc 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -2,40 +2,57 @@ package frc.robot.subsystems; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.SpindexerConstants; public class Spindexer extends SubsystemBase { - private TalonFX motor; + private TalonFX spindexerMotor; - private DutyCycleOut motorOutput; + private SparkMax feederMotor; + + private DutyCycleOut spindexerMotorOutput; public Spindexer() { - motor = new TalonFX(SpindexerConstants.kMotorCANID); + spindexerMotor = new TalonFX(SpindexerConstants.kSpindexerMotorCANID); - motor.getConfigurator().apply(SpindexerConstants.kCurrentLimitConfig); - motor.getConfigurator().apply(SpindexerConstants.kMotorConfig); + feederMotor = new SparkMax(SpindexerConstants.kFeederMotorCANID, MotorType.kBrushless); - motorOutput = new DutyCycleOut(0); + spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerCurrentLimitConfig); + spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerMotorConfig); + + feederMotor.configure( + SpindexerConstants.kFeederConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + spindexerMotorOutput = new DutyCycleOut(0); } public Command spinToShooter() { return run(() -> { - motor.setControl(motorOutput.withOutput(1)); + spindexerMotor.setControl(spindexerMotorOutput.withOutput(1)); + feederMotor.set(1); }); } public Command spinToIntake() { return run(() -> { - motor.setControl(motorOutput.withOutput(-1)); + spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1)); + feederMotor.set(-1); }); } public Command stop() { return run(() -> { - motor.setControl(motorOutput.withOutput(0)); + spindexerMotor.setControl(spindexerMotorOutput.withOutput(0)); + feederMotor.set(0); }); }