Added test for servoWrite