Servo functions put on Timer1, jitter eliminated