i hacked the futuba s3003 for continuous rotation.your code is ok! but i think that my code also do this operation.i calculated the delay code with delay spreadsheet.and test the servos by using servo driver circuit.every thing seems ok!
tomorrow i will look the output of pins with osciloscope.and i will try your code!