Skip to content

Commit

Permalink
Abstract subsystems from state machine (#136)
Browse files Browse the repository at this point in the history 
* completed abstraction

* leds should be blue and yellow for our team colors >:(

* Update src/main/java/frc/robot/commands/Autos/ShootSequence.java

* Update src/main/java/frc/robot/commands/Autos/ShootSequence.java

---------

Co-authored-by: Tayler Uva <[email protected]>
  • Loading branch information
@Alenguye582 @TaylerUva
Alenguye582 and TaylerUva authored Nov 12, 2024
1 parent 70b9999 commit 62ed075
Show file tree
Hide file tree
Showing 7 changed files with 52 additions and 65 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -686,7 +686,7 @@ public static class constLEDs {
(LED_NUMBER / 2) - 5,
TwinklePercent.Percent100,
8);
public static final TwinkleAnimation DISABLED_COLOR_2 = new TwinkleAnimation(255, 100, 0, 0, 0.5,
public static final TwinkleAnimation DISABLED_COLOR_2 = new TwinkleAnimation(255, 255, 0, 0, 0.5,
(LED_NUMBER / 2),
TwinklePercent.Percent100, (LED_NUMBER / 2) + 3);

Expand Down
57 changes: 20 additions & 37 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,6 @@ public class RobotContainer {
private final SN_XboxController conOperator = new SN_XboxController(mapControllers.OPERATOR_USB);
private final SN_XboxController conTestOperator = new SN_XboxController(mapControllers.TEST_OPERATOR_USB);

public final static StateMachine subStateMachine = new StateMachine();
private final static Climber subClimber = new Climber();
private final static Drivetrain subDrivetrain = new Drivetrain();
private final static Elevator subElevator = new Elevator();
Expand All @@ -65,6 +64,8 @@ public class RobotContainer {
private final static Shooter subShooter = new Shooter();
private final static Transfer subTransfer = new Transfer();
private final static Limelight subLimelight = new Limelight();
public final static StateMachine subStateMachine = new StateMachine(subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter);

private final Trigger gamePieceStoredTrigger = new Trigger(() -> subTransfer.getGamePieceStored());
private final Trigger gamePieceCollectedTrigger = new Trigger(() -> subIntake.getGamePieceCollected());
Expand Down Expand Up @@ -107,8 +108,7 @@ public RobotContainer() {
gamePieceStoredTrigger
.onTrue(Commands
.deferredProxy(
() -> subStateMachine.tryState(RobotState.STORE_FEEDER, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter))
() -> subStateMachine.tryState(RobotState.STORE_FEEDER))
.andThen(Commands.deferredProxy(
() -> subStateMachine.tryTargetState(subStateMachine, subIntake, subLEDs, subShooter, subTransfer,
subElevator, subDrivetrain))))
Expand Down Expand Up @@ -148,8 +148,7 @@ public RobotContainer() {
conDriver.setTriggerPressThreshold(0.2);

NamedCommands.registerCommand("Intaking", Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
() -> subStateMachine.tryState(RobotState.INTAKING))
.until(gamePieceStoredTrigger));

SmartDashboard.putNumber("Preload Only Delay", 0);
Expand All @@ -167,10 +166,8 @@ private void configureDriverBindings(SN_XboxController controller) {
Commands.runOnce(() -> subDrivetrain.resetPoseToPose(constField.getFieldPositions().get()[6].toPose2d())));

// Intake from source
controller.btn_East.whileTrue(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKE_SOURCE,
subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter)))
.onFalse(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber,
subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
controller.btn_East.whileTrue(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKE_SOURCE)))
.onFalse(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.NONE))
.unless(() -> comIntakeSource.getIntakeSourceGamePiece()));
}

Expand All @@ -179,78 +176,64 @@ private void configureOperatorBindings(SN_XboxController controller) {
// Intake
controller.btn_LeftTrigger
.whileTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter)))
() -> subStateMachine.tryState(RobotState.INTAKING)))
.onFalse(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
() -> subStateMachine.tryState(RobotState.NONE))
.unless(gamePieceStoredTrigger));

// Shoot
controller.btn_RightTrigger.whileTrue(
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING,
subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter)))
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING)))
.onFalse(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter))
() -> subStateMachine.tryState(RobotState.NONE))
.unless(gamePieceStoredTrigger));

// Prep with vision
controller.btn_RightBumper.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)))
.onTrue(Commands
.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_VISION)));

// Ejecting
controller.btn_LeftBumper.whileTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.EJECTING, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter)))
() -> subStateMachine.tryState(RobotState.EJECTING)))
.onFalse(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.NONE)));

// Prep spike
controller.btn_X.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPIKE)))
.onTrue(
Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_SPIKE, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_SPIKE)));

controller.btn_B.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_AMP)))
.onTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_AMP, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_AMP)));

controller.btn_Y.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SUB_BACKWARDS)))
.onTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS)));

// "Unalive Shooter"
controller.btn_A.onTrue(
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE,
subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE))
.alongWith(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_NONE))))
.onFalse(Commands.runOnce(() -> subShooter.setShootingNeutralOutput()));

// Prep subwoofer
controller.btn_South.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPEAKER)))
.onTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_SPEAKER, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_SPEAKER)));

// Prep wing
controller.btn_North.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_WING)))
.onTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_WING, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_WING)));

// Prep shuffle
controller.btn_West.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SHUFFLE)))
.onTrue(Commands.deferredProxy(
() -> subStateMachine.tryState(RobotState.PREP_SHUFFLE, subStateMachine, subClimber, subDrivetrain,
subElevator, subIntake, subLEDs, subTransfer, subShooter)));
() -> subStateMachine.tryState(RobotState.PREP_SHUFFLE)));

// Game Piece Override
controller.btn_East.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true))
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/commands/Autos/Centerline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s
() -> subDrivetrain.resetPoseToPose(getInitialPose().get())),

// -- PRELOAD --
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
.until(() -> subTransfer.getGamePieceStored()).withTimeout(3),

Commands.deferredProxy(shootSequence),
Expand Down
9 changes: 3 additions & 6 deletions src/main/java/frc/robot/commands/Autos/PreloadOnly.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,22 +78,19 @@ public PreloadOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain

Commands.waitSeconds(secondSupplier.getAsDouble()),

Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
.until(() -> subTransfer.getGamePieceStored()),

Commands.waitUntil(() -> subShooter.readyToShoot()),

// Shoot! (Ends when we don't have a game piece anymore)
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
subTransfer, subShooter)
.tryState(RobotState.SHOOTING)
.until(() -> !subTransfer.getGamePieceStored())),

// Reset subsystems to chill
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
subTransfer, subShooter)));
.tryState(RobotState.NONE)));
}

public Supplier<Pose2d> getInitialPose() {
Expand Down
12 changes: 4 additions & 8 deletions src/main/java/frc/robot/commands/Autos/PreloadTaxi.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,7 @@ public class PreloadTaxi extends SequentialCommandGroup {
Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)),

Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
subTransfer, subShooter)
.tryState(RobotState.PREP_VISION)
.repeatedly().until(() -> subShooter.readyToShoot())),

Commands.runOnce(() -> subDrivetrain.drive(
Expand All @@ -62,14 +61,12 @@ public class PreloadTaxi extends SequentialCommandGroup {

// Shoot! (Ends when we don't have a game piece anymore)
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
subLEDs, subTransfer, subShooter)
.tryState(RobotState.SHOOTING)
.until(() -> !subTransfer.getGamePieceStored())),

// Reset subsystems to chill
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
subTransfer, subShooter)),
.tryState(RobotState.NONE)),

Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)));

Expand All @@ -91,8 +88,7 @@ public PreloadTaxi(StateMachine subStateMachine, Climber subClimber, Drivetrain
getInitialPose().get())),

// -- PRELOAD --
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
.until(() -> subTransfer.getGamePieceStored()),

Commands.deferredProxy(() -> shootSequence),
Expand Down
10 changes: 3 additions & 7 deletions src/main/java/frc/robot/commands/Autos/ShootSequence.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,7 @@ public ShootSequence(StateMachine subStateMachine, Climber subClimber, Drivetrai

Commands.parallel(
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain, subElevator,
subIntake, subLEDs, subTransfer, subShooter)
.tryState(RobotState.PREP_VISION)
.repeatedly()),

Commands.runOnce(() -> subDrivetrain.drive(
Expand All @@ -65,17 +64,14 @@ public ShootSequence(StateMachine subStateMachine, Climber subClimber, Drivetrai

// Shoot! (Ends when we don't have a game piece anymore)
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
subLEDs, subTransfer, subShooter)
.tryState(RobotState.SHOOTING)
.until(() -> !subTransfer.getGamePieceStored())),

Commands.waitSeconds(constShooter.AUTO_PREP_NONE_DELAY.in(Units.Seconds)),

// Reset subsystems to chill
Commands.deferredProxy(() -> subStateMachine
.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
subLEDs, subTransfer,
subShooter)))
.tryState(RobotState.NONE)))
.unless(() -> !subTransfer.getGamePieceStored()));

}
Expand Down
24 changes: 20 additions & 4 deletions src/main/java/frc/robot/subsystems/StateMachine.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,29 @@
public class StateMachine extends SubsystemBase {
public static RobotState currentState;
public static TargetState currentTargetState;
Climber subClimber;
Drivetrain subDrivetrain;
Elevator subElevator;
Intake subIntake;
LEDs subLEDs;
Transfer subTransfer;
Shooter subShooter;
StateMachine subStateMachine = this;

/** Creates a new StateMachine. */
public StateMachine() {
public StateMachine(Climber subClimber,
Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, LEDs subLEDs, Transfer subTransfer,
Shooter subShooter) {
currentState = RobotState.NONE;
currentTargetState = TargetState.PREP_NONE;

this.subClimber = subClimber;
this.subDrivetrain = subDrivetrain;
this.subElevator = subElevator;
this.subIntake = subIntake;
this.subLEDs = subLEDs;
this.subTransfer = subTransfer;
this.subShooter = subShooter;
}

public void setRobotState(RobotState robotState) {
Expand Down Expand Up @@ -59,9 +77,7 @@ public TargetState getTargetState() {
* possible from your current state
* @return The Command to run for that desired state
*/
public Command tryState(RobotState desiredState, StateMachine subStateMachine, Climber subClimber,
Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, LEDs subLEDs, Transfer subTransfer,
Shooter subShooter) {
public Command tryState(RobotState desiredState) {

// TODO: Write this functionality in a later pr
if (isGivenStateTargetState(desiredState)) {
Expand Down

0 comments on commit 62ed075

Please sign in to comment.