Milestone 3

Servo control functionality.