Adding states and transitions between driving and walking states.
Fixing errors in StepInto gait.
Adding define macros for log to make serial debugging a bit less messy.