We focused mostly on getting the low-level parts of the software working and added the high-level behaviors that we had time left for. We also had the experience of adding significantly more capabilities in code than actually appeared on the final robot. While many things were written and tested in isolation, there wasn't enough time to integrate them. Instead, we focused on the best "simple" state machine.
The most relevant files are:
There were several modules that were developed and tested with success, but could not be integrated into the final project due to time constraints; the ones of note are:
We used simple PID control on the wheels. We found the most robust way to do this was to create a module that contained access functions that set driving direction and speeds. Internal to the module, we calculated the control inputs, including a feed-forward term derived from experimentally measuring the relationship between PWM and rotation rate. It also includes ISRs for the encoders and numerous helper functions, meaning that the user simply has to call something like DriveForward(100) and the robot will drive.
We thought a lot about how to manage keeping track of the game states such as our ball color, recycling bin, etc. We accomplished it by having all of these states tracked in a single module with static variables that could be accessed by any other module that required the information. This kept the state machines decoupled.
The SPI communication with the COMPASS was handled by a hierarchical state machine that handled all communications and kept GameState updated for other state machines to use. It included many helper functions to make this easier.
Color handling largely just called the provided functions for the color sensor. The most important choice made was to not classify a ball based on a single color classification. Instead, we waited for a second consistent measurement because of the large penalty for incorrectly recycling trash.
The cookie guy was a servo that moved balls from the color sensor to the correct bin. We made the decision to often move it to trash because the downside of sitting there with a ball not being classified was significant. The potential loss of points was deemed better than obtaining no points for the rest of the game from a missed classification.