added new teensy code for C3Pi