Massive controller overhaul.
Uncommented world coords.
Fixed state transitions.
Added different positions.  Tweaked PD controllers.  Changed up v_com.  Made it walk!