C++ code for our previous years robots is available on GitHub at https://github.com/KennedyRoboEagles/PublicRobotCode. There are several good examples of combining state machines with the Command-Based Robot framework. If you have questions, ask away.