import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.commands.gpm.BrownOutControl;
-import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.LockedShoot;
import frc.robot.commands.gpm.RunSpindexer;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.Intake.IntakeIO;
import frc.robot.subsystems.Intake.IntakeIOTalonFX;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.hood.HoodIOTalonFX;
import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterIO;
+import frc.robot.subsystems.shooter.ShooterIOTalonFX;
import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.spindexer.SpindexerIOTalonFX;
import frc.robot.subsystems.turret.Turret;
import frc.robot.util.PathGroupLoader;
import frc.robot.util.Vision.DetectedObject;
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
- private LinearClimb linearClimb = null;
private LED led = null;
// TODO: move to correct robot and put the correct port?
default:
case PrimeJr: // AKA Valence
- spindexer = new Spindexer();
+ spindexer = new Spindexer(new SpindexerIOTalonFX());
intake = new Intake(new IntakeIOTalonFX());
led = new LED();
case WaffleHouse: // AKA Betabot
turret = new Turret();
- shooter = new Shooter();
- hood = new Hood();
+ shooter = new Shooter(new ShooterIOTalonFX());
+ hood = new Hood(new HoodIOTalonFX());
case TwinBot:
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb);
+ driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
operator = new Operator(drive);
// Detected objects need access to the drivetrain
}));
}
- if (linearClimb != null && drive != null) {
- NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive));
- }
}
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.Climb.LinearClimb;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-public class ClimbDriveCommand extends SequentialCommandGroup{
-
- public ClimbDriveCommand(LinearClimb climb, Drivetrain drive){
- addCommands(
- new ParallelCommandGroup(
- new InstantCommand(() -> climb.climbPosition()),
- new DriveToPose(drive, () -> FieldConstants.getClimbLocation())
- )
- );
- }
-
-
-}
import frc.robot.commands.gpm.Superstructure;
import frc.robot.constants.Constants;
import frc.robot.constants.swerve.DriveConstants;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.hood.Hood;
* 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 LinearClimb climb;
-
- public PS5ControllerDriverConfig(
+ 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(
Drivetrain drive,
Shooter shooter,
Turret turret,
Hood hood,
Intake intake,
- Spindexer spindexer,
- LinearClimb climb) {
+ Spindexer spindexer
+ ) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.hood = hood;
this.intake = intake;
this.spindexer = spindexer;
- this.climb = climb;
}
- 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;
- }
- }));
- }
-
- // Spindexer
- if (spindexer != null && turret != null && hood != null && intake != null) {
-
- // 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);
- }));
- }
-
- // Hood
- if (spindexer != null) {
- // Set the hood down -- for safety measures under trench
- controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
- .onFalse(new InstantCommand(()-> shootFocus(false)));
- }
+ 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));
}
- 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
- );
+ // Intake
+ if (intake != null) {
+ // Toggle intake
+ controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
+ intake.extend();
+ intake.spinStart();
+ intakeBoolean = false;
} 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);
+ 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;
+ }
+ }));
}
- @Override
- public double getRawSideTranslation() {
- return controller.get(PS5Axis.LEFT_X);
- }
-
- @Override
- public double getRawForwardTranslation() {
- return controller.get(PS5Axis.LEFT_Y);
- }
+ // Spindexer
+ if (spindexer != null && turret != null && hood != null && intake != null) {
- @Override
- public double getRawRotation() {
- return controller.get(PS5Axis.RIGHT_X);
+ // Toggle spindexer
+ controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
+ new RunSpindexer(spindexer, turret, hood, intake));
}
- @Override
- public double getRawHeadingAngle() {
- return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2;
+ // 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);
}
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y));
+ // 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 boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
+ // Hood
+ if (spindexer != null) {
+ // Set the hood down -- for safety measures under trench
+ controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true)))
+ .onFalse(new InstantCommand(() -> shootFocus(false)));
}
-
- @Override
- public boolean getIsAlign() {
- return false;
+ }
+
+ 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);
}
+ }
+
+ @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;
+ }
}
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Robot;
import frc.robot.commands.gpm.AutoShootCommand;
-import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.ReverseMotors;
import frc.robot.constants.Constants;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.Intake.Intake;
*/
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;
- private LinearClimb climb;
-
- // 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 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,
- LinearClimb climb) {
- super(drive);
- this.shooter = shooter;
- this.turret = turret;
- this.hood = hood;
- this.intake = intake;
- this.spindexer = spindexer;
- this.climb = climb;
- }
-
- 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();
+ 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 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();
+ }));
+
+ // 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();
- }
- }));
- }
-
- // 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();
- }));
- }
-
- // 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);
- }
- }));
+ // 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();
}
-
- // Climb
- if (climb != null) {
- // Calibration
- controller.get(OPTIONS).onTrue(new InstantCommand(() -> {
- climb.hardstopCalibration();
- })).onFalse(new InstantCommand(() -> {
- climb.stopCalibrating();
- }));
-
- // Climb retract
- controller.get(CROSS).onTrue(new InstantCommand(() -> {
- climb.retract();
- }));
-
- // Drive to climb position and rumble
- controller.get(TRIANGLE).onTrue(new SequentialCommandGroup(
- new ClimbDriveCommand(climb, getDrivetrain()),
- new InstantCommand(() -> this.startRumble()),
- new WaitCommand(1),
- new InstantCommand(() -> this.endRumble())));
- }
-
- // 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;
+ // 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();
+ }));
}
- @Override
- public double getRawHeadingMagnitude() {
- return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y));
+ // 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()));
}
- @Override
- public boolean getIsSlowMode() {
- return slowModeSupplier.getAsBoolean();
+ // 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);
+ }
+ }));
}
- @Override
- public boolean getIsAlign() {
- return false;
- }
+ // Climb
- public void startRumble() {
- controller.setRumble(GameController.RumbleStatus.RUMBLE_ON);
+ // Hood
+ if (hood != null) {
+ controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+ hood.calibrate();
+ })).onFalse(new InstantCommand(() -> {
+ hood.stopCalibrating();
+ }));
}
- 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 com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
import frc.robot.constants.IdConstants;
-import sun.jvm.hotspot.utilities.Unsigned5.SetPosition;
public class HoodIOTalonFX implements HoodIO {
private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB);
return Math.abs(getShooterVelocity() - shooterTargetSpeed) < 1.0;
}
- public void setNewCurrentLimits(double limit) {
+ public void setNewCurrentLimit(double limit) {
io.setNewCurrentLimit(limit);
}
package frc.robot.subsystems.spindexer;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.hardware.TalonFX;
-
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-public class Spindexer extends SubsystemBase implements SpindexerIO {
- private TalonFX motor = new TalonFX(IdConstants.SPINDEXER_ID, Constants.CANIVORE_SUB);
+public class Spindexer extends SubsystemBase {
private double power = 0.0;
public int ballCount = 0;
public boolean noIndexing = false;
+ private SpindexerIO io;
- public Spindexer() {
- updateInputs();
-
- // configure current limit
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = SpindexerConstants.CURRENT_SPIKE_LIMIT;
- limitConfig.StatorCurrentLimitEnable = true;
- limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit;
- limitConfig.SupplyCurrentLowerTime = 1.5;
- motor.getConfigurator().apply(limitConfig);
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer()));
- SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer()));
- SmartDashboard.putData("Spindexer Stop", new InstantCommand(() -> stopSpindexer()));
- }
+ public Spindexer(SpindexerIO io) {
+ this.io = io;
+ io.updateInputs(inputs);
resetPID.setTolerance(0.05);
}
@Override
public void periodic() {
- updateInputs();
+ io.updateInputs(inputs);
Logger.processInputs("Spindexer", inputs);
if (resetPos == null) {
- motor.setPosition(0.1 * gearRatio);
- resetPos = (motor.getPosition().getValueAsDouble() / gearRatio) % 1.0;
+ io.setPositionRaw(0.1 * gearRatio);
+ resetPos = (inputs.spindexerPosition / gearRatio) % 1.0;
resetPID.reset();
}
if (state == SpindexerState.MAX) {
- motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true));
+ io.setControl(new DutyCycleOut(SpindexerConstants.spindexerMaxPower).withEnableFOC(true));
reversing = false;
} else if (state == SpindexerState.REVERSE) {
- motor.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true));
+ io.setControl(new DutyCycleOut(SpindexerConstants.spindexerReversePower).withEnableFOC(true));
reversing = true;
} else if (state == SpindexerState.STOPPED) {
- motor.setControl(new DutyCycleOut(0.0).withEnableFOC(true));
+ io.setControl(new DutyCycleOut(0.0).withEnableFOC(true));
reversing = false;
} else if (state == SpindexerState.RESET && resetPos != null) {
- motor.setControl(new DutyCycleOut(resetPID.calculate((motor.getPosition().getValueAsDouble() / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
+ io.setControl(new DutyCycleOut(resetPID.calculate((inputs.spindexerPosition / gearRatio) % 1.0, resetPos)).withEnableFOC(true));
} else {
- motor.setControl(new DutyCycleOut(power).withEnableFOC(true));
+ io.setControl(new DutyCycleOut(power).withEnableFOC(true));
reversing = false;
}
}
public void setNewCurrentLimit(double newCurrentLimit) {
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = newCurrentLimit;
- limitConfig.StatorCurrentLimitEnable = true;
- limitConfig.SupplyCurrentLowerLimit = newCurrentLimit;
- limitConfig.SupplyCurrentLowerTime = 1.5;
- motor.getConfigurator().apply(limitConfig);
+ io.setNewCurrentLimit(newCurrentLimit);
}
- @Override
- public void updateInputs() {
- inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;
- inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
- }
private Double resetPos;
private PIDController resetPID = new PIDController(4.0, 0.0, 0);
import org.littletonrobotics.junction.AutoLog;
+import com.ctre.phoenix6.controls.ControlRequest;
+
public interface SpindexerIO {
- @AutoLog
- public static class SpindexerIOInputs {
- public double spindexerVelocity = 0.0;
- public double spindexerCurrent = 0.0;
- }
+ @AutoLog
+ public static class SpindexerIOInputs {
+ public double spindexerVelocity = 0.0;
+ public double spindexerCurrent = 0.0;
+ public double spindexerPosition = 0.0;
+ }
+
+ public void updateInputs(SpindexerIOInputs inputs);
+
+ public void setControl(ControlRequest request);
+
+ public void setPositionRaw(double pos);
- public void updateInputs();
+ public void setNewCurrentLimit(double newCurrentLimit);
}