From 6e193cbd8937fb96110d51686f4c4fb0e4f62aa3 Mon Sep 17 00:00:00 2001 From: moo Date: Mon, 13 Apr 2026 10:55:28 -0700 Subject: [PATCH] spindexer rewrite for advantagekit, cleanup --- src/main/java/frc/robot/RobotContainer.java | 18 +- .../robot/commands/gpm/ClimbDriveCommand.java | 23 - .../controls/PS5ControllerDriverConfig.java | 291 +++++++------ .../controls/PS5XboxModeDriverConfig.java | 397 ++++++++---------- .../robot/subsystems/hood/HoodIOTalonFX.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/spindexer/Spindexer.java | 55 +-- .../subsystems/spindexer/SpindexerIO.java | 21 +- 8 files changed, 368 insertions(+), 441 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 721165c..52666e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,7 +25,6 @@ import frc.robot.commands.LogCommand; 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; @@ -37,7 +36,6 @@ import frc.robot.constants.VisionConstants; 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; @@ -45,8 +43,12 @@ import frc.robot.subsystems.LED.LED; 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; @@ -77,7 +79,6 @@ public class RobotContainer { // 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? @@ -124,14 +125,14 @@ public class RobotContainer { 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: @@ -147,7 +148,7 @@ public class RobotContainer { 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 @@ -267,9 +268,6 @@ public class RobotContainer { })); } - if (linearClimb != null && drive != null) { - NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive)); - } } diff --git a/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java deleted file mode 100644 index 6819c09..0000000 --- a/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -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()) - ) - ); - } - - -} diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index c42f596..c6e2ce6 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -14,7 +14,6 @@ import frc.robot.commands.gpm.RunSpindexer; 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; @@ -31,179 +30,175 @@ import lib.controllers.PS5Controller.PS5Button; * 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; + } } diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java index 3cdc02b..84c9293 100644 --- a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -12,10 +12,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 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; @@ -40,227 +38,204 @@ import lib.controllers.GameController.DPad; */ 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); + } } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java b/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java index 8619363..2027227 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java @@ -8,8 +8,8 @@ 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; -import sun.jvm.hotspot.utilities.Unsigned5.SetPosition; public class HoodIOTalonFX implements HoodIO { private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 917e118..1e34897 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -75,7 +75,7 @@ public class Shooter extends SubsystemBase { return Math.abs(getShooterVelocity() - shooterTargetSpeed) < 1.0; } - public void setNewCurrentLimits(double limit) { + public void setNewCurrentLimit(double limit) { io.setNewCurrentLimit(limit); } diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index e9cb913..ae572ad 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -1,20 +1,14 @@ 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; @@ -25,23 +19,12 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { 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); } @@ -56,28 +39,28 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { @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; } @@ -132,19 +115,9 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { } 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); diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java index 1cf6740..321cb1d 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerIO.java @@ -2,12 +2,21 @@ package frc.robot.subsystems.spindexer; 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); } -- 2.39.5