Added testing sketch

Used to verify that the servos functioned