Adding Feedback Servo code: commit 1

Im on a mobile device, I cant commit all in 1 go