One basic thing. The Servo library does not control servo position to 4 decimal places. Servo.write(angle) takes an integer for the angle. Trying to use a float with it and subtracting 0.5 is very often not going to do what you're probably expecting.
Steve