Restructure control modes and state machine

[zflerner]

Investigate ways to improve expandability of new control modes, like for adding knee proportional control.

Consider improving usability/expandability of the state machine.

[go226]

Modified Two/One_Toe_Sensor state machine to improve readability and expandibility.