Teensy code for Servo demonstration