Code Listing
The source code for this project is broken down into state machines, services, and component controls. Each component (servo, drive motor...) has its own initialization function and helper functions to be able to manipulate the component from the state machines. SPI communication is a service that runs independently of the state machines.