http://mbed.org/users/dhamilton31/code/Servo/#dec99beeafee