Added tolerance for servo code.
Added tolerance for servo.
Tidied up code.