Added in code to build up servo waypoints to be executed by the motion manager.
(also fixed up stray tabs)