Last commit of the servo rewrite
Fixed sketch to test simultaneous interrupts