[Iaquinto.Joe]

Originally Posted by apalrd

-SNIP-
-All systems operate using either math algorithms or state-machines. Several use both. State-machines are a highly recommended way to reduce clutter by grouping desires to change state and actions to perform when changing states into a single area of the code, and resulting in a definitive state for this iteration. If the state is enumerated, you can use it to do table lookups for positions, motor command values, etc. and keep calibrations in simple data tables, which is more organized then writing them directly in code.

I agree with the bee. I recommend reprogramming your bot, OP, using Finite State Machines during the offseason. The process very much reduces complexity, and leaves no memory leaks. Diagramming each subsystem of your bot will give you exactly what needs to go into the code.