Added readMicroseconds function and delay calc
Moved servo delay to servo lib
Removed extra delay
Tweaks