import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakeIOTalonFX;
import frc.robot.subsystems.LED.LED;
+ import frc.robot.subsystems.PowerControl.EMABreaker;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
default:
case PrimeJr: // AKA Valence
- spindexer = new Spindexer();
- intake = new Intake();
+ spindexer = new Spindexer(new SpindexerIOTalonFX());
+ intake = new Intake(new IntakeIOTalonFX());
led = new LED();
+ breaker = new EMABreaker();
case WaffleHouse: // AKA Betabot
- turret = new Turret();
- shooter = new Shooter();
- hood = new Hood();
+ turret = new Turret(new TurretIOTalonFX());
+ shooter = new Shooter(new ShooterIOTalonFX());
+ hood = new Hood(new HoodIOTalonFX());
case TwinBot:
* Driver controls for the PS5 controller
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
- private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
+ private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
- public PS5ControllerDriverConfig(
+ public PS5ControllerDriverConfig(
Drivetrain drive,
Shooter shooter,
Turret turret,
this.spindexer = spindexer;
}
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null) {
- controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
- }
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Reverse motors
+ if (intake != null && spindexer != null) {
+ controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
+ }
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
- }
- }, intake));
-
- // Retract if hold for 2 seconds
- controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- intake.spinStop();
- }, intake));
-
- // Make the intake go in and out while shooting
- controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
- .alongWith(new InstantCommand(()-> intakeBoolean = true)));
-
- // Calibration: you can now calibrate easily using this button
- if (hood != null && intake != null) {
- controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- hood.calibrate();
- }, intake, hood)).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- hood.stopCalibrating();
- }, intake, hood));
- }
-
- // Stop intake roller
- controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
- if(intakeBoolean){
- intake.spinStart();
- intakeBoolean = false;
- } else{
- intake.spinStop();
- intakeBoolean = true;
- }
- }));
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
}
- // Calibration: you can now calibrate easily using this button
- if (hood != null && intake != null) {
- controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- hood.calibrate();
- }, intake, hood)).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- hood.stopCalibrating();
- }, intake, hood));
- }
-
+ }, intake));
+
+ // Retract if hold for 2 seconds
+ controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ intake.spinStop();
+ }, intake));
+
+ // Make the intake go in and out while shooting
+ controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake)
+ .alongWith(new InstantCommand(() -> intakeBoolean = true)));
+
+ // Stop intake roller
+ controller.get(DPad.DOWN).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.spinStop();
+ intakeBoolean = true;
+ }
+ }));
+ }
- // Spindexer
- if (spindexer != null && turret != null && hood != null && intake != null) {
+ // Spindexer
+ if (spindexer != null && turret != null && hood != null && intake != null) {
- // Toggle spindexer
- controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
- new RunSpindexer(spindexer, turret, hood, intake)
- );
- }
+ // Toggle spindexer
+ controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+ new RunSpindexer(spindexer, turret, hood, intake));
+ }
- // Auto shoot
- if (turret != null && hood != null && shooter != null && spindexer != null) {
- autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
- controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
- }
-
- // Hood
- if (hood != null) {
- // Set the hood down -- for safety measures under trench
- controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
- hood.forceHoodDown(true);
- }, hood)).onFalse(new InstantCommand(() -> {
- hood.forceHoodDown(false);
- }));
- }
-
- // shoot focus mode: reduces drive current
- if (spindexer != null) {
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
- .onFalse(new InstantCommand(() -> shootFocus(false)));
- }
- }
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
+ }
- private void shootFocus(boolean turnOn) {
- if (turnOn) {
- System.out.println("Shooting is now Focused");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT,
- DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
- } else {
- System.out.println("Shooting back to normal (From focus)");
- spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
- drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
+
+ // Hood
+ if (hood != null) {
+ // Set the hood down -- for safety measures under trench
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
+ hood.forceHoodDown(true);
+ }, hood)).onFalse(new InstantCommand(()->{
+ hood.forceHoodDown(false);
+ }));
+ }
}
- }
- @Override
- public double getRawSideTranslation() {
- return controller.get(PS5Axis.LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(PS5Axis.LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(PS5Axis.RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(PS5Axis.LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(PS5Axis.LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(PS5Axis.RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
}
*/
public class PS5XboxModeDriverConfig extends BaseDriverConfig {
- private final GameController controller = new GameController(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = () -> false;
- private boolean intakeBoolean = true;
- private Command autoShoot = null;
- private Command reverseMotors = null;
- private Shooter shooter;
- private Turret turret;
- private Hood hood;
- private Intake intake;
- private Spindexer spindexer;
-
- // PS5 button aliases
- private final Button CROSS = Button.A;
- private final Button CIRCLE = Button.B;
- private final Button SQUARE = Button.X;
- private final Button TRIANGLE = Button.Y;
- private final Button LB = Button.LB;
- private final Button RB = Button.RB;
- private final Button CREATE = Button.BACK;
- private final Button OPTIONS = Button.START;
- private final Button LEFT_JOY = Button.LEFT_JOY;
- private final Button RIGHT_JOY = Button.RIGHT_JOY;
+ private final GameController controller = new GameController(Constants.DRIVER_JOY);
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
+ private Command reverseMotors = null;
+ private Shooter shooter;
+ private Turret turret;
+ private Hood hood;
+ private Intake intake;
+ private Spindexer spindexer;
+
+ // PS5 button aliases
+ // private final Button CROSS = Button.A;
+ private final Button CIRCLE = Button.B;
+ private final Button SQUARE = Button.X;
+ // private final Button TRIANGLE = Button.Y;
+ // private final Button LB = Button.LB;
+ private final Button RB = Button.RB;
+ private final Button CREATE = Button.BACK;
+ // private final Button OPTIONS = Button.START;
+ private final Button LEFT_JOY = Button.LEFT_JOY;
+ private final Button RIGHT_JOY = Button.RIGHT_JOY;
- // PS5 trigger buttons
- private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
- private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
+ // PS5 trigger buttons
+ private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON;
+ private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON;
- // PS5 axis aliases
- private final Axis LEFT_X = Axis.LEFT_X;
- private final Axis LEFT_Y = Axis.LEFT_Y;
- private final Axis RIGHT_X = Axis.RIGHT_X;
- private final Axis RIGHT_Y = Axis.RIGHT_Y;
- private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
- private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
-
- public PS5XboxModeDriverConfig(
- Drivetrain drive,
- Shooter shooter,
- Turret turret,
- Hood hood,
- Intake intake,
- Spindexer spindexer) {
- super(drive);
- this.shooter = shooter;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
- this.spindexer = spindexer;
- }
+ // PS5 axis aliases
+ private final Axis LEFT_X = Axis.LEFT_X;
+ private final Axis LEFT_Y = Axis.LEFT_Y;
+ private final Axis RIGHT_X = Axis.RIGHT_X;
+ private final Axis RIGHT_Y = Axis.RIGHT_Y;
+ // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
+ // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
+
+ public PS5XboxModeDriverConfig(
+ Drivetrain drive,
+ Shooter shooter,
+ Turret turret,
+ Hood hood,
+ Intake intake,
+ Spindexer spindexer) {
+ super(drive);
+ this.shooter = shooter;
+ this.turret = turret;
+ this.hood = hood;
+ this.intake = intake;
+ this.spindexer = spindexer;
+ }
- public void configureControls() {
- // Reset the yaw. Mainly useful for testing/driver practice
- controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
-
- // Cancel commands
- controller.get(RB).onTrue(new InstantCommand(() -> {
- getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(() -> null);
- CommandScheduler.getInstance().cancelAll();
+ public void configureControls() {
+ // Reset the yaw. Mainly useful for testing/driver practice
+ controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
+
+ // Cancel commands
+ controller.get(RB).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setIsAlign(false);
+ getDrivetrain().setDesiredPose(() -> null);
+ CommandScheduler.getInstance().cancelAll();
+ }));
+
+ // Align wheels
+ controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
+
+ // Trench align
+ controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(true);
+ getDrivetrain().setTrenchAlign(true);
+ }))
+ .onFalse(new InstantCommand(() -> {
+ getDrivetrain().setTrenchAssist(false);
+ getDrivetrain().setTrenchAlign(false);
}));
- // Align wheels
- controller.get(DPad.RIGHT).onTrue(new FunctionalCommand(
- () -> getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted -> getDrivetrain().setStateDeadband(true),
- () -> false, getDrivetrain()).withTimeout(2));
-
- // Trench align
- controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(true);
- getDrivetrain().setTrenchAlign(true);
- }))
- .onFalse(new InstantCommand(() -> {
- getDrivetrain().setTrenchAssist(false);
- getDrivetrain().setTrenchAlign(false);
- }));
-
- // Reverse motors
- if (intake != null && spindexer != null && shooter != null) {
- controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
- reverseMotors = new ReverseMotors(intake);
- CommandScheduler.getInstance().schedule(reverseMotors);
- })).onFalse(new InstantCommand(() -> {
- if (reverseMotors != null) {
- reverseMotors.cancel();
- }
- }));
+ // Reverse motors
+ if (intake != null && spindexer != null && shooter != null) {
+ controller.get(CIRCLE).onTrue(new InstantCommand(() -> {
+ reverseMotors = new ReverseMotors(intake);
+ CommandScheduler.getInstance().schedule(reverseMotors);
+ })).onFalse(new InstantCommand(() -> {
+ if (reverseMotors != null) {
+ reverseMotors.cancel();
}
+ }));
+ }
- // Intake
- if (intake != null) {
- // Toggle intake
- controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
- if (intakeBoolean) {
- intake.extend();
- intake.spinStart();
- intakeBoolean = false;
- } else {
- intake.intermediateExtend();
- intake.spinStop();
- intakeBoolean = true;
- }
- }));
-
- // Retract if hold for 3 seconds
- controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
- intake.retract();
- intakeBoolean = true;
- }));
-
- // Calibration
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- })).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- }));
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
+ } else {
+ intake.intermediateExtend();
+ intake.spinStop();
+ intakeBoolean = true;
}
+ }));
- // Spindexer
- if (spindexer != null) {
- // Will only run if we are not calling default shoot command
- controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
- .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
- }
+ // Retract if hold for 3 seconds
+ controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> {
+ intake.retract();
+ intakeBoolean = true;
+ }));
+
- // Calibration
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- intake.calibrate();
- })).onFalse(new InstantCommand(() -> {
- intake.stopCalibrating();
- }));
+ }
+
+ // Spindexer
+ if (spindexer != null) {
+ // Will only run if we are not calling default shoot command
+ controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
+ .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
+ }
- // Auto shoot
- if (turret != null && hood != null && shooter != null) {
- controller.get(SQUARE).onTrue(
- new InstantCommand(() -> {
- if (autoShoot != null && autoShoot.isScheduled()) {
- autoShoot.cancel();
- } else {
- autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
- CommandScheduler.getInstance().schedule(autoShoot);
- }
- }));
- }
-
- // Climb
-
- // Hood
- if (hood != null) {
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- hood.calibrate();
- })).onFalse(new InstantCommand(() -> {
- hood.stopCalibrating();
- }));
- }
+ // Auto shoot
+ if (turret != null && hood != null && shooter != null && spindexer != null) {
+ autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
+ controller.get(SQUARE).toggleOnTrue(autoShoot);
+ }
- // Hood
- if (hood != null) {
- controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
- hood.calibrate();
- })).onFalse(new InstantCommand(() -> {
- hood.stopCalibrating();
- }));
- }
-
- // Rumble test
- controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
- new WaitCommand(0.5),
- new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
- }
-
- @Override
- public double getRawSideTranslation() {
- return controller.get(LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(LEFT_Y);
- }
-
- @Override
- public double getRawRotation() {
- return controller.get(RIGHT_X);
- }
-
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
- }
-
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
- }
-
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
- }
-
- @Override
- public boolean getIsAlign() {
- return false;
- }
-
- public void startRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
- }
-
- public void endRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
- }
++
+ // Rumble test
+ controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup(
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)),
+ new WaitCommand(0.5),
+ new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF))));
+ }
+
+ @Override
+ public double getRawSideTranslation() {
+ return controller.get(LEFT_X);
+ }
+
+ @Override
+ public double getRawForwardTranslation() {
+ return controller.get(LEFT_Y);
+ }
+
+ @Override
+ public double getRawRotation() {
+ return controller.get(RIGHT_X);
+ }
+
+ @Override
+ public double getRawHeadingAngle() {
+ return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2;
+ }
+
+ @Override
+ public double getRawHeadingMagnitude() {
+ return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ }
+
+ @Override
+ public boolean getIsSlowMode() {
+ return slowModeSupplier.getAsBoolean();
+ }
+
+ @Override
+ public boolean getIsAlign() {
+ return false;
+ }
+
+ public void startRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ }
+
+ public void endRumble() {
+ controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF);
+ }
}
import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
- import edu.wpi.first.math.filter.Debouncer;
- import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color8Bit;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
- import frc.robot.constants.IntakeConstants;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-public class Intake extends SubsystemBase implements IntakeIO{
- // Mechanism Display...
- private final Mechanism2d mechanism;
- private final MechanismLigament2d robotExtension;
- @SuppressWarnings("unused")
- private final MechanismLigament2d robotFrame;
- private final MechanismLigament2d robotHeight;
- private final MechanismLigament2d robotPos;
-
- // create the motors
- /** Motor to move the roller */
- private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB);
- /** Right motor (master) */
- private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB);
- /** Left motor (slave) */
- private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB);
-
- /** Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox) */
- private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1);
- /** Motor characteristics for the extending pair of Kraken X44 motors (aka gearbox) */
- private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2);
-
- private double maxVelocity;
- private double maxAcceleration;
-
- // Use FlywheelSim for the roller
- private FlywheelSim rollerSim;
-
- // Use ElevatorSim for the extender
- private ElevatorSim intakeSim;
-
- private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
-
- private double setpointInches = 0.0;
+public class Intake extends SubsystemBase {
- private boolean calibrating = false;
- private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
+ private boolean calibrating = false;
- private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
-
- public Intake() {
-
- // get the maximum free speed
- double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec)/ IntakeConstants.GEAR_RATIO;
-
- // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio
- // safety margin, limits velocity to .75 free speed
- maxVelocity = 0.75 * maxFreeSpeed;
- maxAcceleration = maxVelocity / 0.25;
-
- // ----Rollers
- // Configure the motors
- // Build the configuration for the roller
- TalonFXConfiguration rollerConfig = new TalonFXConfiguration();
-
- // config Slot 0 PID params
- var slot0Configs = rollerConfig.Slot0;
- slot0Configs.kP = 5.0;
- slot0Configs.kI = 0.0;
- slot0Configs.kD = 0.0;
- slot0Configs.kV = 0.0;
- slot0Configs.kA = 0.0;
-
- // set the brake mode
- rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
-
- // apply the configuration to the right motor (master)
- rollerMotor.getConfigurator().apply(rollerConfig);
-
- // --- Extenders
-
- // Build the configuration for the left and right Motor
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- // config Slot 0 PID params
- var extenderSlot0Configs = config.Slot0;
- extenderSlot0Configs.kP = 0.5;
- extenderSlot0Configs.kI = 0.0;
- extenderSlot0Configs.kD = 0.0;
- extenderSlot0Configs.kV = 0.0;
- extenderSlot0Configs.kA = 0.0;
-
- // configure MotionMagic
- MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
-
- motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2;
- motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration/IntakeConstants.RADIUS_RACK_PINION/Math.PI/2;
-
- rightMotor.getConfigurator().apply(config);
- leftMotor.getConfigurator().apply(config);
-
- leftMotor.getConfigurator().apply(
- new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
- .withNeutralMode(NeutralModeValue.Coast)
- );
-
- rightMotor.getConfigurator().apply(
- new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
- );
-
-
- leftMotor.setPosition(0.0);
- rightMotor.setPosition(0.0);
-
- setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
-
- // Build the mechanism for display
- mechanism = new Mechanism2d(80, 80);
- MechanismRoot2d root = mechanism.getRoot("Root", 0, 1);
- robotPos = root.append(new MechanismLigament2d("robotPos", 40, 0.0, 1, new Color8Bit(0, 0, 0)));
- robotFrame = robotPos.append(new MechanismLigament2d("Robot Frame",28,0.0, 2, new Color8Bit(0, 255, 255)));
- robotHeight = robotPos.append(new MechanismLigament2d("Robot Height", 22.5, 90, 1, new Color8Bit(0,255,255)));
- // extensiion is initially retracted.
- robotExtension = robotHeight.append(new MechanismLigament2d("Robot Extension", 0, 90, 2, new Color8Bit(255, 0, 0) ));
-
- // add some test commands.
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Extension Mechanism", mechanism);
- SmartDashboard.putData("Intake Calibrate", new InstantCommand(() -> calibrate()));
- SmartDashboard.putData("Intake Stop Calibrating", new InstantCommand(() -> stopCalibrating()));
- SmartDashboard.putData("Extend Intake", new InstantCommand(() -> extend()));
- SmartDashboard.putData("Retract Intake", new InstantCommand(() -> retract()));
- }
-
- if (RobotBase.isSimulation()) {
- // Extender simulation
- // the supply voltage should change with load....
- rightMotor.getSimState().setSupplyVoltage(12.0);
-
- // rack pinion is 10 teeth and 10 DP for a radius of 1 inches
- double drumRadiusMeters = Units.inchesToMeters(1.0);
- double minHeightMeters = Units.inchesToMeters(0.0);
- double maxHeightMeters = Units.inchesToMeters(IntakeConstants.MAX_EXTENSION);
- // start retracted
- double startingHeightMeters = Units.inchesToMeters(0.0);
- intakeSim = new ElevatorSim(
- dcMotorExtend,
- IntakeConstants.GEAR_RATIO,
- IntakeConstants.CARRIAGE_MASS_KG,
- drumRadiusMeters,
- minHeightMeters,
- maxHeightMeters,
- false,
- startingHeightMeters);
-
- // Roller simulation
- rollerSim = new FlywheelSim(
- LinearSystemId.createFlywheelSystem(dcMotorRoller, IntakeConstants.ROLLER_MOI_KG_M_SQ, IntakeConstants.ROLLER_GEARING),
- dcMotorRoller);
- }
- }
-
- public void periodic() {
- double inchExtension = getPosition();
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Intake/Setpoint", setpointInches);
- }
- robotExtension.setLength(inchExtension);
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Intake Extension (in)", inchExtension);
- SmartDashboard.putBoolean("Intake Extended", inchExtension > 1.0);
- }
-
- if(calibrating){
- leftMotor.set(-0.1);
- rightMotor.set(-0.1);
- }
-
- updateInputs();
- Logger.processInputs("Intake", inputs);
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putBoolean("Intake Calibrated", !calibrating);
- SmartDashboard.putBoolean("Intake At Setpoint", Math.abs(inchExtension - setpointInches) < 0.5);
- }
- }
-
- public void simulationPeriodic(){
- // get the applied motor voltage
- double voltage = rightMotor.getMotorVoltage().getValueAsDouble();
-
- // tell the simulator that voltage
- intakeSim.setInputVoltage(voltage);
- // run the siimulation
- intakeSim.update(0.02);
-
- // get the simulation result
- double metersExtend = intakeSim.getPositionMeters();
- double inchesExtend = Units.metersToInches(metersExtend);
- double motorRotations = inchesToRotations(inchesExtend);
-
- // set the motor to that position
- rightMotor.getSimState().setRawRotorPosition(motorRotations);
-
- // update the display
- robotExtension.setLength(inchesExtend);
-
- // simulate roller
- voltage = rollerMotor.getMotorVoltage().getValueAsDouble();
- rollerSim.setInputVoltage(voltage);
- rollerSim.update(0.020);
-
- double velocity = Units.radiansToRotations(rollerSim.getAngularVelocityRadPerSec()) * IntakeConstants.ROLLER_GEARING;
-
- rollerMotor.getSimState().setRotorVelocity(velocity);
- }
-
- /**
- * Set the intake extender position
- * @param setpoint in inches
- */
- public void setPosition(double setpoint) {
- double motorRotations = -inchesToRotations(setpoint);
- rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
- leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
-
- setpointInches = setpoint;
- }
-
- /**
- * Get the intake extender position
- * @return inches
- */
- public double getPosition(){
- return inputs.leftPosition;
- }
-
- /**
- * convert rotations to inches
- * @param rotations of the motor
- * @return inches of rack travel
- */
- public double rotationsToInches(double motorRotations) {
- // circumference of the rack pinion
- double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
- double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO;
- double inches = pinionRotations * circ;
- return inches;
- }
-
- /**
- * convert inches to rotations
- * @param inches of rack travel
- * @return motor rotations
- */
- public double inchesToRotations(double inches){
- double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
- double pinionRotations = inches / circ;
- double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO;
- return motorRotations;
- }
-
- /**
- * Set the roller speed.
- * @param speed duty cycle in the range [-1, 1]
- */
- public void spin(double speed) {
- rollerMotor.set(speed);
- }
-
- public double getSpeed() {
- return rollerMotor.get();
- }
-
- /**
- * Start the intake roller spinning.
- */
- public void spinStart() {
- spin(IntakeConstants.SPEED);
- }
-
- /**
- * Stop the intake roller.
- */
- public void spinStop() {
- spin(0.0);
- }
-
- /**
- * Reverses the intake roller
- */
- public void spinReverse() {
- spin(-IntakeConstants.SPEED);
- }
-
- /** Extend the intake the maximum distance. */
- public void extend() {
- setPosition(IntakeConstants.MAX_EXTENSION);
- }
-
- /** Extend to a position that doesn't hit the spindexer */
- public void intermediateExtend(){
- setPosition(IntakeConstants.INTERMEDIATE_EXTENSION);
- }
-
- /** Retract the intake to a safe starting position. */
- public void retract(){
- setPosition(IntakeConstants.STOW_EXTENSION);
- }
-
- /** Goes to the zero position */
- public void zeroPosition(){
- setPosition(0.0);
- }
-
- public void zeroMotors() {
- rightMotor.setPosition(0.0);
- leftMotor.setPosition(0.0);
- }
-
- /**
- * Reclaim all the resources (e.g., motors and other devices).
- * This step is necessary for multiple unit tests to work.
- */
- public void close() {
- leftMotor.close();
- rightMotor.close();
- rollerMotor.close();
- }
-
- /**
- * Starts calibrating by running it backwards
- */
- public void calibrate(){
- setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS);
- calibrating = true;
- }
-
- /**
- * Stops, zeros, and moves it to retract position
- */
- public void stopCalibrating(){
- zeroMotors();
- setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
- calibrating = false;
- retract();
- }
-
- public void setNewCurrentLimit(double statorExtenders, double supplyExtenders, double statorRoller, double supplyRollers) {
- CurrentLimitsConfigs limitConfigExtender = new CurrentLimitsConfigs();
- limitConfigExtender.StatorCurrentLimit = statorExtenders;
- limitConfigExtender.StatorCurrentLimitEnable = true;
- limitConfigExtender.SupplyCurrentLimit = statorExtenders;
- limitConfigExtender.SupplyCurrentLimitEnable = true;
- leftMotor.getConfigurator().apply(limitConfigExtender);
- rightMotor.getConfigurator().apply(limitConfigExtender);
-
- // roller
- CurrentLimitsConfigs limitConfigRoller = new CurrentLimitsConfigs();
- limitConfigRoller.StatorCurrentLimit = statorRoller;
- limitConfigRoller.StatorCurrentLimitEnable = true;
- limitConfigRoller.SupplyCurrentLimit = supplyRollers;
- limitConfigRoller.SupplyCurrentLimitEnable = true;
- rollerMotor.getConfigurator().apply(limitConfigRoller);
- }
-
- public double getSubsystemStatorCurrent() {
- return inputs.leftStatorCurrent + inputs.rightStatorCurrent + inputs.rollerStatorCurrent;
- }
-
- public double getSubsystemSupplyCurrent() {
- return inputs.leftSupplyCurrent + inputs.rightSupplyCurrent + inputs.rollerSupplyCurrent;
- }
-
- @Override
- public void updateInputs() {
- inputs.leftPosition = rotationsToInches(leftMotor.getPosition().getValueAsDouble());
- inputs.rightPosition = rotationsToInches(rightMotor.getPosition().getValueAsDouble());
- inputs.leftStatorCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
- inputs.rightStatorCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
- inputs.leftSupplyCurrent = leftMotor.getSupplyCurrent().getValueAsDouble();
- inputs.rightSupplyCurrent = rightMotor.getSupplyCurrent().getValueAsDouble();
- inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
- inputs.rollerStatorCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
- inputs.rollerSupplyCurrent = rollerMotor.getSupplyCurrent().getValueAsDouble();
- }
-
+ private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+ private final IntakeIO io;
+
+ public Intake(IntakeIO io) {
+ this.io = io;
+ }
+
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Intake", inputs);
+
+ double inchExtension = inputs.leftPosition;
+
+ if (calibrating) {
+ io.setRightMotor(-0.1);
+ io.setLeftMotor(-0.1);
+ boolean atHardStop = Math
+ .abs((inputs.leftCurrent + inputs.rightCurrent)
+ / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD;
+ }
+
+ }
+
+ /**
+ * convert rotations to inches
+ *
+ * @param rotations of the motor
+ * @return inches of rack travel
+ */
+ public static double rotationsToInches(double motorRotations) {
+ // circumference of the rack pinion
- double circ = 2 * Math.PI * 0.5;
++ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
+ double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO;
+ double inches = pinionRotations * circ;
+ return inches;
+ }
+
+ /**
+ * convert inches to rotations
+ *
+ * @param inches of rack travel
+ * @return motor rotations
+ */
+ public static double inchesToRotations(double inches) {
- double circ = 2 * Math.PI * 0.5;
++ double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
+ double pinionRotations = inches / circ;
+ double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO;
+ return motorRotations;
+ }
+
+ /**
+ * Set the roller speed.
+ *
+ * @param speed duty cycle in the range [-1, 1]
+ */
+ public void spin(double speed) {
+ io.setRollerMotor(speed);
+ }
+
+ public double getSpeed() {
+ return inputs.rollerSetSpeed;
+ }
+
+ /**
+ * Get the intake extender position
+ *
+ * @return inches
+ */
+ public double getPosition() {
+ return inputs.leftPosition;
+ }
+
+ /**
+ * Start the intake roller spinning.
+ */
+ public void spinStart() {
+ spin(IntakeConstants.SPEED);
+ }
+
+ /**
+ * Stop the intake roller.
+ */
+ public void spinStop() {
+ spin(0.0);
+ }
+
+ /**
+ * Reverses the intake roller
+ */
+ public void spinReverse() {
+ spin(-IntakeConstants.SPEED);
+ }
+
+ /** Extend the intake the maximum distance. */
+ public void extend() {
+ io.setPosition(IntakeConstants.MAX_EXTENSION);
+ }
+
+ /** Extend to a position that doesn't hit the spindexer */
+ public void intermediateExtend() {
+ io.setPosition(IntakeConstants.INTERMEDIATE_EXTENSION);
+ }
+
+ /** Retract the intake to a safe starting position. */
+ public void retract() {
+ io.setPosition(IntakeConstants.STOW_EXTENSION);
+ }
+
+ /** Goes to the zero position */
+ public void zeroPosition() {
+ io.setPosition(0.0);
+ }
+
+ public void zeroMotors() {
+ io.setRawPosition(0.0);
+ }
+
+ /**
+ * Reclaim all the resources (e.g., motors and other devices).
+ * This step is necessary for multiple unit tests to work.
+ */
+ public void close() {
+ io.close();
+ }
+
- /**
- * Starts calibrating by running it backwards
- */
- public void calibrate() {
- setCurrentLimits(IntakeConstants.CALIBRATING_CURRENT_LIMITS);
- calibrating = true;
- }
-
- /**
- * Stops, zeros, and moves it to retract position
- */
- public void stopCalibrating() {
- zeroMotors();
- setCurrentLimits(IntakeConstants.EXTENDER_CURRENT_LIMITS);
- calibrating = false;
- retract();
- }
++// /**
++// * Starts calibrating by running it backwards
++// */
++// public void calibrate() {
++// setNewCurrentLimit(IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS, IntakeConstants.CALIBRATING_CURRENT_LIMITS);
++// calibrating = true;
++// }
++
++// /**
++// * Stops, zeros, and moves it to retract position
++// */
++// public void stopCalibrating() {
++// zeroMotors();
++// setNewCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT, IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT, IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT, IntakeConstants.SUPPLY_ROLLER_CURRENT_LIMIT);
++// calibrating = false;
++// retract();
++// }
+
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limit the current limit for stator and supply current
+ */
+ public void setCurrentLimits(double limit) {
+ CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
+ .withStatorCurrentLimitEnable(true)
+ .withStatorCurrentLimit(limit)
+ .withSupplyCurrentLimitEnable(true)
+ .withSupplyCurrentLimit(limit);
+
+ io.setLimits(limits);
+ }
}
--- /dev/null
- import frc.robot.constants.IntakeConstants;
+package frc.robot.subsystems.Intake;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
- .withStatorCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
+
+public class IntakeIOTalonFX implements IntakeIO {
+
+ // create the motors
+ /** Motor to move the roller */
+ private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Right motor (master) */
+ private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Left motor (slave) */
+ private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB);
+
+ private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+
+ /**
+ * Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox)
+ */
+ private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1);
+ /**
+ * Motor characteristics for the extending pair of Kraken X44 motors (aka
+ * gearbox)
+ */
+ private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2);
+
+ public IntakeIOTalonFX() {
+
+ // get the maximum free speed
+ double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec) / IntakeConstants.GEAR_RATIO;
+
+ // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio
+ // safety margin, limits velocity to .75 free speed
+ double maxVelocity = 0.75 * maxFreeSpeed;
+ double maxAcceleration = maxVelocity / 0.25;
+
+ // Configure the motors
+ // Build the configuration for the roller
+ TalonFXConfiguration rollerConfig = new TalonFXConfiguration();
+
+ // config Slot 0 PID params
+ var slot0Configs = rollerConfig.Slot0;
+ slot0Configs.kP = 5.0;
+ slot0Configs.kI = 0.0;
+ slot0Configs.kD = 0.0;
+ slot0Configs.kV = 0.0;
+ slot0Configs.kA = 0.0;
+
+ // set the brake mode
+ rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
+
+ // apply the configuration to the right motor (master)
+ rollerMotor.getConfigurator().apply(rollerConfig);
+
+ // Build the configuration for the left and right Motor
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // config the current limits (low value for testing)
+ config.CurrentLimits
- .withSupplyCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
++ .withStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_EXTENDER_LIMIT)
+ .withStatorCurrentLimitEnable(true)
- limitConfig.StatorCurrentLimit = IntakeConstants.NORMAL_CURRENT_LIMIT;
++ .withSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_EXTENDER_LIMIT)
+ .withSupplyCurrentLimitEnable(true);
+
+ // config Slot 0 PID params
+ var rollerSlot0Configs = config.Slot0;
+ rollerSlot0Configs.kP = 5.0;
+ rollerSlot0Configs.kI = 0.0;
+ rollerSlot0Configs.kD = 0.0;
+ rollerSlot0Configs.kV = 0.0;
+ rollerSlot0Configs.kA = 0.0;
+
+ // configure MotionMagic
+ MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
+
+ motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity
+ / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2;
+ motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration
+ / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2;
+
+ rightMotor.getConfigurator().apply(config);
+ leftMotor.getConfigurator().apply(config);
+
+ leftMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast));
+
+ rightMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
+
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
++ limitConfig.StatorCurrentLimit = IntakeConstants.STATOR_ROLLER_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ leftMotor.getConfigurator().apply(limitConfig);
+ rightMotor.getConfigurator().apply(limitConfig);
+
+ leftMotor.setPosition(0.0);
+ rightMotor.setPosition(0.0);
+ }
+
+ @Override
+ public void updateInputs(IntakeIOInputs inputs) {
+ inputs.leftPosition = Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble());
+ inputs.rightPosition = Intake.rotationsToInches(rightMotor.getPosition().getValueAsDouble());
+ inputs.leftCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
+ inputs.rollerCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightVoltage = rightMotor.getMotorVoltage().getValueAsDouble();
+ inputs.leftVoltage = leftMotor.getMotorVoltage().getValueAsDouble();
+ inputs.rollerSetSpeed = rollerMotor.get();
+ }
+
+ /**
+ * Set the intake extender position
+ *
+ * @param setpoint in inches
+ */
+ @Override
+ public void setPosition(double setpoint) {
+ double motorRotations = -Intake.inchesToRotations(setpoint);
+ rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+ leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+ }
+
+ @Override
+ public void setLeftMotor(double speed) {
+ leftMotor.set(speed);
+ }
+
+ @Override
+ public void setRightMotor(double speed) {
+ rightMotor.set(speed);
+ }
+
+ @Override
+ public void setRollerMotor(double speed) {
+ rollerMotor.set(speed);
+ }
+
+ @Override
+ public void setLimits(CurrentLimitsConfigs limits) {
+ leftMotor.getConfigurator().apply(limits);
+ rightMotor.getConfigurator().apply(limits);
+ }
+
+ @Override
+ public void setRawPosition(double pos) {
+ leftMotor.setPosition(pos);
+ rightMotor.setPosition(pos);
+ }
+
+
+ @Override
+ public void close() {
+ leftMotor.close();
+ rightMotor.close();
+ rollerMotor.close();
+ }
+
+}
}
// for current limit setting (brownout protection)
- public void applyNewModuleCurrents(double steerCurrent, double driveCurrent) {
+ public void applyNewModuleCurrents(
+ double steerCurrentStator, double steerCurrentSupply,
+ double driveCurrentStator, double driveCurrentSupply) {
for (Module module : modules) { // iterate over our modules
- module.setNewCurrentLimit(steerCurrent, driveCurrent);
+ module.setNewCurrentLimit(steerCurrentStator, steerCurrentSupply, driveCurrentStator, driveCurrentSupply);
}
}
- public double getSubsystemStatorCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleStatorCurrent();
- }
- return sum;
- }
-
- public double getSubsystemSupplyCurrent() {
- double sum = 0;
- for (Module module : modules) {
- sum += module.getModuleSupplyCurrent();
- }
- return sum;
- }
+
/**
* Sets the desired states for all swerve modules.
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.SensorDirectionValue;
++import com.ctre.phoenix6.swerve.jni.SwerveJNI.DriveState;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.Debouncer;
import frc.robot.util.PhoenixOdometryThread;
import lib.CTREModuleState;
-
-public class Module implements ModuleIO{
- private final ModuleType type;
-
- // Degrees
- private final double angleOffset;
-
- private final TalonFX angleMotor;
- private final TalonFX driveMotor;
- private final CANcoder CANcoder;
- private SwerveModuleState desiredState;
-
- protected boolean stateDeadband = true;
-
- private boolean optimizeStates = true;
-
- // Inputs from drive motor
- private final StatusSignal<Angle> drivePosition;
- private final StatusSignal<AngularVelocity> driveVelocity;
- private final StatusSignal<Voltage> driveAppliedVolts;
- private final StatusSignal<Current> driveCurrent;
-
- // Inputs from turn motor
- private final StatusSignal<Angle> turnAbsolutePosition;
- private final StatusSignal<Angle> turnPosition;
- private final StatusSignal<AngularVelocity> turnVelocity;
- private final StatusSignal<Voltage> turnAppliedVolts;
- private final StatusSignal<Current> turnCurrent;
-
- // Timestamp inputs from Phoenix thread
- protected final Queue<Double> timestampQueue;
- protected final Queue<Double> drivePositionQueue;
- protected final Queue<Double> turnPositionQueue;
-
- private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
- // Connection debouncers
- private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
- private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
-
- private final Alert driveDisconnectedAlert;
- private final Alert turnDisconnectedAlert;
- private final Alert turnEncoderDisconnectedAlert;
-
- protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
-
- private ModuleConstants moduleConstants;
- private final MotionMagicVelocityVoltage velocityRequest =
- new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
-
-
- public Module(ModuleConstants moduleConstants) {
- this.moduleConstants = moduleConstants;
-
- type = moduleConstants.getType();
- angleOffset = moduleConstants.getSteerOffset();
-
- /* Angle Encoder Config */
- CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
- /* Angle Motor Config */
- angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
- driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
- // Create drive status signals
- drivePosition = driveMotor.getPosition();
- driveVelocity = driveMotor.getVelocity();
- driveAppliedVolts = driveMotor.getMotorVoltage();
- driveCurrent = driveMotor.getStatorCurrent();
-
- // Create turn status signals
- turnAbsolutePosition = CANcoder.getAbsolutePosition();
- turnPosition = angleMotor.getPosition();
- turnVelocity = angleMotor.getVelocity();
- turnAppliedVolts = angleMotor.getMotorVoltage();
- turnCurrent = angleMotor.getStatorCurrent();
-
- // Create timestamp queue
- timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- drivePositionQueue =
- PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
- updateInputs();
-
- configCANcoder();
- configAngleMotor();
- configDriveMotor();
-
- driveDisconnectedAlert =
- new Alert(
- "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
- turnDisconnectedAlert =
- new Alert(
- "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
- turnEncoderDisconnectedAlert =
- new Alert(
- "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
- AlertType.kError);
-
-
- // Configure periodic frames
- BaseStatusSignal.setUpdateFrequencyForAll(
- 250, drivePosition, turnPosition);
- BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0,
- driveVelocity,
- driveAppliedVolts,
- driveCurrent,
- turnAbsolutePosition,
- turnVelocity,
- turnAppliedVolts,
- turnCurrent);
-
- setDesiredState(new SwerveModuleState(0, getAngle()), false);
- }
-
- public void close() {
- angleMotor.close();
- driveMotor.close();
- CANcoder.close();
- }
-
- @Override
- public void updateInputs() {
- // Refresh all signals
- var driveStatus =
- BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var turnStatus =
- BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
- var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
-
- // Update drive inputs
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
- inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO);
- inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
- inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
-
- // Update turn inputs
- inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
- inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
- inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
- inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
-
- // Update encoder inputs
- inputs.encoderOffset = Units.rotationsToDegrees(CANcoder.getAbsolutePosition().getValueAsDouble());
-
- // Update odometry inputs
- inputs.odometryTimestamps =
- timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
- inputs.odometryDrivePositionsRad =
- drivePositionQueue.stream()
- .mapToDouble((Double value) -> Units.rotationsToRadians(value))
- .toArray();
- inputs.odometryTurnPositions =
- turnPositionQueue.stream()
- .map((Double value) -> Rotation2d.fromRotations(value))
- .toArray(Rotation2d[]::new);
- timestampQueue.clear();
- drivePositionQueue.clear();
- turnPositionQueue.clear();
-
- inputs.driveStator = driveMotor.getStatorCurrent().getValueAsDouble();
- inputs.driveSupply = driveMotor.getSupplyCurrent().getValueAsDouble();
- inputs.steerStator = angleMotor.getStatorCurrent().getValueAsDouble();
- inputs.steerSupply = angleMotor.getSupplyCurrent().getValueAsDouble();
- }
-
- public void periodic() {
- updateInputs();
- Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
-
- // Calculate positions for odometry
- int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
- odometryPositions = new SwerveModulePosition[sampleCount];
- for (int i = 0; i < sampleCount; i++) {
- double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS;
- Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
- }
- // Update alerts
- driveDisconnectedAlert.set(!inputs.driveConnected);
- turnDisconnectedAlert.set(!inputs.turnConnected);
- turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
- }
- }
-
- public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
- // Separate if here and in setAngle() to avoid warning
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- /*
- * This is a custom optimize function, since default WPILib optimize assumes
- * continuous controller which CTRE and Rev onboard is not
- */
- desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
- }else{
- desiredState = wantedState;
- }
- setAngle();
- setSpeed(isOpenLoop);
- }
-
- public void setSpeed(boolean isOpenLoop) {
- if(desiredState == null){
- return;
- }
- if (isOpenLoop) {
- double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
- driveMotor.setControl(new DutyCycleOut(percentOutput));
- } else {
- double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO;
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
- }
-
- double feedforward = velocity * moduleConstants.getDriveV();
- driveMotor.setControl(
- velocityRequest
- .withVelocity(velocity)
- .withFeedForward(feedforward));
- }
- }
-
- private void setAngle() {
- if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){
- // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement.
- if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
- stop();
- return;
- }
- }
- if(desiredState == null){
- return;
- }
- angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setDriveVoltage(Voltage voltage){
- driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
- }
- public void setAngle(Rotation2d angle){
- angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio));
- }
-
- public void setOptimize(boolean enable) {
- optimizeStates = enable;
- }
-
- public byte getModuleIndex() {
- return type.id;
- }
-
- public Rotation2d getAngle() {
- return inputs.turnPosition;
- }
-
- public Rotation2d getCANcoder() {
- return inputs.turnAbsolutePosition;
- }
-
- public void resetToAbsolute() {
- // Sensor ticks
- double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
- angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio);
- }
-
- private void configCANcoder() {
- CANcoder.getConfigurator().apply(new CANcoderConfiguration());
- CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).
- withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive));
- }
-
- private void configAngleMotor() {
- angleMotor.getConfigurator().apply(new TalonFXConfiguration());
-
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(config);
- angleMotor.getConfigurator().apply(new Slot0Configs()
- .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
- .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
- .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
- angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
- angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
- angleMotor.setPosition(0);
-
- // optimize bus utilization for angle motor
- angleMotor.optimizeBusUtilization();
-
- resetToAbsolute();
- }
-
- /**
- * @return Speed in RPM
- */
- public double getDriveVelocity() {
- return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI;
- }
-
- public double getDriveVoltage(){
- return inputs.driveAppliedVolts;
- }
-
- public double getDriveStatorCurrent(){
- return inputs.driveCurrentAmps;
- }
-
- public double getModuleStatorCurrent() {
- return inputs.steerStator + inputs.driveStator;
- }
-
- public double getModuleSupplyCurrent() {
- return inputs.steerSupply + inputs.driveSupply;
- }
-
- // I took the config things straight from this file
- public void setNewCurrentLimit(double currentSteerStator, double currentSteerSupply, double currentDriveStator, double currentDriveSupply) {
- CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
- // steer
- steerConfig.SupplyCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimitEnable = true;
- steerConfig.StatorCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLimit = currentSteerSupply;
- steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
- angleMotor.getConfigurator().apply(steerConfig); // apply
-
- // drive
- CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
- driveConfig.SupplyCurrentLimitEnable = true;
- driveConfig.StatorCurrentLimitEnable = true;
- driveConfig.SupplyCurrentLimit = currentDriveSupply;
- driveConfig.StatorCurrentLimit = currentDriveStator;
- driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- driveMotor.getConfigurator().apply(driveConfig); // apply
- }
-
- private void configDriveMotor() {
- var talonFXConfigs = new TalonFXConfiguration();
- // set Motion Magic settings
- var motionMagicConfigs = talonFXConfigs.MotionMagic;
- motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO;
- var slot0Configs = talonFXConfigs.Slot0;
- slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
- slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
- slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
- slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
- slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
- slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
- driveMotor.getConfigurator().apply(talonFXConfigs);
- CurrentLimitsConfigs config = new CurrentLimitsConfigs();
- config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
- config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
- config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
- config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
- driveMotor.getConfigurator().apply(config);
- driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
- driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
- driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
- driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
-
- // optimize bus utilization for drive motor
- driveMotor.optimizeBusUtilization();
-
- }
-
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(
- inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS,
- getAngle());
- }
-
- public SwerveModuleState getDesiredState() {
- return desiredState;
- }
-
-
- public double getDriveVelocityError() {
- return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
- }
-
- public void stop() {
- driveMotor.set(0);
- angleMotor.set(0);
- }
-
- public ModuleType getModuleType(){
- return type;
- }
-
- public void setStateDeadband(boolean enabled) {
- stateDeadband = enabled;
- }
-
- public double getDesiredVelocity() {
- return getDesiredState().speedMetersPerSecond;
+public class Module implements ModuleIO {
+ private final ModuleType type;
+
+ // Degrees
+ private final double angleOffset;
+
+ private final TalonFX angleMotor;
+ private final TalonFX driveMotor;
+ private final CANcoder CANcoder;
+ private SwerveModuleState desiredState;
+
+ protected boolean stateDeadband = true;
+
+ private boolean optimizeStates = true;
+
+ // Inputs from drive motor
+ private final StatusSignal<Angle> drivePosition;
+ private final StatusSignal<AngularVelocity> driveVelocity;
+ private final StatusSignal<Voltage> driveAppliedVolts;
+ private final StatusSignal<Current> driveCurrent;
+
+ // Inputs from turn motor
+ private final StatusSignal<Angle> turnAbsolutePosition;
+ private final StatusSignal<Angle> turnPosition;
+ private final StatusSignal<AngularVelocity> turnVelocity;
+ private final StatusSignal<Voltage> turnAppliedVolts;
+ private final StatusSignal<Current> turnCurrent;
+
+ // Timestamp inputs from Phoenix thread
+ protected final Queue<Double> timestampQueue;
+ protected final Queue<Double> drivePositionQueue;
+ protected final Queue<Double> turnPositionQueue;
+
+ private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+
+ // Connection debouncers
+ private final Debouncer driveConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnConnectedDebounce = new Debouncer(0.5);
+ private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5);
+
+ private final Alert driveDisconnectedAlert;
+ private final Alert turnDisconnectedAlert;
+ private final Alert turnEncoderDisconnectedAlert;
+
+ protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
+
+ private ModuleConstants moduleConstants;
+ private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
+
+ public Module(ModuleConstants moduleConstants) {
+ this.moduleConstants = moduleConstants;
+
+ type = moduleConstants.getType();
+ angleOffset = moduleConstants.getSteerOffset();
+
+ /* Angle Encoder Config */
+ CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN);
+ /* Angle Motor Config */
+ angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN);
+ driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN);
+ // Create drive status signals
+ drivePosition = driveMotor.getPosition();
+ driveVelocity = driveMotor.getVelocity();
+ driveAppliedVolts = driveMotor.getMotorVoltage();
+ driveCurrent = driveMotor.getStatorCurrent();
+
+ // Create turn status signals
+ turnAbsolutePosition = CANcoder.getAbsolutePosition();
+ turnPosition = angleMotor.getPosition();
+ turnVelocity = angleMotor.getVelocity();
+ turnAppliedVolts = angleMotor.getMotorVoltage();
+ turnCurrent = angleMotor.getStatorCurrent();
+
+ // Create timestamp queue
+ timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
+ drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition());
+ turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition());
+ updateInputs();
+
+ configCANcoder();
+ configAngleMotor();
+ configDriveMotor();
+
+ driveDisconnectedAlert = new Alert(
+ "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+ turnDisconnectedAlert = new Alert(
+ "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError);
+ turnEncoderDisconnectedAlert = new Alert(
+ "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".",
+ AlertType.kError);
+
+ // Configure periodic frames
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 250, drivePosition, turnPosition);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 50.0,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent);
+
+ setDesiredState(new SwerveModuleState(0, getAngle()), false);
+ }
+
+ public void close() {
+ angleMotor.close();
+ driveMotor.close();
+ CANcoder.close();
+ }
+
+ @Override
+ public void updateInputs() {
+ // Refresh all signals
+ var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+
+ // Update drive inputs
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.drivePositionRad = Units
+ .rotationsToRadians(drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveVelocityRadPerSec = Units
+ .rotationsToRadians(driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO);
+ inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
+ inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
+
+ // Update turn inputs
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK());
+ inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+ inputs.turnPosition = Rotation2d
+ .fromRotations(turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnVelocityRadPerSec = Units
+ .rotationsToRadians(turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
+ inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
+
+ // Update encoder inputs
+ inputs.encoderOffset = inputs.turnAbsolutePosition.getDegrees();
+
+ // Update odometry inputs
+ inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
+ inputs.odometryDrivePositionsRad = drivePositionQueue.stream()
+ .mapToDouble((Double value) -> Units.rotationsToRadians(value))
+ .toArray();
+ inputs.odometryTurnPositions = turnPositionQueue.stream()
+ .map((Double value) -> Rotation2d.fromRotations(value))
+ .toArray(Rotation2d[]::new);
+ timestampQueue.clear();
+ drivePositionQueue.clear();
+ turnPositionQueue.clear();
+
+ }
+
+ public void periodic() {
+ updateInputs();
+ Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs);
+
+ // Calculate positions for odometry
+ int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ odometryPositions = new SwerveModulePosition[sampleCount];
+ for (int i = 0; i < sampleCount; i++) {
+ double positionMeters = inputs.odometryDrivePositionsRad[i] / DriveConstants.DRIVE_GEAR_RATIO
+ * DriveConstants.WHEEL_RADIUS;
+ Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ odometryPositions[i] = new SwerveModulePosition(positionMeters, angle);
+ }
+ // Update alerts
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Angle " + moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360));
+ }
+ }
+
+ public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) {
+ // Separate if here and in setAngle() to avoid warning
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ /*
+ * This is a custom optimize function, since default WPILib optimize assumes
+ * continuous controller which CTRE and Rev onboard is not
+ */
+ desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState;
+ } else {
+ desiredState = wantedState;
+ }
+ setAngle();
+ setSpeed(isOpenLoop);
+ }
+
+ public void setSpeed(boolean isOpenLoop) {
+ if (desiredState == null) {
+ return;
+ }
+ if (isOpenLoop) {
+ double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED;
+ driveMotor.setControl(new DutyCycleOut(percentOutput));
+ } else {
+ double velocity = desiredState.speedMetersPerSecond / DriveConstants.WHEEL_RADIUS / 2 / Math.PI
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity);
}
-
- public Rotation2d getDesiredAngle() {
- return getDesiredState().angle;
- }
-
- /** Returns the module positions received this cycle. */
- public SwerveModulePosition[] getOdometryPositions() {
- return odometryPositions;
- }
- /** Returns the timestamps of the samples received this cycle. */
- public double[] getOdometryTimestamps() {
- return inputs.odometryTimestamps;
- }
-
- /** returns the drive position status signal for time-synced odometry. */
- public StatusSignal<Angle> getDrivePositionSignal() {
- return drivePosition;
- }
-
- /** returns the turn position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnPositionSignal() {
- return turnPosition;
- }
-
- /** returns the turn absolute position status signal for time-synced odometry. */
- public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
- return turnAbsolutePosition;
- }
-
- public TalonFX[] getMotors() {
- return new TalonFX[]{angleMotor, driveMotor};
+ double feedforward = velocity * moduleConstants.getDriveV();
+ driveMotor.setControl(
+ velocityRequest
+ .withVelocity(velocity)
+ .withFeedForward(feedforward));
+ }
+ }
+
+ private void setAngle() {
+ if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) {
+ // Prevent rotating module if desired speed < 1%. Prevents jittering and
+ // unnecessary movement.
+ if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) {
+ stop();
+ return;
+ }
}
+ if (desiredState == null) {
+ return;
+ }
+ angleMotor.setControl(
+ new PositionDutyCycle(desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setDriveVoltage(Voltage voltage) {
+ driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude()));
+ }
+
+ public void setAngle(Rotation2d angle) {
+ angleMotor.setControl(new PositionDutyCycle(angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio));
+ }
+
+ public void setOptimize(boolean enable) {
+ optimizeStates = enable;
+ }
+
+ public byte getModuleIndex() {
+ return type.id;
+ }
+
+ public Rotation2d getAngle() {
+ return inputs.turnPosition;
+ }
+
+ public Rotation2d getCANcoder() {
+ return inputs.turnAbsolutePosition;
+ }
+
+ public void resetToAbsolute() {
+ // Sensor ticks
+ double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset);
+ angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio);
+ }
+
+ private void configCANcoder() {
+ CANcoder.getConfigurator().apply(new CANcoderConfiguration());
+ CANcoder.getConfigurator()
+ .apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).withSensorDirection(
+ DriveConstants.MODULE_CONSTANTS.canCoderInvert ? SensorDirectionValue.Clockwise_Positive
+ : SensorDirectionValue.CounterClockwise_Positive));
+ }
+
+ private void configAngleMotor() {
+ angleMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(config);
+ angleMotor.getConfigurator().apply(new Slot0Configs()
+ .withKP(DriveConstants.MODULE_CONSTANTS.angleKP)
+ .withKI(DriveConstants.MODULE_CONSTANTS.angleKI)
+ .withKD(DriveConstants.MODULE_CONSTANTS.angleKD));
+ angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR));
+ angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE);
+ angleMotor.setPosition(0);
+
+ // optimize bus utilization for angle motor
+ angleMotor.optimizeBusUtilization();
+
+ resetToAbsolute();
+ }
+
+ /**
+ * @return Speed in RPM
+ */
+ public double getDriveVelocity() {
+ return inputs.driveVelocityRadPerSec * 60 / DriveConstants.MODULE_CONSTANTS.driveGearRatio / 2 / Math.PI;
+ }
+
+ public double getDriveVoltage() {
+ return inputs.driveAppliedVolts;
+ }
+
+ public double getDriveStatorCurrent() {
+ return inputs.driveCurrentAmps;
+ }
+
+ // I took the config things straight from this file
+ public void setNewCurrentLimit(double currentSteer, double currentDrive) {
+ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
+ // steer
+ steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
+ steerConfig.SupplyCurrentLimit = currentSteer;
+ steerConfig.SupplyCurrentLowerLimit = currentSteer;
+ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
+ angleMotor.getConfigurator().apply(steerConfig); // apply
+
+ // drive
+ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
+ driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveConfig.SupplyCurrentLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerLimit = currentDrive;
+ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ driveConfig.StatorCurrentLimit = currentDrive;
+ driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(driveConfig); // apply
+ }
+
+ private void configDriveMotor() {
+ var talonFXConfigs = new TalonFXConfiguration();
+ // set Motion Magic settings
+ var motionMagicConfigs = talonFXConfigs.MotionMagic;
+ motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL / DriveConstants.WHEEL_CIRCUMFERENCE
+ * DriveConstants.DRIVE_GEAR_RATIO;
+ var slot0Configs = talonFXConfigs.Slot0;
+ slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction
+ slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output
+ slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output
+ slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output
+ slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error
+ slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output
+ driveMotor.getConfigurator().apply(talonFXConfigs);
+ CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+ config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT;
+ config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
+ config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT;
+ config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
+ driveMotor.getConfigurator().apply(config);
+ driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR));
+ driveMotor.getConfigurator()
+ .apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP));
+ driveMotor.getConfigurator()
+ .apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP));
+ driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE);
+
+ // optimize bus utilization for drive motor
+ driveMotor.optimizeBusUtilization();
+
+ }
+
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(
+ inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS,
+ getAngle());
+ }
+
+ public SwerveModuleState getDesiredState() {
+ return desiredState;
+ }
+
+ public double getDriveVelocityError() {
+ return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond;
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public ModuleType getModuleType() {
+ return type;
+ }
+
+ public void setStateDeadband(boolean enabled) {
+ stateDeadband = enabled;
+ }
+
+ public double getDesiredVelocity() {
+ return getDesiredState().speedMetersPerSecond;
+ }
+
+ public Rotation2d getDesiredAngle() {
+ return getDesiredState().angle;
+ }
+
+ /** Returns the module positions received this cycle. */
+ public SwerveModulePosition[] getOdometryPositions() {
+ return odometryPositions;
+ }
+
+ /** Returns the timestamps of the samples received this cycle. */
+ public double[] getOdometryTimestamps() {
+ return inputs.odometryTimestamps;
+ }
+
+ /** returns the drive position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getDrivePositionSignal() {
+ return drivePosition;
+ }
+
+ /** returns the turn position status signal for time-synced odometry. */
+ public StatusSignal<Angle> getTurnPositionSignal() {
+ return turnPosition;
+ }
+
+ /**
+ * returns the turn absolute position status signal for time-synced odometry.
+ */
+ public StatusSignal<Angle> getTurnAbsolutePositionSignal() {
+ return turnAbsolutePosition;
+ }
+
+ public TalonFX[] getMotors() {
+ return new TalonFX[] { angleMotor, driveMotor };
+ }
++
++ public void setNewCurrentLimit(double steerCurrentStator, double steerCurrentSupply, double driveCurrentStator,
++ double driveCurrentSupply) {
++ CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs();
++ // steer
++ steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT;
++ steerConfig.SupplyCurrentLimit = steerCurrentSupply;
++ steerConfig.SupplyCurrentLowerLimit = steerCurrentSupply;
++ steerConfig.StatorCurrentLimit = steerCurrentStator;
++ steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION;
++ angleMotor.getConfigurator().apply(steerConfig); // apply
++
++ // drive
++ CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs();
++ driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
++ driveConfig.SupplyCurrentLimit = driveCurrentSupply;
++ driveConfig.SupplyCurrentLowerLimit = driveCurrentSupply;
++ driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION;
++ driveConfig.StatorCurrentLimit = driveCurrentStator;
++ driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT;
++ driveMotor.getConfigurator().apply(driveConfig); // apply
++ }
}
goalVelocityRadPerSec = 0.0;
}
+
double setpointRad = goalAngle.getRadians();
- // calculate shortest angular delta
+ // calculate shortest angular delta
double delta = setpointRad - lastRawSetpoint;
delta = MathUtil.angleModulus(delta);
-
+
// filter delta
double filteredDelta = setpointFilter.calculate(delta);
-
+
// apply filtered range
lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta);
lastRawSetpoint = setpointRad;
// Multiply goal velocity by kV
double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV;
- if (calibrating){
- motor.set(0.1);
- boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD;
+ if (calibrating) {
+ io.setMotorRaw(0.1);
+ boolean atZero = Math
+ .abs(inputs.motorCurrent) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD;
boolean calibrated = calibrateDebouncer.calculate(atZero);
- if (calibrated) {
- if (calibrated){
-- stopCalibrating();
-- }
- } else{
+ } else {
// Set control with feedforward
- motor.setControl(mmVoltageRequest
- .withPosition(motorGoalRotations)
- .withFeedForward(velocityCompensation)
- .withEnableFOC(true));
+ io.setMotorControl(mmVoltageRequest
+ .withPosition(motorGoalRotations)
+ .withFeedForward(velocityCompensation)
+ .withEnableFOC(true));
}
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue());
- Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
- Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()));
- }
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putBoolean("Hood Calibrated", !calibrating);
- SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0);
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Hood/Voltage", inputs.motorVoltage);
+ Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO);
+ Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()));
}
}
- public void calibrate() {
- public void calibrate(){
-- calibrating = true;
- setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT);
- setNewCurrentLimit(HoodConstants.CALIBRATING_CURRENT_LIMIT, HoodConstants.CALIBRATING_CURRENT_LIMIT);
-- }
--
- public void stopCalibrating() {
- io.setPositionRaw(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
- public void stopCalibrating(){
- motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
-- goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE));
- setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
- setNewCurrentLimit(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
-- calibrating = false;
-- }
-
- public void setNewCurrentLimit(double stator, double supply) {
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = stator;
- limitConfig.StatorCurrentLimitEnable = true;
- limitConfig.SupplyCurrentLimit = supply;
- limitConfig.SupplyCurrentLimitEnable = true;
- motor.getConfigurator().apply(limitConfig);
- }
-
- public double getSubsystemStatorCurrent() {
- return inputs.motorStatorCurrent;
- }
-
- public double getSubsystemSupplyCurrent() {
- return inputs.motorSupplyCurrent;
- }
--
- @Override
- public void updateInputs() {
- inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
- inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
- inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
- inputs.motorSupplyCurrent = motor.getStatorCurrent().getValueAsDouble();
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limitAmps the current limit for stator and supply current
+ */
- public void setCurrentLimits(double limitAmps) {
- io.setCurrentLimits(limitAmps);
++ public void setCurrentLimits(double stator, double supply) {
++ io.setCurrentLimits(stator, supply);
}
}
import org.littletonrobotics.junction.AutoLog;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+
public interface HoodIO {
-- @AutoLog
-- public static class HoodIOInputs{
-- public double positionDeg = HoodConstants.MAX_ANGLE;
-- public double velocityRadPerSec = 0.0;
- public double motorCurrent = 0.0;
- public double motorVoltage = 0.0;
- public double motorStatorCurrent = 0.0;
- public double motorSupplyCurrent = 0.0;
-- }
-
- public void updateInputs();
++ @AutoLog
++ public static class HoodIOInputs {
++ public double positionDeg = HoodConstants.MAX_ANGLE;
++ public double velocityRadPerSec = 0.0;
++ public double motorCurrent = 0.0;
++ public double motorVoltage = 0.0;
++ }
+
- public void updateInputs(HoodIOInputs inputs);
++ public void updateInputs(HoodIOInputs inputs);
+
- public void setMotorRaw(double speed);
++ public void setMotorRaw(double speed);
+
- public void setMotorControl(MotionMagicVoltage control);
++ public void setMotorControl(MotionMagicVoltage control);
+
- public void setPositionRaw(double pos);
-
- /**
- * sets supply and stator current limits
- *
- * @param limitAmps the current limit for stator and supply current
- */
- void setCurrentLimits(double limitAmps);
++ public void setPositionRaw(double pos);
+
++ /**
++ * sets supply and stator current limits
++ *
++ * @param limitAmps the current limit for stator and supply current
++ */
++ public void setCurrentLimits(double stator, double supply);
}
-
--- /dev/null
- setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT);
+package frc.robot.subsystems.hood;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class HoodIOTalonFX implements HoodIO {
+ private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB);
+
+ public HoodIOTalonFX() {
+ motor.setNeutralMode(NeutralModeValue.Brake);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+ config.Slot0.kP = 2.0;
+ config.Slot0.kS = 0.1; // Static friction compensation
+ config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
+ config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot
+
+ var mm = config.MotionMagic;
+ mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO;
+ mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION)
+ * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety
+ mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
+ motor.getConfigurator().apply(config);
+
- public void setCurrentLimits(double limitAmps) {
++ setCurrentLimits(HoodConstants.STATOR_CURRENT_LIMIT, HoodConstants.SUPPLY_CURRENT_LIMIT);
+
+ motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO);
+ }
+
+ @Override
+ public void updateInputs(HoodIOInputs inputs) {
+ inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble())
+ / HoodConstants.HOOD_GEAR_RATIO;
+ inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble())
+ / HoodConstants.HOOD_GEAR_RATIO;
+ inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
+ inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
+ }
+
+ @Override
+ public void setMotorRaw(double speed) {
+ motor.set(speed);
+
+ }
+
+ @Override
+ public void setMotorControl(MotionMagicVoltage control) {
+ motor.setControl(control);
+ }
+
+ @Override
+ public void setPositionRaw(double pos) {
+ motor.setPosition(pos);
+ }
+
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limitAmps the current limit for stator and supply current
+ */
+ @Override
- .withStatorCurrentLimit(limitAmps)
++ public void setCurrentLimits(double stator, double supply) {
+ CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
+ .withStatorCurrentLimitEnable(true)
- .withSupplyCurrentLimit(limitAmps);
++ .withStatorCurrentLimit(stator)
+ .withSupplyCurrentLimitEnable(true)
++ .withSupplyCurrentLimit(supply);
+
+ motor.getConfigurator().apply(limits);
+ }
+}
import org.littletonrobotics.junction.AutoLog;
public interface ShooterIO {
- @AutoLog
- public static class ShooterIOInputs {
- public double shooterSpeedLeft = 0.0;
- public double shooterSpeedRight = 0.0;
- public double shooterStatorCurrentLeft = 0.0;
- public double shooterStatorCurrentRight = 0.0;
- public double shooterSupplyCurrentLeft = 0.0;
- public double shooterSupplyCurrentRight = 0.0;
- }
+ @AutoLog
+ public static class ShooterIOInputs {
+ public double shooterSpeedLeft = 0.0;
+ public double shooterSpeedRight = 0.0;
- public double shooterCurrentLeft = 0.0;
- public double shooterCurrentRight = 0.0;
++ public double shooterStatorCurrentLeft = 0.0;
++ public double shooterStatorCurrentRight = 0.0;
++ public double shooterSupplyCurrentLeft = 0.0;
++ public double shooterSupplyCurrentRight = 0.0;
+ }
- public void updateInputs();
+ public void updateInputs(ShooterIOInputs inputs);
+
+ public void setNewCurrentLimit(double newCurrentLimit);
+
+ public void setTargetVelocityRps(double target);
}
--- /dev/null
- inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
- inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class ShooterIOTalonFX implements ShooterIO {
+
+ private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB);
+ private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB);
+
+ // TODO Add current limits
+
+ // Velocity in rotations per second
+ VelocityVoltage voltageRequest = new VelocityVoltage(0);
+
+ public ShooterIOTalonFX() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Slot0.kP = 0.5; // 0.5 stable
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0.0;
+ config.Slot0.kV = 0.125; // Maximum rps = 100 --> 12V/100rps
+
+ config.CurrentLimits
+ .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT)
+ .withSupplyCurrentLimitEnable(true);
+
+ shooterMotorLeft.getConfigurator().apply(config);
+ shooterMotorRight.getConfigurator().apply(config);
+
+ shooterMotorLeft.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast));
+
+ shooterMotorRight.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
+
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+ }
+
+ @Override
+ public void updateInputs(ShooterIOInputs inputs) {
+ inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble())
+ * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2;
+ inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())
+ * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2;
++ inputs.shooterStatorCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
++ inputs.shooterStatorCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
++ inputs.shooterSupplyCurrentLeft = shooterMotorLeft.getSupplyCurrent().getValueAsDouble();
++ inputs.shooterSupplyCurrentRight = shooterMotorRight.getSupplyCurrent().getValueAsDouble();
+
+ }
+
+ @Override
+ public void setNewCurrentLimit(double newCurrentLimit) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = newCurrentLimit;
+ limitConfig.StatorCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+ }
+
+ @Override
+ public void setTargetVelocityRps(double target) {
+ shooterMotorLeft.setControl(voltageRequest.withVelocity(target).withEnableFOC(true));
+ shooterMotorRight.setControl(voltageRequest.withVelocity(target).withEnableFOC(true));
+ }
+}
@Override
public void periodic() {
- updateInputs();
+ io.updateInputs(inputs);
Logger.processInputs("Spindexer", inputs);
+ if (resetPos == null) {
+ io.setPositionRaw(0.1 * gearRatio);
+ resetPos = (inputs.spindexerOneVelocity / gearRatio) % 1.0;
+ resetPID.reset();
+ }
+
if (state == SpindexerState.MAX) {
- setMotorVoltages(SpindexerConstants.spindexerForwardVoltage);
+ io.setControl(new DutyCycleOut(SpindexerConstants.spindexerForwardVoltage / 12.0).withEnableFOC(true));
} else if (state == SpindexerState.REVERSE) {
- 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));
+ } else if (state == SpindexerState.RESET && resetPos != null) {
+ io.setControl(new DutyCycleOut(resetPID.calculate((inputs.spindexerOneVelocity / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
} else {
- setMotorVoltages(power);
+ io.setControl(new DutyCycleOut(power).withEnableFOC(true));
}
- setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_REVERSE_STATOR_LIMIT);
+ if (state == SpindexerState.REVERSE) {
- setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT, SpindexerConstants.CURRENT_FORWARD_STATOR_LIMIT);
++ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT);
+ } else {
++ setNewCurrentLimit(SpindexerConstants.SUPPLY_CURRENT_LIMIT);
+ }
+ // 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);
}
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 void resetSpindexer() {
+ state = SpindexerState.RESET;
}
- public double getSubsystemStatorCurrent() {
+ public void resetResetAngle() {
+ resetPos = null;
+ }
+
+ public double getStatorCurrent() {
- return inputs.spindexerOneCurrent + inputs.spindexerTwoCurrent;
+ return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent;
+ }
+
- public double getSubsystemSupplyCurrent() {
- return inputs.spindexerOneSupplyCurrent + inputs.spindexerTwoSupplyCurrent;
++ public double getSubsystemStatorCurrent() {
++ return inputs.spindexerOneStatorCurrent + inputs.spindexerTwoStatorCurrent;
}
- @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();
+ public void setNewCurrentLimit(double newCurrentLimit) {
+ io.setNewCurrentLimit(newCurrentLimit);
}
+
+
+ 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
+
+
+
}
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.0;
+
+ public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0;
+ public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0;
public static final double CURRENT_TIME_LIMIT = 1.0; //s
public static final double JAM_CURRENT_THRESHOLD = 75.0; // A
public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds
@AutoLog
public static class SpindexerIOInputs {
public double spindexerOneVelocity = 0.0;
- public double spindexerOneCurrent = 0.0;
+ public double spindexerOneSupplyCurrent = 0.0;
+ public double spindexerOneStatorCurrent = 0.0;
public double spindexerTwoVelocity = 0.0;
- public double spindexerTwoCurrent = 0.0;
+ public double spindexerTwoStatorCurrent = 0.0;
+ public double spindexerTwoSupplyCurrent = 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);
}
--- /dev/null
- limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit;
+package frc.robot.subsystems.spindexer;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class SpindexerIOTalonFX 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);
+
+ public SpindexerIOTalonFX() {
+ // configure current limit
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
- inputs.spindexerOneCurrent = motorOne.getStatorCurrent().getValueAsDouble();
++ limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.SUPPLY_CURRENT_LIMIT;
+ limitConfig.SupplyCurrentLowerTime = 1.5;
+ motorOne.getConfigurator().apply(limitConfig);
+ motorTwo.getConfigurator().apply(limitConfig);
+
+ // Invert motor two so they spin in opposite directions
+ MotorOutputConfigs motorConfig = new MotorOutputConfigs();
+ motorConfig.Inverted = InvertedValue.Clockwise_Positive;
+ motorTwo.getConfigurator().apply(motorConfig);
+ }
+
+ @Override
+ public void updateInputs(SpindexerIOInputs inputs) {
+ inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble();
- inputs.spindexerTwoCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
++ inputs.spindexerOneStatorCurrent = motorOne.getStatorCurrent().getValueAsDouble();
+ inputs.spindexerTwoVelocity = motorTwo.getVelocity().getValueAsDouble();
++ inputs.spindexerTwoStatorCurrent = motorTwo.getStatorCurrent().getValueAsDouble();
+ }
+
+ @Override
+ public void setNewCurrentLimit(double newCurrentLimit) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = newCurrentLimit;
+ limitConfig.StatorCurrentLimitEnable = true;
+ limitConfig.SupplyCurrentLowerLimit = newCurrentLimit;
+ limitConfig.SupplyCurrentLowerTime = 1.5;
+ motorOne.getConfigurator().apply(limitConfig);
+ motorTwo.getConfigurator().apply(limitConfig);
+ }
+
+ @Override
+ public void setControl(ControlRequest request) {
+ motorOne.setControl(request);
+ motorTwo.setControl(request);
+ }
+
+ @Override
+ public void setPositionRaw(double pos) {
+ motorOne.setPosition(pos);
+ motorTwo.setPosition(pos);
+ }
+}
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
- import frc.robot.util.ModifiedCRT;
-import frc.robot.constants.IdConstants;
-public class Turret extends SubsystemBase implements TurretIO{
+public class Turret extends SubsystemBase {
// Super low magnitude filter for the position to make it less jittery
private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);
- private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
+ private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
- public boolean locked = false;
+ private TurretIO io;
+
+ public boolean locked = false;
private boolean calibrating;
- private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
/* ---------------- Hardware ---------------- */
private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
- ModifiedCRT crt;
-
/* ---------------- Constructor ---------------- */
- public Turret() {
- motor.setNeutralMode(NeutralModeValue.Brake);
-
- TalonFXConfiguration config = new TalonFXConfiguration();
- config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-
- config.Slot0.kP = 1.5;
- config.Slot0.kS = 0.0; // Static friction compensation
- config.Slot0.kV = 0.0; // Adjusted kV for the gear ratio
- config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot
- config.Slot0.kA = 0.0;
-
- var mm = config.MotionMagic;
- mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO;
- mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) * TurretConstants.GEAR_RATIO; // Lowered for belt safety
- mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
- motor.getConfigurator().apply(config);
-
- setNewCurrentLimit(TurretConstants.STATOR_CURRENT_LIMIT, TurretConstants.SUPPLY_CURRENT_LIMIT);
+ public Turret(TurretIO io) {
+ this.io = io;
- setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
-
lastGoalRad = 0.0;
- if (RobotBase.isSimulation()) {
- simState = motor.getSimState();
- turretSim = new SingleJointedArmSim(
- edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1),
- TurretConstants.GEAR_RATIO,
- 0.01,
- 0.15,
- Units.degreesToRadians(TurretConstants.MIN_ANGLE),
- Units.degreesToRadians(TurretConstants.MAX_ANGLE),
- false,
- 0.0);
- }
-
if (!Constants.DISABLE_SMART_DASHBOARD) {
SmartDashboard.putData("Turret Mech", mech);
- SmartDashboard.putData("Start turret calibration", new InstantCommand(() -> calibrate()));
- SmartDashboard.putData("Stop turret calibration", new InstantCommand(() -> stopCalibrating()));
- SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition()));
SendableChooser<InstantCommand> turretTestChooser = new SendableChooser<>();
- turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0)));
- turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0)));
- turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0)));
- turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0)));
- turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0)));
+ turretTestChooser.setDefaultOption("Turn to 0",
+ new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0)));
+ turretTestChooser.addOption("Turn to -90",
+ new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0)));
+ turretTestChooser.addOption("Turn to 90",
+ new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0)));
+ turretTestChooser.addOption("Turn to 200",
+ new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0)));
+ turretTestChooser.addOption("Turn to -200",
+ new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0)));
SmartDashboard.putData("Turret Test Positions", turretTestChooser);
}
// Multiply goal velocity by kV
double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO;
- if (calibrating) {
- io.setMotorRaw(0.05);
- boolean calibrated = Math
- .abs(inputs.motorCurrent) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD;
- if (calibrationDebouncer.calculate(calibrated)) {
- stopCalibrating();
- }
- } else {
- // Sets motor control with feedforward
- io.setControl(mmVoltageRequest
- .withPosition(motorGoalRotations)
- .withFeedForward(robotTurnCompensation)
- .withEnableFOC(true));
- }
+ // Sets motor control with feedforward
- motor.setControl(mmVoltageRequest
- .withPosition(motorGoalRotations)
- .withFeedForward(robotTurnCompensation)
- .withEnableFOC(true));
++ io.setControl(mmVoltageRequest
++ .withPosition(motorGoalRotations)
++ .withFeedForward(robotTurnCompensation)
++ .withEnableFOC(true));
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
+ if (!Constants.DISABLE_LOGGING) {
+ Logger.recordOutput("Turret/Voltage", inputs.motorVoltage);
Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees());
- }
+ }
// --- Visualization ---
ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * TurretConstants.GEAR_RATIO);
}
- @Override
- public void updateInputs() {
- inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
- inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
- inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
- inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble();
- inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limit the current limit for stator and supply current
+ */
+ public void setCurrentLimits(double limit) {
+ io.setCurrentLimits(limit);
}
- public void setNewCurrentLimit(double stator, double supply) {
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = stator;
- limitConfig.StatorCurrentLimitEnable = true;
- limitConfig.SupplyCurrentLimit = supply;
- limitConfig.SupplyCurrentLimitEnable = true;
- motor.getConfigurator().apply(limitConfig);
- }
-
- public double getSubsystemStatorCurrent() {
- return inputs.motorStatorCurrent;
- }
-
- public double getSubsystemSupplyCurrent() {
- return inputs.motorSupplyCurrent;
- }
+ // Also ignore this for now
+ private double wrapUnit(double value) {
+ value %= 1.0;
+ if (value < 0)
+ value += 1.0;
+ return value;
+ }
-
- private void calibrate() {
- setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
- calibrating = true;
- }
-
- private void stopCalibrating() {
- io.setMotorRaw(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO);
- setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT);
- calibrating = false;
- setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0);
- }
}
import org.littletonrobotics.junction.AutoLog;
+import com.ctre.phoenix6.controls.ControlRequest;
+
public interface TurretIO {
- @AutoLog
- public static class TurretIOInputs{
- public double positionDeg = 0;
- public double velocityRadPerSec = 0;
- public double motorStatorCurrent = 0;
- public double motorSupplyCurrent = 0;
- public double encoderLeftRot = 0;
- public double encoderRightRot = 0;
- public double motorVoltage = 0;
- }
+ @AutoLog
+ public static class TurretIOInputs {
+ public double positionDeg = 0;
+ public double velocityRadPerSec = 0;
- public double motorCurrent = 0;
++ public double motorStatorCurrent = 0;
++ public double motorSupplyCurrent = 0;
++ public double encoderLeftRot = 0;
++ public double encoderRightRot = 0;
+ public double motorVoltage = 0;
+ }
+
+ public void updateInputs(TurretIOInputs inputs);
+
+ public void setMotorRaw(double speed);
+
+ public void setControl(ControlRequest request);
- public void updateInputs();
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limit the current limit for stator and supply current
+ */
+ public void setCurrentLimits(double limit);
}
--- /dev/null
- inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
+package frc.robot.subsystems.turret;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class TurretIOTalonFX implements TurretIO {
+
+ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB);
+
+ public TurretIOTalonFX() {
+ motor.setNeutralMode(NeutralModeValue.Brake);
+
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+ config.Slot0.kP = 12.0;
+ config.Slot0.kS = 0.1; // Static friction compensation
+ config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio
+ config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot
+
++ CurrentLimitsConfigs limitsConfigs = new CurrentLimitsConfigs();
++ limitsConfigs.StatorCurrentLimit = TurretConstants.STATOR_CURRENT_LIMIT;
++ limitsConfigs.SupplyCurrentLimit = TurretConstants.SUPPLY_CURRENT_LIMIT;
++ motor.getConfigurator().apply(limitsConfigs);
++
+ var mm = config.MotionMagic;
+ mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO;
+ mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION)
+ * TurretConstants.GEAR_RATIO; // Lowered for belt safety
+ mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed
+ motor.getConfigurator().apply(config);
+
+ motor.setPosition(0.0);
+ }
+
+ @Override
+ public void updateInputs(TurretIOInputs inputs) {
+ inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
+ inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble())
+ / TurretConstants.GEAR_RATIO;
- if (limit == TurretConstants.NORMAL_CURRENT_LIMIT) {
- limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT;
++ inputs.motorStatorCurrent = motor.getStatorCurrent().getValueAsDouble();
++ inputs.motorSupplyCurrent = motor.getSupplyCurrent().getValueAsDouble();
+ inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
+
+ }
+
+ @Override
+ public void setMotorRaw(double speed) {
+ motor.set(speed);
+ }
+
+ @Override
+ public void setControl(ControlRequest request) {
+ motor.setControl(request);
+ }
+
+ /**
+ * sets supply and stator current limits
+ *
+ * @param limit the current limit for stator and supply current
+ */
+ @Override
+ public void setCurrentLimits(double limit) {
+ CurrentLimitsConfigs limits = new CurrentLimitsConfigs()
+ .withStatorCurrentLimitEnable(true)
+ .withStatorCurrentLimit(limit)
+ .withSupplyCurrentLimitEnable(true)
+ .withSupplyCurrentLimit(limit);
+
++ if (limit == TurretConstants.SUPPLY_CURRENT_LIMIT) {
++ limits.SupplyCurrentLowerLimit = TurretConstants.SUPPLY_CURRENT_LIMIT * .5;
+ limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second
+ }
+
+ motor.getConfigurator().apply(limits);
++
+ }
+
+}