Convert motor movement to a state-machine, and fix some compile bugs