Added a int16 cast in firmware to fix Teensy bug.  Switched to ros_arduino_msgs.