Code for circuit 4 - Servo