add testroutine to control servo using interrupts