Servo move added

This code needs more review and testing. It did not run on the brick.