Added test code to turn servos from GPIO