2022 Semester 2
//TODO
- document code more
- factor the code to use functions more
- document a possible Finite State Machine that calls these functions
- implement a watchdog timer to detect if robot is not making progress
== Diagnostic Routine == Possibly create a separate program to act as a diagnostic program to test all functions of the hardward.