Created
November 11, 2020 15:26
-
-
Save NoahBres/51c821612457db65949a7c4334488af3 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
public class StatePOC extends LinearOpMode { | |
private StateMachine autoMachine; | |
private StateMachine wobbleMachine; | |
enum WobbleState { | |
IDLE, | |
GRAB, | |
LIFT, | |
HOLD, | |
DUMP, | |
} | |
enum AutoState { | |
IDLE, | |
GO_TO_POINT_1, | |
GO_TO_POINT_2, | |
GRAB_WOBBLE, | |
GO_TO_POINT_3, | |
DUMP_WOBBLE | |
} | |
public void runOpMode() { | |
SampleMecanumDrive drive = new SampleMecanumDrive(); | |
// Do your trajectory stuff | |
autoMachine = new StateMachineBuilder<AutoState>() | |
.state(IDLE) | |
.waitForStart() | |
.state(GO_TO_POINT_1) | |
.onEnter(() -> drive.followTrajectoryAsync(traj1)) // followTrajectory upon entering state | |
.transition(() -> !drive.isBusy()) // Transition when trajectory is finished | |
.state(GO_TO_POINT_2) | |
.onEnter(() -> drive.followTrajectoryAsync(traj2)) // followTrajectory upon entering state | |
.transition(() -> !drive.isBusy()) // Transition when trajectory is finished | |
.state(GRAB_WOBBLE) | |
.onEnter(() -> wobbleMachine.start()) // Start the wobble machine upon entering state | |
.transition(() -> wobbleMachine.getState() == HOLD) // We wait until wobble machine is on HOLD | |
.state(GO_TO_POINT_3) | |
.onEnter(() -> drive.followTrajectoryAsync(traj3)) // followTrajectory upon entering state | |
.transition(() -> !drive.isBusy()) // Transition when trajectory is finished | |
.state(DUMP_WOBBLE) | |
.transition(() -> wobbleMachine.transition()) // Force wobble machine to transition to next state | |
.exit(IDLE) // Exit to an IDLE state | |
.build() | |
wobbleMachine = new StateMachineBuilder<WobbleState>() | |
.state(IDLE) | |
.waitForStart() | |
.state(GRAB) | |
.onEnter(() -> servo1.setPosition(0.5)) // Set position on enter | |
.transitionTimed(0.3) // Transition to next state after 0.3 seconds | |
.state(LIFT) | |
.onEnter(() -> servo2.setPosition(0.5)) // Set position on enter | |
.transitionTimed(0.5) // Transition to next state after 0.5 seconds | |
.state(HOLD) // HOLD state doesn't have a transition | |
// state machine will not transition unless transition() is called externally | |
.state(DUMP) | |
.onEnter(() -> servo1.setPosition(0.5)) // Set position on enter | |
.transitionTimed(0.3) // Transition to next state after 0.3 seconds | |
.onExit(() -> servo2.setPosition(0.3)) // Set position on exit | |
.exit(IDLE) // Exit to an IDLE state | |
.build(); | |
waitForStart() | |
autoMachine.start(); | |
while(opModeIsActive()) { | |
drive.update(); | |
autoMachine.update(); | |
wobbleMachine.update(); | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment