Autonomous Non-Blocking State Machines

Task: Design a state machine class to make autonomous easier

In the past our autonomous routines were tedious and difficult to change. Adding one step to the beginning of an autonomous would require changing the indexes of every single step afterwards, which could take a long time depending on the size of the routine. In addition, simple typos could go undetected, and cause lots of problems. Finally, there was so much repetitive code, making our routines over 400 lines long.

In order to remedy this, we decided to create a state machine class that takes care of the repetitive parts of our autonomous code. We created a StateMachine class, which allows us to build autonomous routines as sequences of "states", or individual steps. This new state machine system makes autonomous routines much easier to code and tune, as well as removing the possibility for small bugs. We also were able to shorten our code by converting it to the new system, reducing each routine from over 400 lines to approximately 30 lines.

Internally, StateMachine uses instances of the functional interface State (or some of its subclasses, SingleState for states that only need to be run once, TimedState, for states that are run on a timer, or MineralState, for states that do different things depending on the sampling order). Using a functional interface lets us use lambdas, which further reduce the length of our code. When it is executed, the state machine takes the current state and runs it. If the state is finished, the current state index (stored in a class called Stage) is incremented, and a state switch action is run, which stops all motors.

Here is an autonomous routine which has been converted to the new system:

private StateMachine auto_depotSample = getStateMachine(autoStage)
            .addNestedStateMachine(auto_setup) //common states to all autonomous
            .addMineralState(mineralStateProvider, //turn to mineral, depending on mineral
                    () -> robot.rotateIMU(39, TURN_TIME), //turn left
                    () -> true, //don't turn if mineral is in the middle
                    () -> robot.rotateIMU(321, TURN_TIME)) //turn right
            .addMineralState(mineralStateProvider, //move to mineral
                    () -> robot.driveForward(true, .604, DRIVE_POWER), //move more on the sides
                    () -> robot.driveForward(true, .47, DRIVE_POWER), //move less in the middle
                    () -> robot.driveForward(true, .604, DRIVE_POWER))
            .addMineralState(mineralStateProvider, //turn to depot
                    () -> robot.rotateIMU(345, TURN_TIME),
                    () -> true,
                    () -> robot.rotateIMU(15, TURN_TIME))
            .addMineralState(mineralStateProvider, //move to depot
                    () -> robot.driveForward(true, .880, DRIVE_POWER),
                    () -> robot.driveForward(true, .762, DRIVE_POWER),
                    () -> robot.driveForward(true, .890, DRIVE_POWER))
            .addTimedState(4, //turn on intake for 4 seconds
                    () -> robot.collector.eject(),
                    () -> robot.collector.stopIntake())
            .build();