Servo fix

Still needs the layout bugs fixed, but all working