Autonomous Non-Blocking State MachinesTags: Control and Software
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.
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: