Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
b123f24
added stop all command + other things needed for it to function
RoRoBout212 Feb 21, 2026
cfe717b
made stop all sequential instead of parallel
RoRoBout212 Feb 21, 2026
75a453f
started creating encoders for the stop command, need to find if we ne…
RoRoBout212 Feb 21, 2026
9698c24
fixed encoder issues to allow for climber reset encoder command to work
RoRoBout212 Feb 28, 2026
b358474
started testing in simulation: need to find how to fix climber reset …
RoRoBout212 Feb 28, 2026
3e1de9d
removed encoder reset for climber
RoRoBout212 Feb 28, 2026
430742d
continued testing (committed to do a pull from main)
RoRoBout212 Feb 28, 2026
df42bfb
pulled from main
RoRoBout212 Feb 28, 2026
cdf59c1
finished testing in sim made adjustments based to reset all based on …
RoRoBout212 Feb 28, 2026
8fb2677
started testing as parallel
RoRoBout212 Mar 5, 2026
64d6afe
Merge branch 'main' of https://github.com/FRC4048/Java_2026 into RT-C…
RoRoBout212 Mar 7, 2026
3493523
finsihed testing in sim and finalized what reset all command will loo…
RoRoBout212 Mar 7, 2026
630c589
started testing on robot
RoRoBout212 Mar 7, 2026
4e7949a
Merge branch 'main' into RT-Cancel
Sahiltheram Mar 10, 2026
1dd39ba
added auto
Sahiltheram Mar 10, 2026
2128889
Testing reset all and fixing errors
Sahiltheram Mar 10, 2026
4638469
made changes based on comments
RoRoBout212 Mar 15, 2026
80052d7
Merge branch 'main' of https://github.com/FRC4048/Java_2026 into RT-C…
RoRoBout212 Mar 15, 2026
2b46d88
started to fix reset all with new intake deploying command. Need to f…
RoRoBout212 Mar 15, 2026
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 14 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,11 @@
import frc.robot.commands.lightStrip.SetLedFromShootingState;
import frc.robot.commands.parallels.RunHopperAndFeeder;
import frc.robot.commands.parallels.SetShootingStateAndLight;
import frc.robot.commands.sequences.ResetAll;
import frc.robot.autochooser.AutoChooser;
import frc.robot.commands.angler.AimAngler;
import frc.robot.commands.angler.DefaultAnglerControl;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.StowAngler;
import frc.robot.commands.shooter.DefaultShooterControl;
import frc.robot.commands.auto.ExampleAuto;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -299,6 +300,9 @@ private void configureBindings() {
controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB));
controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING));
driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem,
hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState)));

if (controllerSubsystem != null) {
steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem));
}
Expand Down Expand Up @@ -432,8 +436,16 @@ public void putShuffleboardCommands() {

SmartDashboard.putData(
"angler/Home Rev (Reset)",
new RunAnglerToReverseLimit(anglerSubsystem));
new StowAngler(anglerSubsystem));

SmartDashboard.putData("stop Intake",
new StopIntake(intakeSubsystem));

SmartDashboard.putData(
"reset All",
new ResetAll(anglerSubsystem, climberSubsystem, feederSubsystem,
hopperSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem,
turretSubsystem, shootState));
SmartDashboard.putData(
"turret/TurretTest/Turret Go 45",
new SetTurretAngle(turretSubsystem, 45));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
/**
* Runs the angler to the reverse limit switch and resets the encoder to zero.
*/
public class RunAnglerToReverseLimit extends LoggableCommand {
public class StowAngler extends LoggableCommand {
private final TimeoutLogger timeoutCounter;
private final AnglerSubsystem angler;
private final Timer timer = new Timer();

public RunAnglerToReverseLimit(AnglerSubsystem angler) {
public StowAngler(AnglerSubsystem angler) {
timeoutCounter = new TimeoutLogger(getName());
this.angler = angler;
addRequirements(angler);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueDepotShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is incorrect, it has to be stowAngler. the same is true for all the autonomous commands. I'm not sure why this was even changed in this PR, it shouldn't be part of it. This will create conflicts to Michael who is working on the autonomous commands.
I think it would be best to not rename back StowAngler to RunAnglerToReverseLimit and undo all the changes that are related to this.

),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueMidShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public BlueOutpostShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedDepotShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedMidShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.SetTurretAngle;
import frc.robot.constants.enums.ShootingState;
Expand All @@ -26,7 +26,7 @@ public RedOutpostShoot(
super(
new LoggableParallelCommandGroup(
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueDepotShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueMidShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public BlueOutpostShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("BlueOutpostShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedDepotShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedDepotShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedMidShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidShoot")),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import choreo.auto.AutoFactory;
import frc.robot.commands.ShootButton;
import frc.robot.commands.angler.RunAnglerToReverseLimit;
import frc.robot.commands.angler.ResetAnglerEncoder;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.climber.ClimberUp;
import frc.robot.commands.shooter.SetShootingState;
Expand Down Expand Up @@ -31,7 +31,7 @@ public RedOutpostShootClimb(
new LoggableParallelCommandGroup(
new ClimberDown(climber),
new SetTurretAngle(turret, 0),
new RunAnglerToReverseLimit(angler)
new ResetAnglerEncoder(angler)
),
new SetShootingState(shootstate, ShootState.FIXED_2), //or some other shoot state
LoggableCommandWrapper.wrap(auto.resetOdometry("RedOutpostShoot")),
Expand Down

This file was deleted.

This file was deleted.

42 changes: 42 additions & 0 deletions src/main/java/frc/robot/commands/sequences/ResetAll.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
package frc.robot.commands.sequences;

import frc.robot.commands.angler.StowAngler;
import frc.robot.commands.climber.ClimberDown;
import frc.robot.commands.intakeDeployment.ToggleDeployment;
import frc.robot.commands.shooter.SetShootingState;
import frc.robot.commands.turret.RunTurretToRevLimit;
import frc.robot.constants.Constants;
import frc.robot.constants.enums.DeploymentState;
import frc.robot.constants.enums.ShootingState;
import frc.robot.constants.enums.ShootingState.ShootState;
import frc.robot.subsystems.AnglerSubsystem;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.FeederSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.IntakeDeployerSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.TurretSubsystem;
import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;

public class ResetAll extends LoggableSequentialCommandGroup {
public ResetAll(AnglerSubsystem anglerSubsystem, ClimberSubsystem climberSubsystem,
FeederSubsystem feederSubsystem, HopperSubsystem hopperSubsystem,
IntakeDeployerSubsystem intakeDeployerSubsystem, IntakeSubsystem intakeSubsystem,
ShooterSubsystem shooterSubsystem, TurretSubsystem turretSubsystem, ShootingState shootState) {

super(
DeploymentState deploymentState = intakeDeployerSubsystem.getDeploymentState();
if(deploymentState == DeploymentState.DOWN)
new ToggleDeployment(intakeDeployerSubsystem);
new LoggableParallelCommandGroup(
new StowAngler(anglerSubsystem),
new ClimberDown(climberSubsystem),
new SetShootingState(shootState, ShootState.STOPPED),
new RunTurretToRevLimit(turretSubsystem)
)
);
}
}

Loading