Fix tests and update servoWrite value from 0-255 to 0-180