Initial Firmware Commit

Basic firmware to control the teensy. Implements all of the movement
functions, awaiting IR.