diff --git a/src/main/java/frc/robot/arm/ArmSubsystem.java b/src/main/java/frc/robot/arm/ArmSubsystem.java index 4ea40cb..9564db6 100644 --- a/src/main/java/frc/robot/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/arm/ArmSubsystem.java @@ -44,6 +44,97 @@ public ArmSubsystem(TalonFX leftMotor, TalonFX rightMotor) { rightMotor.getConfigurator().apply(RobotConfig.get().arm().rightMotorConfig()); RobotConfig.get().arm().feedSpotDistanceToAngle().accept(feedSpotDistanceToAngle); RobotConfig.get().arm().speakerDistanceToAngle().accept(speakerDistanceToAngle); + + createHandler(ArmState.PRE_MATCH_HOMING) + .onEnter( + () -> { + leftMotor.disable(); + rightMotor.disable(); + }); + createHandler(ArmState.IDLE) + .onEnter( + () -> { + leftMotor.setControl( + motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE)))); + rightMotor.setControl( + motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE)))); + }); + createHandler(ArmState.CLIMBING_1_LINEUP) + .onEnter( + () -> { + leftMotor.setControl( + motionMagicRequest.withPosition( + Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP)))); + rightMotor.setControl( + motionMagicRequest.withPosition( + Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP)))); + }); + createHandler(ArmState.CLIMBING_2_HANGING) + .onEnter( + () -> { + leftMotor.setControl( + motionMagicRequest.withPosition( + Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING)))); + rightMotor.setControl( + motionMagicRequest.withPosition( + Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING)))); + }); + createHandler(ArmState.DROP) + .onEnter( + () -> { + leftMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP)))); + rightMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP)))); + }); + createHandler(ArmState.PODIUM_SHOT) + .onEnter( + () -> { + leftMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM)))); + rightMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM)))); + }); + createHandler(ArmState.SUBWOOFER_SHOT) + .onEnter( + () -> { + leftMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER)))); + rightMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER)))); + }); + createHandler(ArmState.AMP) + .onEnter( + () -> { + leftMotor.setControl( + motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP)))); + rightMotor.setControl( + motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP)))); + }); + createHandler(ArmState.PASS) + .onEnter( + () -> { + leftMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS)))); + rightMotor.setControl( + pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS)))); + }); + createHandler(ArmState.SPEAKER_SHOT) + .withPeriodic( + () -> { + var newAngle = + Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker))); + leftMotor.setControl(pidRequest.withPosition(newAngle)); + rightMotor.setControl(pidRequest.withPosition(newAngle)); + }); + createHandler(ArmState.FEEDING) + .withPeriodic( + () -> { + var newAngle = + Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot))); + leftMotor.setControl(pidRequest.withPosition(newAngle)); + rightMotor.setControl(pidRequest.withPosition(newAngle)); + }); } public void setState(ArmState newState) { @@ -98,92 +189,10 @@ protected void collectInputs() { } } - @Override - protected void afterTransition(ArmState newState) { - switch (newState) { - case PRE_MATCH_HOMING -> { - leftMotor.disable(); - rightMotor.disable(); - } - case IDLE -> { - leftMotor.setControl( - motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE)))); - rightMotor.setControl( - motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.IDLE)))); - } - case CLIMBING_1_LINEUP -> { - leftMotor.setControl( - motionMagicRequest.withPosition( - Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP)))); - rightMotor.setControl( - motionMagicRequest.withPosition( - Units.degreesToRotations(clamp(ArmAngle.CLIMBING_1_LINEUP)))); - } - case CLIMBING_2_HANGING -> { - leftMotor.setControl( - motionMagicRequest.withPosition( - Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING)))); - rightMotor.setControl( - motionMagicRequest.withPosition( - Units.degreesToRotations(clamp(ArmAngle.CLIMBING_2_HANGING)))); - } - - case DROP -> { - leftMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP)))); - rightMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.DROP)))); - } - - case PODIUM_SHOT -> { - leftMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM)))); - rightMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PODIUM)))); - } - case SUBWOOFER_SHOT -> { - leftMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER)))); - rightMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.SUBWOOFER)))); - } - - case AMP -> { - leftMotor.setControl( - motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP)))); - rightMotor.setControl( - motionMagicRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.AMP)))); - } - case PASS -> { - leftMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS)))); - rightMotor.setControl( - pidRequest.withPosition(Units.degreesToRotations(clamp(ArmAngle.PASS)))); - } - default -> {} - } - } - @Override public void robotPeriodic() { super.robotPeriodic(); - switch (getState()) { - case SPEAKER_SHOT -> { - var newAngle = - Units.degreesToRotations(clamp(speakerDistanceToAngle.get(distanceToSpeaker))); - leftMotor.setControl(pidRequest.withPosition(newAngle)); - rightMotor.setControl(pidRequest.withPosition(newAngle)); - } - case FEEDING -> { - double newAngle = - Units.degreesToRotations(clamp(feedSpotDistanceToAngle.get(distanceToFeedSpot))); - leftMotor.setControl(pidRequest.withPosition(newAngle)); - rightMotor.setControl(pidRequest.withPosition(newAngle)); - } - default -> {} - } - DogLog.log("Arm/Left/StatorCurrent", leftMotor.getStatorCurrent().getValueAsDouble()); DogLog.log("Arm/Left/SupplyCurrent", leftMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log( diff --git a/src/main/java/frc/robot/intake/IntakeSubsystem.java b/src/main/java/frc/robot/intake/IntakeSubsystem.java index b71cd4a..b83c0cc 100644 --- a/src/main/java/frc/robot/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/intake/IntakeSubsystem.java @@ -19,6 +19,35 @@ public IntakeSubsystem(TalonFX mainMotor, CANSparkMax centeringMotor) { mainMotor.getConfigurator().apply(RobotConfig.get().intake().mainMotorConfig()); RobotConfig.get().intake().centeringMotorConfig().accept(centeringMotor); + + createHandler(IntakeState.IDLE) + .onEnter( + () -> { + mainMotor.disable(); + centeringMotor.disable(); + }); + createHandler(IntakeState.INTAKING) + .onEnter( + () -> { + mainMotor.setVoltage(12); + centeringMotor.setVoltage(8); + }); + createHandler(IntakeState.OUTTAKING) + .onEnter( + () -> { + mainMotor.setVoltage(-6); + centeringMotor.setVoltage(-10); + }); + createHandler(IntakeState.INTAKING_BACK) + .onEnter( + () -> { + mainMotor.setVoltage(-1); + }); + createHandler(IntakeState.INTAKING_FORWARD_PUSH) + .onEnter( + () -> { + mainMotor.setVoltage(1); + }); } public void setState(IntakeState newState) { @@ -29,37 +58,6 @@ public double getIntakeRotations() { return mainMotor.getRotorPosition().getValueAsDouble(); } - @Override - protected IntakeState getNextState(IntakeState currentState) { - return currentState; - } - - @Override - protected void afterTransition(IntakeState newState) { - switch (newState) { - case IDLE -> { - mainMotor.disable(); - centeringMotor.disable(); - } - - case INTAKING -> { - mainMotor.setVoltage(12); - centeringMotor.setVoltage(8); - } - - case OUTTAKING -> { - mainMotor.setVoltage(-6); - centeringMotor.setVoltage(-10); - } - case INTAKING_BACK -> { - mainMotor.setVoltage(-1); - } - case INTAKING_FORWARD_PUSH -> { - mainMotor.setVoltage(1); - } - } - } - @Override public void robotPeriodic() { super.robotPeriodic(); diff --git a/src/main/java/frc/robot/queuer/QueuerSubsystem.java b/src/main/java/frc/robot/queuer/QueuerSubsystem.java index 4e4ddf8..bc5c0f3 100644 --- a/src/main/java/frc/robot/queuer/QueuerSubsystem.java +++ b/src/main/java/frc/robot/queuer/QueuerSubsystem.java @@ -28,6 +28,19 @@ public QueuerSubsystem(TalonFX motor, DigitalInput sensor) { this.motor = motor; motor.getConfigurator().apply(RobotConfig.get().queuer().motorConfig()); + + createHandler(QueuerState.IDLE).onEnter(() -> motor.disable()); + createHandler(QueuerState.SHOOTING).onEnter(() -> motor.setVoltage(10)); + createHandler(QueuerState.INTAKING).onEnter(() -> motor.setVoltage(1)); + createHandler(QueuerState.OUTTAKING).onEnter(() -> motor.setVoltage(-4)); + createHandler(QueuerState.AMPING).onEnter(() -> motor.setVoltage(-10)); + createHandler(QueuerState.INTAKING_BACK).onEnter(() -> motor.setVoltage(-1)); + createHandler(QueuerState.INTAKING_FORWARD_PUSH) + .onEnter( + () -> { + goalPosition = motorPosition + 2; + motor.setControl(pidRequest.withPosition(goalPosition)); + }); } public void setState(QueuerState newState) { @@ -48,32 +61,10 @@ protected void collectInputs() { motorPosition = motor.getRotorPosition().getValueAsDouble(); } - @Override - protected QueuerState getNextState(QueuerState currentState) { - // State transitions are done by robot manager, not here - return currentState; - } - public boolean hasNote() { return debouncedSensorHasNote; } - @Override - protected void afterTransition(QueuerState newState) { - switch (newState) { - case IDLE -> motor.disable(); - case SHOOTING -> motor.setVoltage(10); - case INTAKING -> motor.setVoltage(1); - case OUTTAKING -> motor.setVoltage(-4); - case AMPING -> motor.setVoltage(-10); - case INTAKING_BACK -> motor.setVoltage(-1); - case INTAKING_FORWARD_PUSH -> { - goalPosition = motorPosition + 2; - motor.setControl(pidRequest.withPosition(goalPosition)); - } - } - } - @Override public void robotPeriodic() { super.robotPeriodic(); diff --git a/src/main/java/frc/robot/robot_manager/RobotManager.java b/src/main/java/frc/robot/robot_manager/RobotManager.java index 3fb9c35..e486523 100644 --- a/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -54,6 +54,324 @@ public RobotManager( this.intake = intake; this.queuer = queuer; this.swerve = swerve; + + // Idle + createHandler(RobotState.IDLE_NO_GP) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }); + createHandler(RobotState.IDLE_WITH_GP) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_WARMUP); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }); + + // Subwoofer + createHandler(RobotState.SUBWOOFER_WAITING) + .onEnter( + () -> { + arm.setState(ArmState.SUBWOOFER_SHOT); + shooter.setState(ShooterState.SUBWOOFER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getSubwooferAngle()); + }); + createHandler(RobotState.SUBWOOFER_PREPARE_TO_SCORE) + .onEnter(RobotState.SUBWOOFER_WAITING) + .withTransitions( + () -> shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : null); + createHandler(RobotState.SUBWOOFER_SCORING) + .onEnter( + () -> { + arm.setState(ArmState.SUBWOOFER_SHOT); + shooter.setState(ShooterState.SUBWOOFER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getSubwooferAngle()); + }) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Podium + createHandler(RobotState.PODIUM_WAITING) + .onEnter( + () -> { + arm.setState(ArmState.PODIUM_SHOT); + shooter.setState(ShooterState.PODIUM_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getPodiumAngle()); + }); + createHandler(RobotState.PODIUM_PREPARE_TO_SCORE) + .onEnter(RobotState.PODIUM_WAITING) + .withTransitions(() -> shooter.atGoal() && arm.atGoal() ? RobotState.PODIUM_SCORING : null); + createHandler(RobotState.PODIUM_SCORING) + .onEnter( + () -> { + arm.setState(ArmState.PODIUM_SHOT); + shooter.setState(ShooterState.PODIUM_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getPodiumAngle()); + }) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Speaker + createHandler(RobotState.SPEAKER_PREPARE_TO_SCORE) + .onEnter( + () -> { + arm.setState(ArmState.SPEAKER_SHOT); + shooter.setState(ShooterState.SPEAKER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); + }) + .withPeriodic( + () -> { + shooter.setDistanceToSpeaker(distanceToSpeaker); + arm.setDistanceToSpeaker(distanceToSpeaker); + swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); + }) + .withTransitions( + () -> + (shooter.atGoal() && arm.atGoal()) + && (DriverStation.isAutonomous() || confirmShotActive == true) + ? RobotState.SPEAKER_SCORING + : null); + createHandler(RobotState.SPEAKER_WAITING) + .onEnter(RobotState.SPEAKER_PREPARE_TO_SCORE) + .withPeriodic(RobotState.SPEAKER_PREPARE_TO_SCORE); + createHandler(RobotState.SPEAKER_SCORING) + .onEnter( + () -> { + arm.setState(ArmState.SPEAKER_SHOT); + shooter.setState(ShooterState.SPEAKER_SHOT); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); + }) + .withPeriodic(RobotState.SPEAKER_PREPARE_TO_SCORE) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Amp + createHandler(RobotState.AMP_PREPARE_TO_SCORE) + .onEnter( + () -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.IDLE_WARMUP); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getAmpAngle()); + }) + .withTransitions(() -> shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : null); + createHandler(RobotState.AMP_WAITING).onEnter(RobotState.AMP_PREPARE_TO_SCORE); + createHandler(RobotState.AMP_SCORING) + .onEnter( + () -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.IDLE_WARMUP); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.AMPING); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getAmpAngle()); + }) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Feeding + createHandler(RobotState.FEEDING_PREPARE_TO_SHOOT) + .onEnter( + () -> { + arm.setState(ArmState.FEEDING); + shooter.setState(ShooterState.FEEDING); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(fieldRelativeAngleToFeedSpot); + }) + .withPeriodic( + () -> { + shooter.setDistanceToFeedSpot(distanceToFeedSpot); + arm.setDistanceToFeedSpot(distanceToFeedSpot); + }); + createHandler(RobotState.FEEDING_WAITING) + .onEnter(RobotState.FEEDING_PREPARE_TO_SHOOT) + .withPeriodic(RobotState.FEEDING_PREPARE_TO_SHOOT) + .withTransitions( + () -> shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : null); + createHandler(RobotState.FEEDING_SHOOTING) + .onEnter( + () -> { + arm.setState(ArmState.FEEDING); + shooter.setState(ShooterState.FEEDING); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(fieldRelativeAngleToFeedSpot); + }) + .withPeriodic(RobotState.FEEDING_PREPARE_TO_SHOOT) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Passing + createHandler(RobotState.PASS_PREPARE_TO_SHOOT) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.PASS); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }) + .withTransitions(() -> shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : null); + createHandler(RobotState.PASS_SHOOTING) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.PASS); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.SHOOTING); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.IDLE_NO_GP); + + // Unjam + createHandler(RobotState.UNJAM) + .onEnter( + () -> { + arm.setState(ArmState.AMP); + shooter.setState(ShooterState.PASS); + intake.setState(IntakeState.OUTTAKING); + queuer.setState(QueuerState.OUTTAKING); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }); + + // Intaking + createHandler(RobotState.INTAKING) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING); + queuer.setState(QueuerState.INTAKING); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }) + .withPeriodic( + () -> { + if (arm.atGoal()) { + intake.setState(IntakeState.INTAKING); + } else { + intake.setState(IntakeState.IDLE); + } + }) + .withTransitions(() -> queuer.hasNote() ? RobotState.INTAKING_BACK : null); + createHandler(RobotState.INTAKE_ASSIST) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING); + queuer.setState(QueuerState.INTAKING); + // We don't use the setSnaps here, since intake assist is a separate state + if (DriverStation.isTeleop()) { + swerve.setState(SwerveState.INTAKE_ASSIST_TELEOP); + } else { + swerve.setState(SwerveState.INTAKE_ASSIST_AUTO); + } + }) + .withPeriodic(RobotState.INTAKING) + .withTransitions(RobotState.INTAKING); + createHandler(RobotState.INTAKING_BACK) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING_BACK); + queuer.setState(QueuerState.INTAKING_BACK); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }) + .withPeriodic( + () -> { + if (arm.atGoal()) { + intake.setState(IntakeState.INTAKING_BACK); + } else { + intake.setState(IntakeState.IDLE); + } + }) + .withTransitions(() -> queuer.hasNote() ? null : RobotState.INTAKING_FORWARD_PUSH); + createHandler(RobotState.INTAKING_FORWARD_PUSH) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.INTAKING_FORWARD_PUSH); + queuer.setState(QueuerState.INTAKING_FORWARD_PUSH); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }) + .withPeriodic( + () -> { + if (arm.atGoal()) { + intake.setState(IntakeState.INTAKING_FORWARD_PUSH); + } else { + intake.setState(IntakeState.IDLE); + } + }) + .withTransitions(() -> queuer.hasNote() ? RobotState.IDLE_WITH_GP : null); + + // Outtake + createHandler(RobotState.OUTTAKING) + .onEnter( + () -> { + arm.setState(ArmState.IDLE); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.OUTTAKING); + queuer.setState(QueuerState.OUTTAKING); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }); + + // Climbing + createHandler(RobotState.CLIMBING_1_LINEUP) + .onEnter( + () -> { + arm.setState(ArmState.CLIMBING_1_LINEUP); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(true); + swerve.setSnapToAngle(SnapUtil.getClimbingAngle(imu)); + }); + createHandler(RobotState.CLIMBING_2_HANGING) + .onEnter( + () -> { + arm.setState(ArmState.CLIMBING_2_HANGING); + shooter.setState(ShooterState.IDLE_STOPPED); + intake.setState(IntakeState.IDLE); + queuer.setState(QueuerState.IDLE); + swerve.setSnapsEnabled(false); + swerve.setSnapToAngle(0); + }); } @Override @@ -64,276 +382,6 @@ protected void collectInputs() { localization.getFieldRelativeAngleToPose(FieldUtil.getFeedSpotPose()); } - @Override - protected RobotState getNextState(RobotState currentState) { - return switch (currentState) { - case IDLE_NO_GP, - IDLE_WITH_GP, - CLIMBING_1_LINEUP, - CLIMBING_2_HANGING, - OUTTAKING, - AMP_SCORING, - SPEAKER_WAITING, - AMP_WAITING, - SUBWOOFER_WAITING, - FEEDING_WAITING, - PODIUM_WAITING -> - currentState; - - case SPEAKER_SCORING, FEEDING_SHOOTING, PASS_SHOOTING, SUBWOOFER_SCORING, PODIUM_SCORING -> - queuer.hasNote() ? currentState : RobotState.IDLE_NO_GP; - - case SPEAKER_PREPARE_TO_SCORE -> - (shooter.atGoal() && arm.atGoal()) - && (DriverStation.isAutonomous() || confirmShotActive == true) - ? RobotState.SPEAKER_SCORING - : currentState; - - case AMP_PREPARE_TO_SCORE -> - shooter.atGoal() && arm.atGoal() ? RobotState.AMP_SCORING : currentState; - - case FEEDING_PREPARE_TO_SHOOT -> - shooter.atGoal() && arm.atGoal() ? RobotState.FEEDING_SHOOTING : currentState; - case PASS_PREPARE_TO_SHOOT -> - shooter.atGoal() && arm.atGoal() ? RobotState.PASS_SHOOTING : currentState; - case SUBWOOFER_PREPARE_TO_SCORE -> - shooter.atGoal() && arm.atGoal() ? RobotState.SUBWOOFER_SCORING : currentState; - case PODIUM_PREPARE_TO_SCORE -> - shooter.atGoal() && arm.atGoal() ? RobotState.PODIUM_SCORING : currentState; - - case UNJAM -> currentState; - case INTAKING, INTAKE_ASSIST -> queuer.hasNote() ? RobotState.INTAKING_BACK : currentState; - case INTAKING_BACK -> !queuer.hasNote() ? RobotState.INTAKING_FORWARD_PUSH : currentState; - case INTAKING_FORWARD_PUSH -> queuer.atGoal() ? RobotState.IDLE_WITH_GP : currentState; - }; - } - - @Override - protected void afterTransition(RobotState newState) { - switch (newState) { - case SUBWOOFER_PREPARE_TO_SCORE, SUBWOOFER_WAITING -> { - arm.setState(ArmState.SUBWOOFER_SHOT); - shooter.setState(ShooterState.SUBWOOFER_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getSubwooferAngle()); - } - case PODIUM_PREPARE_TO_SCORE, PODIUM_WAITING -> { - arm.setState(ArmState.PODIUM_SHOT); - shooter.setState(ShooterState.PODIUM_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getPodiumAngle()); - } - case PODIUM_SCORING -> { - arm.setState(ArmState.PODIUM_SHOT); - shooter.setState(ShooterState.PODIUM_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.SHOOTING); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getPodiumAngle()); - } - case SUBWOOFER_SCORING -> { - arm.setState(ArmState.SUBWOOFER_SHOT); - shooter.setState(ShooterState.SUBWOOFER_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.SHOOTING); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getSubwooferAngle()); - } - case SPEAKER_PREPARE_TO_SCORE, SPEAKER_WAITING -> { - arm.setState(ArmState.SPEAKER_SHOT); - shooter.setState(ShooterState.SPEAKER_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); - } - case SPEAKER_SCORING -> { - arm.setState(ArmState.SPEAKER_SHOT); - shooter.setState(ShooterState.SPEAKER_SHOT); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.SHOOTING); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); - } - case AMP_PREPARE_TO_SCORE, AMP_WAITING -> { - arm.setState(ArmState.AMP); - shooter.setState(ShooterState.IDLE_WARMUP); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getAmpAngle()); - } - case AMP_SCORING -> { - arm.setState(ArmState.AMP); - shooter.setState(ShooterState.IDLE_WARMUP); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.AMPING); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getAmpAngle()); - } - case FEEDING_PREPARE_TO_SHOOT, FEEDING_WAITING -> { - arm.setState(ArmState.FEEDING); - shooter.setState(ShooterState.FEEDING); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(fieldRelativeAngleToFeedSpot); - } - case FEEDING_SHOOTING -> { - arm.setState(ArmState.FEEDING); - shooter.setState(ShooterState.FEEDING); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.SHOOTING); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(fieldRelativeAngleToFeedSpot); - } - case PASS_PREPARE_TO_SHOOT -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.PASS); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case PASS_SHOOTING -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.PASS); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.SHOOTING); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case UNJAM -> { - arm.setState(ArmState.AMP); - shooter.setState(ShooterState.PASS); - intake.setState(IntakeState.OUTTAKING); - queuer.setState(QueuerState.OUTTAKING); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case INTAKING -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.INTAKING); - queuer.setState(QueuerState.INTAKING); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case INTAKING_BACK -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.INTAKING_BACK); - queuer.setState(QueuerState.INTAKING_BACK); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case INTAKING_FORWARD_PUSH -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.INTAKING_FORWARD_PUSH); - queuer.setState(QueuerState.INTAKING_FORWARD_PUSH); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case INTAKE_ASSIST -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.INTAKING); - queuer.setState(QueuerState.INTAKING); - // We don't use the setSnaps here, since intake assist is a separate state - if (DriverStation.isTeleop()) { - swerve.setState(SwerveState.INTAKE_ASSIST_TELEOP); - } else { - swerve.setState(SwerveState.INTAKE_ASSIST_AUTO); - } - } - case OUTTAKING -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.OUTTAKING); - queuer.setState(QueuerState.OUTTAKING); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case CLIMBING_1_LINEUP -> { - arm.setState(ArmState.CLIMBING_1_LINEUP); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(true); - swerve.setSnapToAngle(SnapUtil.getClimbingAngle(imu)); - } - case CLIMBING_2_HANGING -> { - arm.setState(ArmState.CLIMBING_2_HANGING); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case IDLE_NO_GP -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_STOPPED); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - case IDLE_WITH_GP -> { - arm.setState(ArmState.IDLE); - shooter.setState(ShooterState.IDLE_WARMUP); - intake.setState(IntakeState.IDLE); - queuer.setState(QueuerState.IDLE); - swerve.setSnapsEnabled(false); - swerve.setSnapToAngle(0); - } - } - } - - @Override - public void robotPeriodic() { - super.robotPeriodic(); - - // Continuous state actions - switch (getState()) { - case SPEAKER_PREPARE_TO_SCORE, SPEAKER_SCORING, SPEAKER_WAITING -> { - shooter.setDistanceToSpeaker(distanceToSpeaker); - arm.setDistanceToSpeaker(distanceToSpeaker); - swerve.setSnapToAngle(fieldRelativeAngleToSpeaker); - } - case FEEDING_PREPARE_TO_SHOOT, FEEDING_SHOOTING, FEEDING_WAITING -> { - shooter.setDistanceToFeedSpot(distanceToFeedSpot); - arm.setDistanceToFeedSpot(distanceToFeedSpot); - } - case INTAKING, INTAKE_ASSIST -> { - if (arm.atGoal()) { - intake.setState(IntakeState.INTAKING); - } else { - intake.setState(IntakeState.IDLE); - } - } - case INTAKING_BACK -> { - if (arm.atGoal()) { - intake.setState(IntakeState.INTAKING_BACK); - } else { - intake.setState(IntakeState.IDLE); - } - } - case INTAKING_FORWARD_PUSH -> { - if (arm.atGoal()) { - intake.setState(IntakeState.INTAKING_FORWARD_PUSH); - } else { - intake.setState(IntakeState.IDLE); - } - } - default -> {} - } - } - public void setConfirmShotActive(boolean newValue) { confirmShotActive = newValue; } diff --git a/src/main/java/frc/robot/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/shooter/ShooterSubsystem.java index 7bf3af0..1f62a54 100644 --- a/src/main/java/frc/robot/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/shooter/ShooterSubsystem.java @@ -38,6 +38,57 @@ public ShooterSubsystem(TalonFX topMotor, TalonFX bottomMotor) { RobotConfig.get().shooter().feedSpotDistanceToRpm().accept(feedSpotDistanceToRpm); RobotConfig.get().shooter().speakerDistanceToRpm().accept(speakerDistanceToRpm); + + createHandler(ShooterState.IDLE_STOPPED) + .onEnter( + () -> { + topMotor.disable(); + bottomMotor.disable(); + }); + createHandler(ShooterState.IDLE_WARMUP) + .onEnter( + () -> { + topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WARMUP / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WARMUP / 60.0)); + }); + createHandler(ShooterState.SUBWOOFER_SHOT) + .onEnter( + () -> { + topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.SUBWOOFER / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.SUBWOOFER / 60.0)); + }); + createHandler(ShooterState.DROP) + .onEnter( + () -> { + topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); + }); + createHandler(ShooterState.PODIUM_SHOT) + .onEnter( + () -> { + topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); + }); + createHandler(ShooterState.PASS) + .onEnter( + () -> { + topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0)); + }); + createHandler(ShooterState.SPEAKER_SHOT) + .withPeriodic( + () -> { + var goalRpm = speakerDistanceToRpm.get(distanceToSpeaker); + topMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); + }); + createHandler(ShooterState.FEEDING) + .withPeriodic( + () -> { + var goalRpm = speakerDistanceToRpm.get(distanceToFeedSpot); + topMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); + bottomMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); + }); } public boolean atGoal() { @@ -74,57 +125,10 @@ protected void collectInputs() { bottomMotorRpm = bottomMotor.getVelocity().getValueAsDouble() * 60.0; } - @Override - protected void afterTransition(ShooterState newState) { - switch (newState) { - case IDLE_STOPPED -> { - topMotor.disable(); - bottomMotor.disable(); - } - case IDLE_WARMUP -> { - topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WARMUP / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WARMUP / 60.0)); - } - - case SUBWOOFER_SHOT -> { - topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.SUBWOOFER / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.SUBWOOFER / 60.0)); - } - - case DROP -> { - topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.DROP / 60.0)); - } - case PODIUM_SHOT -> { - topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PODIUM / 60.0)); - } - case PASS -> { - topMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(ShooterRpms.PASS / 60.0)); - } - default -> {} - } - } - @Override public void robotPeriodic() { super.robotPeriodic(); - switch (getState()) { - case SPEAKER_SHOT -> { - var goalRpm = speakerDistanceToRpm.get(distanceToSpeaker); - topMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); - } - case FEEDING -> { - var goalRpm = speakerDistanceToRpm.get(distanceToFeedSpot); - topMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); - bottomMotor.setControl(velocityRequest.withVelocity(goalRpm / 60.0)); - } - default -> {} - } - DogLog.log("Shooter/Top/StatorCurrent", topMotor.getStatorCurrent().getValueAsDouble()); DogLog.log("Shooter/Top/SupplyCurrent", topMotor.getSupplyCurrent().getValueAsDouble()); DogLog.log("Shooter/Top/RPM", topMotor.getVelocity().getValueAsDouble() * 60.0); diff --git a/src/main/java/frc/robot/util/state_machines/StateHandler.java b/src/main/java/frc/robot/util/state_machines/StateHandler.java new file mode 100644 index 0000000..e133e14 --- /dev/null +++ b/src/main/java/frc/robot/util/state_machines/StateHandler.java @@ -0,0 +1,76 @@ +package frc.robot.util.state_machines; + +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.Supplier; + +public class StateHandler> { + private final Map> stateHandlers; + + private Runnable onEnterAction = () -> {}; + private Supplier withTransitionsAction = () -> null; + private Consumer onExitAction = (nextState) -> {}; + private Runnable periodicAction = () -> {}; + + // TODO: Consider adding in a state-specific collectInputs() method + + StateHandler(Map> stateHandlers) { + this.stateHandlers = stateHandlers; + } + + public StateHandler onEnter(Runnable onEnterAction) { + this.onEnterAction = onEnterAction; + return this; + } + + public StateHandler onEnter(S state) { + this.onEnterAction = stateHandlers.get(state).onEnterAction; + return this; + } + + public StateHandler withTransitions(Supplier withTransitionsAction) { + this.withTransitionsAction = withTransitionsAction; + return this; + } + + public StateHandler withTransitions(S state) { + this.withTransitionsAction = () -> state; + return this; + } + + public StateHandler onExit(Consumer onExitAction) { + this.onExitAction = onExitAction; + return this; + } + + public StateHandler onExit(S state) { + this.onExitAction = stateHandlers.get(state).onExitAction; + return this; + } + + public StateHandler withPeriodic(Runnable periodicAction) { + this.periodicAction = periodicAction; + return this; + } + + public StateHandler withPeriodic(S state) { + this.periodicAction = stateHandlers.get(state).periodicAction; + return this; + } + + void doOnEnter() { + onEnterAction.run(); + } + + S doTransitions() { + return withTransitionsAction.get(); + } + + void doOnExit(S nextState) { + onExitAction.accept(nextState); + } + + void doPeriodic() { + periodicAction.run(); + } +} diff --git a/src/main/java/frc/robot/util/state_machines/StateMachine.java b/src/main/java/frc/robot/util/state_machines/StateMachine.java index a7b2cb0..a3c7d36 100644 --- a/src/main/java/frc/robot/util/state_machines/StateMachine.java +++ b/src/main/java/frc/robot/util/state_machines/StateMachine.java @@ -1,19 +1,21 @@ package frc.robot.util.state_machines; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.LifecycleSubsystemManager; import frc.robot.util.scheduling.SubsystemPriority; +import java.util.EnumMap; import java.util.Set; /** A state machine backed by {@link LifecycleSubsystem}. */ public abstract class StateMachine> extends LifecycleSubsystem { - private S state; - private boolean isInitialized = false; + private S previousState = null; + private S currentState; private double lastTransitionTimestamp = Timer.getFPGATimestamp(); + private final EnumMap> stateHandlers; + private StateHandler currentStateHandler; /** * Creates a new state machine. @@ -23,23 +25,47 @@ public abstract class StateMachine> extends LifecycleSubsystem */ protected StateMachine(SubsystemPriority priority, S initialState) { super(priority); - state = initialState; + currentState = initialState; + stateHandlers = new EnumMap<>(initialState.getDeclaringClass()); + + // Ensures that all states have a default handler + for (S state : initialState.getDeclaringClass().getEnumConstants()) { + stateHandlers.put(state, new StateHandler<>(stateHandlers)); + } + + currentStateHandler = stateHandlers.get(initialState); } /** Processes collecting inputs, state transitions, and state actions. */ @Override public void robotPeriodic() { - // The first time the robot boots up, we need to set the state from null to the initial state - // This also gives us an opportunity to run the state actions for the initial state - // Think of it as transitioning from the robot being off to initialState - if (!isInitialized) { - doTransition(); - isInitialized = true; + collectInputs(); + + // A transition occurred last time, so we handle the exit and enter actions + if (previousState != currentState) { + lastTransitionTimestamp = Timer.getFPGATimestamp(); + currentStateHandler.doOnEnter(); + + if (previousState != null) { + var previousStateHandler = stateHandlers.get(previousState); + previousStateHandler.doOnExit(currentState); + } } - collectInputs(); + currentStateHandler.doPeriodic(); + + var nextStateOrNull = currentStateHandler.doTransitions(); - setStateFromRequest(getNextState(state)); + // A transition should occur next loop + if (nextStateOrNull != null) { + previousState = currentState; + currentState = nextStateOrNull; + currentStateHandler = stateHandlers.get(currentState); + } + } + + public StateHandler createHandler(S state) { + return stateHandlers.get(state); } /** @@ -48,7 +74,7 @@ public void robotPeriodic() { * @return The current state. */ public S getState() { - return state; + return currentState; } /** @@ -58,7 +84,7 @@ public S getState() { * @return A command that waits until the state is equal to the goal state. */ public Command waitForState(S goalState) { - return Commands.waitUntil(() -> this.state == goalState); + return Commands.waitUntil(() -> this.currentState == goalState); } /** @@ -68,7 +94,7 @@ public Command waitForState(S goalState) { * @return A command that waits until the state is equal to any of the goal states. */ public Command waitForStates(Set goalStates) { - return Commands.waitUntil(() -> goalStates.contains(this.state)); + return Commands.waitUntil(() -> goalStates.contains(this.currentState)); } /** @@ -78,25 +104,6 @@ public Command waitForStates(Set goalStates) { */ protected void collectInputs() {} - /** - * Process transitions from one state to another. - * - *

Default behavior is to stay in the current state indefinitely. - * - * @param currentState The current state. - * @return The new state after processing transitions. - */ - protected S getNextState(S currentState) { - return currentState; - } - - /** - * Runs once after entering a new state. This is where you should run state actions. - * - * @param newState The newly entered state. - */ - protected void afterTransition(S newState) {} - /** * Used to change to a new state when a request is made. Will also trigger all logic that should * happen when a state transition occurs. @@ -104,13 +111,7 @@ protected void afterTransition(S newState) {} * @param requestedState The new state to transition to. */ protected void setStateFromRequest(S requestedState) { - if (state == requestedState) { - // No change - return; - } - - state = requestedState; - doTransition(); + currentState = requestedState; } /** @@ -125,13 +126,4 @@ protected boolean timeout(double duration) { return currentStateDuration > duration; } - - /** Run side effects that occur when a state transition happens. */ - private void doTransition() { - DogLog.log(subsystemName + "/State", state); - - lastTransitionTimestamp = Timer.getFPGATimestamp(); - - afterTransition(state); - } }