From abffb5de108238cf26daaf4c9c0e7b9001374a05 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Mon, 16 Feb 2026 16:46:25 -0500 Subject: [PATCH 01/10] added button mappings that were agreed upon by sharon, added xbox controller --- src/main/java/frc/robot/RobotContainer.java | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d684ab9e..9c5bdb12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; @@ -73,6 +74,8 @@ public class RobotContainer { private final AutoChooser autoChooser = new AutoChooser(); // The robot's subsystems and commands are defined here... // private final TiltSubsystem tiltSubsystem; + private final CommandXboxController controller = + new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); private final ClimberSubsystem climberSubsystem; private final AnglerSubsystem anglerSubsystem; private final IntakeSubsystem intakeSubsystem; @@ -183,6 +186,17 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + controller.a().onTrue(new RunDeployer(intakeDeployer)); + controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); + controller.y().onTrue(new ClimberUp(climberSubsystem)); + controller.x().onTrue(new ClimberDown(climberSubsystem)); + controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); + controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); + controller.rightBumper().onTrue(new SpinShooter(shooterSubsystem, 1)); + controller.leftBumper().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) // .onTrue(new ExampleCommand(m_exampleSubsystem)); @@ -199,10 +213,10 @@ private void configureBindings() { new AimAngler( anglerSubsystem, () -> drivebase != null ? drivebase.getPose() : null, - shootState); - } + shootState);} + } - if (!Constants.TESTBED) { + {if (!Constants.TESTBED) { SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driveJoystick.getY() * -1, () -> driveJoystick.getX() * -1) From 5af41d85d277ebb1ac4204b04cd60ab991f82d52 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Mon, 16 Feb 2026 20:34:50 -0500 Subject: [PATCH 02/10] added command --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- .../robot/parallels/RunHopperAndFeeder.java | 19 +++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/parallels/RunHopperAndFeeder.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c5bdb12..8d3b882a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc.robot.constants.enums.DeploymentState; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.parallels.RunHopperAndFeeder; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -194,7 +195,7 @@ private void configureBindings() { controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); - controller.rightBumper().onTrue(new SpinShooter(shooterSubsystem, 1)); + steerJoystick.trigger().onTrue(new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); controller.leftBumper().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); // Schedule `ExampleCommand` when `exampleCondition` changes to `true` @@ -264,6 +265,9 @@ public void putShuffleboardCommands() { SmartDashboard.putNumber("angler/TargetAngle", 0); + SmartDashboard.putData("RunIntakeAndFeeder", + new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); + SmartDashboard.putData( "angler/Set Position", new InstantCommand(() -> anglerSubsystem.setPosition( diff --git a/src/main/java/frc/robot/parallels/RunHopperAndFeeder.java b/src/main/java/frc/robot/parallels/RunHopperAndFeeder.java new file mode 100644 index 00000000..66305fe2 --- /dev/null +++ b/src/main/java/frc/robot/parallels/RunHopperAndFeeder.java @@ -0,0 +1,19 @@ +package frc.robot.parallels; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; + +public class RunHopperAndFeeder extends ParallelCommandGroup{ + + public RunHopperAndFeeder(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem){ + + addCommands( + new SpinHopper(hopperSubsystem), + new SpinFeeder(feederSubsystem) + + ); + } +} From e8763acfc9d5f8014fc4b68d0ed9a9396b041f3c Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Mon, 16 Feb 2026 20:48:10 -0500 Subject: [PATCH 03/10] it builds --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 45990811..bb0cd7fe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,8 @@ import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; From df7861c7ce799e9c921935db6690915d3ee4ca37 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Mon, 16 Feb 2026 21:15:56 -0500 Subject: [PATCH 04/10] created sequenial commands for raising and lowing intake with 1 button --- src/main/java/frc/robot/RobotContainer.java | 103 ++++++++++-------- .../robot/sequences/IntakeDownSequence.java | 19 ++++ .../frc/robot/sequences/IntakeUpSequence.java | 16 +++ 3 files changed, 90 insertions(+), 48 deletions(-) create mode 100644 src/main/java/frc/robot/sequences/IntakeDownSequence.java create mode 100644 src/main/java/frc/robot/sequences/IntakeUpSequence.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb0cd7fe..107aea6c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,8 @@ import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.parallels.RunHopperAndFeeder; +import frc.robot.sequences.IntakeDownSequence; +import frc.robot.sequences.IntakeUpSequence; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -156,7 +158,8 @@ public RobotContainer() { feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); - intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createSimIo(robotVisualizer)); + intakeDeployer = new IntakeDeployerSubsystem( + IntakeDeployerSubsystem.createSimIo(robotVisualizer)); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) @@ -188,16 +191,16 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - controller.a().onTrue(new RunDeployer(intakeDeployer)); - controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); - controller.y().onTrue(new ClimberUp(climberSubsystem)); - controller.x().onTrue(new ClimberDown(climberSubsystem)); - controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); - controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); - controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); - controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); - steerJoystick.trigger().onTrue(new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); - controller.leftBumper().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); + controller.a().onTrue(new IntakeUpSequence(intakeDeployer)); + controller.b().onTrue(new IntakeDownSequence(intakeDeployer, intakeSubsystem)); + controller.y().onTrue(new ClimberUp(climberSubsystem)); + controller.x().onTrue(new ClimberDown(climberSubsystem)); + controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); + controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); + steerJoystick.trigger().onTrue(new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); + controller.leftBumper().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) @@ -215,19 +218,23 @@ private void configureBindings() { new AimAngler( anglerSubsystem, () -> drivebase != null ? drivebase.getPose() : null, - shootState);} - } - - {if (!Constants.TESTBED) { - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driveJoystick.getY() * -1, - () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) - .deadband(Constants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + shootState); + } + + { + if (!Constants.TESTBED) { + SwerveInputStream driveAngularVelocity = SwerveInputStream + .of(drivebase.getSwerveDrive(), + () -> driveJoystick.getY() * -1, + () -> driveJoystick.getX() * -1) + .withControllerRotationAxis(steerJoystick::getX) + .deadband(Constants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + Command driveFieldOrientedAnglularVelocity = drivebase + .driveFieldOriented(driveAngularVelocity); + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + } } } @@ -266,8 +273,8 @@ public void putShuffleboardCommands() { SmartDashboard.putNumber("angler/TargetAngle", 0); - SmartDashboard.putData("RunIntakeAndFeeder", - new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); + SmartDashboard.putData("RunIntakeAndFeeder", + new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); SmartDashboard.putData( "angler/Set Position", @@ -326,25 +333,25 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "intakedeployer/InitlizeDeployer", new InitalRunDeployment(intakeDeployer)); - SmartDashboard.putData( - "Spin Intake", - new SpinIntake(intakeSubsystem)); - - SmartDashboard.putData( - "Start Hopper", - new SpinHopper(hopperSubsystem)); - - SmartDashboard.putData( - "Climber Up", - new ClimberUp(climberSubsystem)); - - SmartDashboard.putData( - "Climber Down", - new ClimberDown(climberSubsystem)); - - SmartDashboard.putData( - "Spin Feeder", - new SpinFeeder(feederSubsystem)); + SmartDashboard.putData( + "Spin Intake", + new SpinIntake(intakeSubsystem)); + + SmartDashboard.putData( + "Start Hopper", + new SpinHopper(hopperSubsystem)); + + SmartDashboard.putData( + "Climber Up", + new ClimberUp(climberSubsystem)); + + SmartDashboard.putData( + "Climber Down", + new ClimberDown(climberSubsystem)); + + SmartDashboard.putData( + "Spin Feeder", + new SpinFeeder(feederSubsystem)); SmartDashboard.putData( "Spin Shooter", @@ -403,9 +410,9 @@ public Command getAutonomousCommand() { return autoChooser.getCommand(); } - public ClimberSubsystem getClimberSubsystem() { - return climberSubsystem; - } + public ClimberSubsystem getClimberSubsystem() { + return climberSubsystem; + } public RobotVisualizer getRobotVisualizer() { return robotVisualizer; diff --git a/src/main/java/frc/robot/sequences/IntakeDownSequence.java b/src/main/java/frc/robot/sequences/IntakeDownSequence.java new file mode 100644 index 00000000..2d0ac943 --- /dev/null +++ b/src/main/java/frc/robot/sequences/IntakeDownSequence.java @@ -0,0 +1,19 @@ +package frc.robot.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; + +public class IntakeDownSequence extends SequentialCommandGroup{ + public IntakeDownSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ + addCommands( + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.DOWN), + new InitalRunDeployment(intakeDeployerSubsystem), + new SpinIntake(intakeSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/sequences/IntakeUpSequence.java b/src/main/java/frc/robot/sequences/IntakeUpSequence.java new file mode 100644 index 00000000..3520e61b --- /dev/null +++ b/src/main/java/frc/robot/sequences/IntakeUpSequence.java @@ -0,0 +1,16 @@ +package frc.robot.sequences; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; + +public class IntakeUpSequence extends SequentialCommandGroup{ + public IntakeUpSequence(IntakeDeployerSubsystem intakeDeployerSubsystem){ + addCommands( + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new InitalRunDeployment(intakeDeployerSubsystem) + ); + } +} From 6240e18df2fc6aa7c72f3322f885145ac6765d1a Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Mon, 16 Feb 2026 21:40:18 -0500 Subject: [PATCH 05/10] adjusted bindings for driver joysticks+press/release to shoot --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/constants/GameConstants.java | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 107aea6c..cea23bcd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -199,8 +199,9 @@ private void configureBindings() { controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); - steerJoystick.trigger().onTrue(new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); - controller.leftBumper().onTrue(new SetShootingState(shootState, ShootState.STOPPED)); + steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); + driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e4f6773c..74f889de 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -61,9 +61,9 @@ public enum Mode { //Timeouts public static final double SPIN_TIMEOUT = 5; public static final double TILT_TIMEOUT = 5; - public static final double HOPPER_TIMEOUT = 10; + public static final double HOPPER_TIMEOUT = 60; public static final double CLIMBER_TIMEOUT = 10; - public static final double FEEDER_TIMEOUT = 3; + public static final double FEEDER_TIMEOUT = 60; public static final double ANGLER_TIMEOUT = 5; public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; public static final double SHOOTER_TIMEOUT = 5; From d21131b30c0c23d52f326989522aa89c2fa164ec Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Tue, 17 Feb 2026 12:46:34 -0500 Subject: [PATCH 06/10] made changes to adjust to testbed testing: intakeworked odd, but should be fine after isDeployed is removed --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../robot/commands/intakeDeployment/InitalRunDeployment.java | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cea23bcd..ce16eae7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -196,9 +196,9 @@ private void configureBindings() { controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); controller.povUp().onTrue(new SetShootingState(shootState, ShootState.FIXED)); - controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); + controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); - controller.povRight().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); + controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java index 0d72ff69..1dca9645 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java @@ -38,6 +38,7 @@ public void execute() { @Override public void end(boolean interrupted) { + subsystem.stopMotors(); } @Override From 195d63de5cda4c0306c7797a6d04b32b859f2bb0 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Tue, 17 Feb 2026 14:26:23 -0500 Subject: [PATCH 07/10] fixed an error from main that caused simulation to crash --- src/main/java/frc/robot/constants/Constants2026.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/Constants2026.java b/src/main/java/frc/robot/constants/Constants2026.java index f82155cb..70d5e7e0 100644 --- a/src/main/java/frc/robot/constants/Constants2026.java +++ b/src/main/java/frc/robot/constants/Constants2026.java @@ -15,7 +15,7 @@ public class Constants2026 extends GameConstants { public static final int SHOOTER_MOTOR_ID = 54; public static final int SHOOTER_FOLLOWER_MOTOR_ID = 53; public static final int INTAKE_DEPLOYMENT_ID = 9; - public static final int TURRET_MOTOR_ID = 9; + public static final int TURRET_MOTOR_ID = 11; public static final double DRIVE_BASE_WIDTH = 0.635; public static final double DRIVE_BASE_LENGTH = 0.635; From 56d9e9d1649e6ec21708db18ad092cc82449841e Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 20 Feb 2026 14:09:17 -0500 Subject: [PATCH 08/10] made changes to meet pr comments --- src/main/java/frc/robot/RobotContainer.java | 10 +++--- .../frc/robot/commands/intake/StopIntake.java | 31 +++++++++++++++++++ .../intakeDeployment/InitalRunDeployment.java | 1 - .../parallels/DeployAndSpinIntake.java | 19 ++++++++++++ .../parallels/RunHopperAndFeeder.java | 8 ++--- .../sequences/IntakeDownSequence.java | 12 +++---- .../commands/sequences/IntakeUpSequence.java | 18 +++++++++++ .../frc/robot/sequences/IntakeUpSequence.java | 16 ---------- 8 files changed, 83 insertions(+), 32 deletions(-) create mode 100644 src/main/java/frc/robot/commands/intake/StopIntake.java create mode 100644 src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java rename src/main/java/frc/robot/{ => commands}/parallels/RunHopperAndFeeder.java (66%) rename src/main/java/frc/robot/{ => commands}/sequences/IntakeDownSequence.java (61%) create mode 100644 src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java delete mode 100644 src/main/java/frc/robot/sequences/IntakeUpSequence.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e3400da4..e26adbbf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,9 @@ import frc.robot.commands.intakeDeployment.InitalRunDeployment; import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.parallels.RunHopperAndFeeder; +import frc.robot.commands.sequences.IntakeDownSequence; +import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.angler.RunAnglerToReverseLimit; @@ -38,9 +41,6 @@ import frc.robot.constants.enums.DeploymentState; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.parallels.RunHopperAndFeeder; -import frc.robot.sequences.IntakeDownSequence; -import frc.robot.sequences.IntakeUpSequence; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -201,7 +201,7 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { - controller.a().onTrue(new IntakeUpSequence(intakeDeployer)); + controller.a().onTrue(new IntakeUpSequence(intakeDeployer, intakeSubsystem)); controller.b().onTrue(new IntakeDownSequence(intakeDeployer, intakeSubsystem)); controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); @@ -284,7 +284,7 @@ public void putShuffleboardCommands() { SmartDashboard.putNumber("angler/TargetAngle", 0); - SmartDashboard.putData("RunIntakeAndFeeder", + SmartDashboard.putData("RunHoppperAndFeeder", new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); SmartDashboard.putData( diff --git a/src/main/java/frc/robot/commands/intake/StopIntake.java b/src/main/java/frc/robot/commands/intake/StopIntake.java new file mode 100644 index 00000000..c9f4df29 --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/StopIntake.java @@ -0,0 +1,31 @@ +package frc.robot.commands.intake; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class StopIntake extends LoggableCommand { + + private final IntakeSubsystem subsystem; + + public StopIntake(IntakeSubsystem subsystem) { + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java index 1dca9645..0d72ff69 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java @@ -38,7 +38,6 @@ public void execute() { @Override public void end(boolean interrupted) { - subsystem.stopMotors(); } @Override diff --git a/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java new file mode 100644 index 00000000..60ad776f --- /dev/null +++ b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java @@ -0,0 +1,19 @@ +package frc.robot.commands.parallels; + +import frc.robot.commands.intake.SpinIntake; +import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; + +public class DeployAndSpinIntake extends LoggableParallelCommandGroup{ + + public DeployAndSpinIntake(IntakeDeployerSubsystem deployer, IntakeSubsystem intakeSubsystem){ + + super( + new InitalRunDeployment(deployer), + new SpinIntake(intakeSubsystem) + + ); + } +} diff --git a/src/main/java/frc/robot/parallels/RunHopperAndFeeder.java b/src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java similarity index 66% rename from src/main/java/frc/robot/parallels/RunHopperAndFeeder.java rename to src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java index 66305fe2..40ea0865 100644 --- a/src/main/java/frc/robot/parallels/RunHopperAndFeeder.java +++ b/src/main/java/frc/robot/commands/parallels/RunHopperAndFeeder.java @@ -1,16 +1,16 @@ -package frc.robot.parallels; +package frc.robot.commands.parallels; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.hopper.SpinHopper; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; -public class RunHopperAndFeeder extends ParallelCommandGroup{ +public class RunHopperAndFeeder extends LoggableParallelCommandGroup{ public RunHopperAndFeeder(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem){ - addCommands( + super( new SpinHopper(hopperSubsystem), new SpinFeeder(feederSubsystem) diff --git a/src/main/java/frc/robot/sequences/IntakeDownSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java similarity index 61% rename from src/main/java/frc/robot/sequences/IntakeDownSequence.java rename to src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java index 2d0ac943..b038476c 100644 --- a/src/main/java/frc/robot/sequences/IntakeDownSequence.java +++ b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java @@ -1,19 +1,19 @@ -package frc.robot.sequences; +package frc.robot.commands.sequences; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.intakeDeployment.InitalRunDeployment; import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.commands.parallels.DeployAndSpinIntake; import frc.robot.constants.enums.DeploymentState; import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; -public class IntakeDownSequence extends SequentialCommandGroup{ +public class IntakeDownSequence extends LoggableSequentialCommandGroup{ public IntakeDownSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ - addCommands( + super( new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.DOWN), - new InitalRunDeployment(intakeDeployerSubsystem), - new SpinIntake(intakeSubsystem) + new DeployAndSpinIntake(intakeDeployerSubsystem, intakeSubsystem) ); } } diff --git a/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java new file mode 100644 index 00000000..7caaaa00 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java @@ -0,0 +1,18 @@ +package frc.robot.commands.sequences; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.SetDeploymentState; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; +import frc.robot.subsystems.IntakeSubsystem; + +public class IntakeUpSequence extends LoggableSequentialCommandGroup{ + public IntakeUpSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ + super( + new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), + new InitalRunDeployment(intakeDeployerSubsystem), + new StopIntake(intakeSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/sequences/IntakeUpSequence.java b/src/main/java/frc/robot/sequences/IntakeUpSequence.java deleted file mode 100644 index 3520e61b..00000000 --- a/src/main/java/frc/robot/sequences/IntakeUpSequence.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.sequences; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; -import frc.robot.commands.intakeDeployment.SetDeploymentState; -import frc.robot.constants.enums.DeploymentState; -import frc.robot.subsystems.IntakeDeployerSubsystem; - -public class IntakeUpSequence extends SequentialCommandGroup{ - public IntakeUpSequence(IntakeDeployerSubsystem intakeDeployerSubsystem){ - addCommands( - new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), - new InitalRunDeployment(intakeDeployerSubsystem) - ); - } -} From 08f6d894fff91cb15549b3479ff7f6bcb57e8dbc Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 20 Feb 2026 18:51:11 -0500 Subject: [PATCH 09/10] fixed merge issues --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 991a9060..de5cbb52 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,6 @@ public class RobotContainer { private final ShooterSubsystem shooterSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; - private final ClimberSubsystem climberSubsystem; private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; From a1d8cdd269e664408529d748cf8ccebaf844ad98 Mon Sep 17 00:00:00 2001 From: Roshan Thadani Date: Fri, 20 Feb 2026 19:11:18 -0500 Subject: [PATCH 10/10] made more adjustments based on comments/fixed inital to initial --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../{InitalRunDeployment.java => InitialRunDeployment.java} | 4 ++-- .../frc/robot/commands/parallels/DeployAndSpinIntake.java | 4 ++-- .../frc/robot/commands/sequences/IntakeDownSequence.java | 2 +- .../java/frc/robot/commands/sequences/IntakeUpSequence.java | 6 +++--- 5 files changed, 10 insertions(+), 10 deletions(-) rename src/main/java/frc/robot/commands/intakeDeployment/{InitalRunDeployment.java => InitialRunDeployment.java} (90%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de5cbb52..8bf97c7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,7 +22,7 @@ import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.RunHopperAndFeeder; @@ -419,7 +419,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "intakedeployer/InitlizeDeployer", - new InitalRunDeployment(intakeDeployer)); + new InitialRunDeployment(intakeDeployer)); SmartDashboard.putData( "Spin Intake", new SpinIntake(intakeSubsystem)); diff --git a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java similarity index 90% rename from src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java rename to src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java index 0d72ff69..7de4143f 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/InitalRunDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/InitialRunDeployment.java @@ -11,11 +11,11 @@ /** * When creating sequences use this AFTER setting the deployment state */ -public class InitalRunDeployment extends LoggableCommand { +public class InitialRunDeployment extends LoggableCommand { private final IntakeDeployerSubsystem subsystem; private final Timer timer; - public InitalRunDeployment(IntakeDeployerSubsystem subsystem) { + public InitialRunDeployment(IntakeDeployerSubsystem subsystem) { timer = new Timer(); this.subsystem = subsystem; addRequirements(subsystem); diff --git a/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java index 60ad776f..f515e6a9 100644 --- a/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java +++ b/src/main/java/frc/robot/commands/parallels/DeployAndSpinIntake.java @@ -1,7 +1,7 @@ package frc.robot.commands.parallels; import frc.robot.commands.intake.SpinIntake; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; @@ -11,7 +11,7 @@ public class DeployAndSpinIntake extends LoggableParallelCommandGroup{ public DeployAndSpinIntake(IntakeDeployerSubsystem deployer, IntakeSubsystem intakeSubsystem){ super( - new InitalRunDeployment(deployer), + new InitialRunDeployment(deployer), new SpinIntake(intakeSubsystem) ); diff --git a/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java index b038476c..63351995 100644 --- a/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java +++ b/src/main/java/frc/robot/commands/sequences/IntakeDownSequence.java @@ -1,7 +1,7 @@ package frc.robot.commands.sequences; import frc.robot.commands.intake.SpinIntake; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.parallels.DeployAndSpinIntake; import frc.robot.constants.enums.DeploymentState; diff --git a/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java index 7caaaa00..bfaa673e 100644 --- a/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java +++ b/src/main/java/frc/robot/commands/sequences/IntakeUpSequence.java @@ -1,7 +1,7 @@ package frc.robot.commands.sequences; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; import frc.robot.commands.intake.StopIntake; -import frc.robot.commands.intakeDeployment.InitalRunDeployment; +import frc.robot.commands.intakeDeployment.InitialRunDeployment; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.constants.enums.DeploymentState; import frc.robot.subsystems.IntakeDeployerSubsystem; @@ -10,9 +10,9 @@ public class IntakeUpSequence extends LoggableSequentialCommandGroup{ public IntakeUpSequence(IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem){ super( + new StopIntake(intakeSubsystem), new SetDeploymentState(intakeDeployerSubsystem, DeploymentState.UP), - new InitalRunDeployment(intakeDeployerSubsystem), - new StopIntake(intakeSubsystem) + new InitialRunDeployment(intakeDeployerSubsystem) ); } }