Completed implementation of Maple's Servo (Not tested on Hardware)