Change in directory structure

18 files changed, 6723 insertions, 0 deletions

diff --git a/Firmware/Arduino/pidmata3/pidmata3.ino b/Firmware/Arduino/pidmata3/pidmata3.ino
new file mode 100644
index 0000000..01a4710
--- /dev/null
+++ b/Firmware/Arduino/pidmata3/pidmata3.ino
@@ -0,0 +1,904 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please click on the following link
+  to open the list of Firmata client libraries in your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated August 17th, 2017
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+#include <AutoPID.h>
+
+//Comment the line below, "#define PID", if you are not using PID else uncomment it and follow further instructions.
+#define PID
+
+//------------PID values----------------
+// Pin 6  - setPoint  (input value)
+// Pin 5  - feedback (input value)
+// Pin 15 - control (output value)
+
+//If you use real analog pin for input for PID just uncomment next line with #define REAL_INPUT, if use virtual sources comment this line
+//If you use both real will be prefered
+//#define REAL_INPUT
+// This line define analog pin for read values from real sources
+#define AREAD_PIN A1
+
+
+#define PID_OUTPUT_MIN -512.0
+#define PID_OUTPUT_MAX 512.0
+#define PID_KP 15
+#define PID_KI 10
+#define PID_KD 10
+#define PID_BANG_BANG 40
+#define PID_INTERVAL  100
+
+
+
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+
+double currentPoint=0.0, setPoint=0.0, control_value=0.0;
+
+#ifdef PID
+  AutoPID myPID(&currentPoint, &setPoint, &control_value, PID_OUTPUT_MIN, PID_OUTPUT_MAX, PID_KP, PID_KI, PID_KD);
+#endif
+
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+// Forward declare a few functions to avoid compiler errors with older versions
+// of the Arduino IDE.
+void setPinModeCallback(byte, int);
+void reportAnalogCallback(byte analogPin, int value);
+void sysexCallback(byte, byte, byte*);
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+ /*==============================================================================
+ * PID
+ *==============================================================================*/
+
+void update_setPoint(int value)
+{
+  
+   #ifdef REAL_INPUT
+     setPoint=(double)analogRead(AREAD_PIN);
+   #else
+     setPoint=(double)(value-512);
+    #endif
+ }
+
+void update_currentPoint(int value)
+{
+  currentPoint=(double)(value-512); 
+}
+  
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
+          // Disable PWM if pin mode was previously set to PWM.
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW);
+        }
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          //analogWrite(PIN_TO_PWM(pin), value);
+          switch(pin)
+          {
+            case 5:
+            update_currentPoint(value);
+            break;
+            case 6:
+            update_setPoint(value);
+            }
+          
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (argc > 1 && delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(DEFAULT_PWM_RESOLUTION);
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+   
+  #ifdef PID
+   //if temperature is more than 4  below or above setpoint, OUTPUT will be set to min or max respectively
+    myPID.setBangBang(PID_BANG_BANG);
+    //set PID update interval to 
+    myPID.setTimeStep(PID_INTERVAL);
+  #endif
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+    #ifdef PID
+       #ifdef REAL_INPUT
+         update_setPoint(1);
+       #endif    
+       myPID.run(); //call every loop, updates automatically at certain time interval
+    #endif
+  
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          #ifdef PID
+            Firmata.sendAnalog(analogPin, (int)(control_value+512.0));
+          #else
+            Firmata.sendAnalog(analogPin, analogRead(analogPin));
+          #endif
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
diff --git a/Firmware/Tiva C/StandardFirmata.zip b/Firmware/Tiva C/StandardFirmata.zip
new file mode 100644
index 0000000..8907dc5
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata.zip
diff --git a/Firmware/Tiva C/StandardFirmata/Boards.h b/Firmware/Tiva C/StandardFirmata/Boards.h
new file mode 100644
index 0000000..b8aaf3d
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/Boards.h
@@ -0,0 +1,566 @@
+/* Boards.h - Hardware Abstraction Layer for Firmata library */
+
+#ifndef Firmata_Boards_h
+#define Firmata_Boards_h
+
+#include <inttypes.h>
+
+/*#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"	// for digitalRead, digitalWrite, etc
+#else
+#include "WProgram.h"
+#endif*/
+#include "Energia.h"
+
+// Normally Servo.h must be included before Firmata.h (which then includes
+// this file).  If Servo.h wasn't included, this allows the code to still
+// compile, but without support for any Servos.  Hopefully that's what the
+// user intended by not including Servo.h
+#ifndef MAX_SERVOS
+#define MAX_SERVOS 8
+#endif
+
+/*
+    Firmata Hardware Abstraction Layer
+
+Firmata is built on top of the hardware abstraction functions of Arduino,
+specifically digitalWrite, digitalRead, analogWrite, analogRead, and 
+pinMode.  While these functions offer simple integer pin numbers, Firmata
+needs more information than is provided by Arduino.  This file provides
+all other hardware specific details.  To make Firmata support a new board,
+only this file should require editing.
+
+The key concept is every "pin" implemented by Firmata may be mapped to
+any pin as implemented by Arduino.  Usually a simple 1-to-1 mapping is
+best, but such mapping should not be assumed.  This hardware abstraction
+layer allows Firmata to implement any number of pins which map onto the
+Arduino implemented pins in almost any arbitrary way.
+
+
+General Constants:
+
+These constants provide basic information Firmata requires.
+
+TOTAL_PINS: The total number of pins Firmata implemented by Firmata.
+    Usually this will match the number of pins the Arduino functions
+    implement, including any pins pins capable of analog or digital.
+    However, Firmata may implement any number of pins.  For example,
+    on Arduino Mini with 8 analog inputs, 6 of these may be used
+    for digital functions, and 2 are analog only.  On such boards,
+    Firmata can implement more pins than Arduino's pinMode()
+    function, in order to accommodate those special pins.  The
+    Firmata protocol supports a maximum of 128 pins, so this
+    constant must not exceed 128.
+
+TOTAL_ANALOG_PINS: The total number of analog input pins implemented.
+    The Firmata protocol allows up to 16 analog inputs, accessed
+    using offsets 0 to 15.  Because Firmata presents the analog
+    inputs using different offsets than the actual pin numbers
+    (a legacy of Arduino's analogRead function, and the way the
+    analog input capable pins are physically labeled on all
+    Arduino boards), the total number of analog input signals
+    must be specified.  16 is the maximum.
+
+VERSION_BLINK_PIN: When Firmata starts up, it will blink the version
+    number.  This constant is the Arduino pin number where a
+    LED is connected.
+
+
+Pin Mapping Macros:
+
+These macros provide the mapping between pins as implemented by
+Firmata protocol and the actual pin numbers used by the Arduino
+functions.  Even though such mappings are often simple, pin
+numbers received by Firmata protocol should always be used as
+input to these macros, and the result of the macro should be
+used with with any Arduino function.
+
+When Firmata is extended to support a new pin mode or feature,
+a pair of macros should be added and used for all hardware
+access.  For simple 1:1 mapping, these macros add no actual
+overhead, yet their consistent use allows source code which
+uses them consistently to be easily adapted to all other boards
+with different requirements.
+
+IS_PIN_XXXX(pin): The IS_PIN macros resolve to true or non-zero
+    if a pin as implemented by Firmata corresponds to a pin
+    that actually implements the named feature.
+
+PIN_TO_XXXX(pin): The PIN_TO macros translate pin numbers as
+    implemented by Firmata to the pin numbers needed as inputs
+    to the Arduino functions.  The corresponding IS_PIN macro
+    should always be tested before using a PIN_TO macro, so
+    these macros only need to handle valid Firmata pin
+    numbers for the named feature.
+
+
+Port Access Inline Funtions:
+
+For efficiency, Firmata protocol provides access to digital
+input and output pins grouped by 8 bit ports.  When these
+groups of 8 correspond to actual 8 bit ports as implemented
+by the hardware, these inline functions can provide high
+speed direct port access.  Otherwise, a default implementation
+using 8 calls to digitalWrite or digitalRead is used.
+
+When porting Firmata to a new board, it is recommended to
+use the default functions first and focus only on the constants
+and macros above.  When those are working, if optimized port
+access is desired, these inline functions may be extended.
+The recommended approach defines a symbol indicating which
+optimization to use, and then conditional complication is
+used within these functions.
+
+readPort(port, bitmask):  Read an 8 bit port, returning the value.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   bitmask: The actual pins to read, indicated by 1 bits.
+
+writePort(port, value, bitmask):  Write an 8 bit port.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   value:   The 8 bit value to write
+   bitmask: The actual pins to write, indicated by 1 bits.
+*/
+
+/*==============================================================================
+ * Board Specific Configuration
+ *============================================================================*/
+
+#ifndef digitalPinHasPWM
+#define digitalPinHasPWM(p)     IS_PIN_DIGITAL(p)
+#endif
+
+// Arduino Duemilanove, Diecimila, and NG
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__)
+#if defined(NUM_ANALOG_INPUTS) && NUM_ANALOG_INPUTS == 6
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#else
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              22 // 14 digital + 8 analog
+#endif
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) < 14 + TOTAL_ANALOG_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Wiring (and board)
+#elif defined(WIRING)
+#define VERSION_BLINK_PIN       WLED
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= FIRST_ANALOG_PIN && (p) < (FIRST_ANALOG_PIN+TOTAL_ANALOG_PINS))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - FIRST_ANALOG_PIN)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p) 
+
+
+// old Arduinos
+#elif defined(__AVR_ATmega8__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Arduino Mega
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_PINS              70 // 54 digital + 16 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Arduino DUE
+#elif defined(__SAM3X8E__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              66 // 54 digital + 12 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21) // 70 71
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Teensy 1.0
+#elif defined(__AVR_AT90USB162__)
+#define TOTAL_ANALOG_PINS       0
+#define TOTAL_PINS              21 // 21 digital + no analog
+#define VERSION_BLINK_PIN       6
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        (0)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           (0)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (0)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 2.0
+#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              25 // 11 digital + 12 analog
+#define VERSION_BLINK_PIN       11
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 11 && (p) <= 22)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 5 || (p) == 6)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p)<22)?21-(p):11)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 3.0
+#elif defined(__MK20DX128__)
+#define TOTAL_ANALOG_PINS       14
+#define TOTAL_PINS              38 // 24 digital + 10 analog-digital + 4 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 34)
+#define IS_PIN_ANALOG(p)        (((p) >= 14 && (p) <= 23) || ((p) >= 34 && (p) <= 38))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p)<=23)?(p)-14:(p)-24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p) 
+
+
+// Teensy++ 1.0 and 2.0
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              46 // 38 digital + 8 analog
+#define VERSION_BLINK_PIN       6
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 38 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 0 || (p) == 1)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 38)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Leonardo
+#elif defined(__AVR_ATmega32U4__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              30 // 14 digital + 12 analog + 4 SPI (D14-D17 on ISP header)
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 18 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 13)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p) - 18
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)  
+
+
+// Sanguino
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              32 // 24 digital + 8 analog
+#define VERSION_BLINK_PIN       0
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Illuminato
+#elif defined(__AVR_ATmega645__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              42 // 36 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 36 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 4 || (p) == 5)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 36)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+// MSP430F5529
+#elif defined(__MSP430F5529__)
+#define TOTAL_ANALOG_PINS       13
+#define TOTAL_PINS              45
+#define VERSION_BLINK_PIN       44
+#define IS_PIN_DIGITAL(p)       ((p)>=2 && (p) <= 44 && (p)!=16 && (p)!=20 && (p)!=21 && (p)!=22 )
+#define IS_PIN_ANALOG(p)        ((p) == 2 || (p) == 6 || \
+								 (p)==23 || (p)==24 || (p)==25 || (p)==26 || (p)==27 || (p)==28 )
+#define IS_PIN_PWM(p)           ( (p)==12 || (p)==19 || ((p)>=35 && (p)<=40) )
+#define IS_PIN_SERVO(p)         IS_PIN_PWM(p)
+#define IS_PIN_I2C(p)           ((p) == 14 || (p) == 15)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p)>=23 && (p)<=27)?((p)-23):(((p)==2)?(5):((p)==28?(12):((p)==6?6:10) ))
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+// MSP430FR5969 FRAM
+#elif defined(__MSP430FR5969__)
+#define TOTAL_ANALOG_PINS       13
+#define TOTAL_PINS              29
+#define VERSION_BLINK_PIN       26
+#define IS_PIN_DIGITAL(p)       ( ((p)>=2 && (p) < 20 && (p)!=16 && (p)!=17) || (p)==25 || (p)==26 )
+#define IS_PIN_ANALOG(p)        ((p) == 2 || (p) == 5 || (p)==11 || (p)==12 || (p)==13 || (p)==18 || (p)==19)
+#define IS_PIN_PWM(p)           ((p)>=3 && (p) < 20 && (p)!=16 && (p)!=17 && (p)!=5)
+#define IS_PIN_SERVO(p)         IS_PIN_PWM(p)
+#define IS_PIN_I2C(p)           ((p) == 14 || (p) == 15)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p)==2?10:\
+								 (p)==5?11:\
+								 (p)==11?3:\
+								 (p)==12?4:\
+								 (p)==13?5:\
+								 (p)==18?12:2)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+// MSP430FR5739
+#elif defined(__MSP430FR5739__)
+// Baudrate only up to 9600, currently does NOT support Firmata
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              28 // 38 digital + 8 analog, but should be multiple of 8 for NodeJS Firmata
+#define VERSION_BLINK_PIN       26
+#define IS_PIN_DIGITAL(p)       ((p)>=2 && (p) < 24 && (p)!=24)
+#define IS_PIN_ANALOG(p)        ((p)>=13 && (p)<=22)
+#define IS_PIN_PWM(p)           ((p)>=5 && (p)<= 15) || ((p)>=20 && (p)<=22)
+#define IS_PIN_SERVO(p)         IS_PIN_PWM(p)
+#define IS_PIN_I2C(p)           ((p) == 11 || (p) == 12)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (22-(p))
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+// MSP430G2553 Value Line
+#elif defined(__MSP430G2553__)
+// Baudrate only up to 9600, currently does NOT support Firmata
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              20 
+#define VERSION_BLINK_PIN       2
+#define IS_PIN_DIGITAL(p)       ((p) >= 3 && (p) < TOTAL_PINS && (p)!=16)
+#define IS_PIN_ANALOG(p)        ((p) >= 3 && (p) < 8)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         IS_PIN_PWM(p)
+#define IS_PIN_I2C(p)           ((p) == 14 || (p) == 15)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p))
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+//#define TOTAL_PORTS				2
+
+// Tiva C TM4C123G - EK-TM4C123GXL and Stellaris LM4F120H5QR
+#elif (defined(__TM4C123GH6PM__) || defined(__LM4F120H5QR__))
+#define TOTAL_ANALOG_PINS       13
+#define TOTAL_PINS              41
+#define VERSION_BLINK_PIN       40
+#define IS_PIN_DIGITAL(p)       ((p)>=2 && (p)<=TOTAL_PINS && (p)!=16 && (p)!=20 && (p)!=21 && (p)!=22)
+#define IS_PIN_ANALOG(p)        ((p)>=23 && (p)<=29) || (p==2) || (p==5) || (p==6) || (p==7) || (p==18)
+#define IS_PIN_PWM(p)           ((p)==2 || (p)==3 || (p)==4 || (p)==7) || \
+								 (p)==14 || (p)==15 || (p)==17 || (p)==19 || \
+								 ((p)>=23 && (p)<=26) || (p)==30 || \
+								 ((p)>=31 && (p)<=40)
+#define IS_PIN_SERVO(p)         (IS_PIN_PWM(p))
+#define IS_PIN_I2C(p)           ((p) == 19 || (p) == 38)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p)>=23 && (p)<=26)?(30-(p)):\
+								 ((p)>=27&&(p)<=29)?(29-p):\
+								 ((p)==18)?(3):\
+								 ((p)==2)?(11):\
+								 ((p)==5)?(9):\
+								 ((p)==6)?(8):(10)
+
+/*#define PIN_TO_ANALOG(p)        ((p)>=23 && (p)<=26)?(p):\
+								 ((p)>=27&&(p)<=29)?(29-p):\
+								 ((p)==18)?(3):\
+								 ((p)==2)?(11):\
+								 ((p)==5)?(9):\
+								 ((p)==6)?(8):(10)
+//#define PIN_TO_ANALOG(p)        (p)
+*/
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+// Tiva C TM4C1294 - EK-TM4C1294XL
+#elif defined(__TM4C1294NCPDT__)
+#define TOTAL_ANALOG_PINS       20
+#define TOTAL_PINS              85
+#define VERSION_BLINK_PIN       81 // LED1
+#define IS_PIN_DIGITAL(p)       ((p)>=2 && (p)<=TOTAL_PINS && (p)!=16 && (p)!=20 && (p)!=21 && (p)!=22)
+#define IS_PIN_ANALOG(p)        ((p)==2 || (p)==6 || (p)==7 || (p)==14 || (p)==15 || ((p)>=23 && (p)<=27) || \
+								(p)==42 || (p)==45 || (p)==46 || ((p)>=63 && (p)<=68) )
+#define IS_PIN_PWM(p)           ((p)== 7 || (p)== 9 || (p)==10 || (p)==14 || (p)==15 || (p)==19 || \
+								 ((p)>=27 && (p)<=30) || (p)==35 || (p)==36 || \
+								 (p)==42 || (p)==45 || (p)==46 || (p)==51 || (p)==57 || (p)==59 || \
+								 (p)==69 || (p)==70 || (p)==75 || (p)==76 || (p)==77 )
+#define IS_PIN_SERVO(p)         (IS_PIN_PWM(p) && (p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p)== 9 || (p)==10 || (p)==34 || (p)==35 || (p)==37 || \
+								 (p)==49 || (p)==50 || (p)==80 )
+#define PIN_TO_DIGITAL(p)       (p)
+// Only 16 analog pins are allowed at a time. Default A0-A15. 
+// To enable A16-A19, other analog pins need to be replaced
+#define PIN_TO_ANALOG(p)        (((p)==2)?(9):\
+								 ((p)==6)?(8):\
+								 ((p)==7)?(12):\
+								 ((p)==14)?(15):\
+								 ((p)==15)?(14):\
+								 ((p)>=23&&(p)<=26)?(26-(p)):\
+								 ((p)==27)?(4):\
+								 ((p)==42)?(13):\
+								 ((p)==45)?(7):\
+								 ((p)==46)?(6):\
+								 ((p)==63)?(10):\
+								 ((p)==64)?(11):\
+								 ((p)==65)?(16):\
+								 ((p)==66)?(17):\
+								 ((p)-49))
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+//#define TOTAL_PORTS				10
+
+
+
+// anything else
+#else
+#error "Please edit Boards.h with a hardware abstraction for this board"
+#endif
+
+
+// as long this is not defined for all boards:
+#ifndef IS_PIN_SPI
+#define IS_PIN_SPI(p)           0
+#endif
+
+/*==============================================================================
+ * readPort() - Read an 8 bit port
+ *============================================================================*/
+
+static inline unsigned char readPort(byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char readPort(byte port, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) return (PIND & 0xFC) & bitmask; // ignore Rx/Tx 0/1
+	if (port == 1) return ((PINB & 0x3F) | ((PINC & 0x03) << 6)) & bitmask;
+	if (port == 2) return ((PINC & 0x3C) >> 2) & bitmask;
+	return 0;
+#else
+	unsigned char out=0, pin=port*8;
+	if (IS_PIN_DIGITAL(pin+0) && (bitmask & 0x01) && digitalRead(PIN_TO_DIGITAL(pin+0))) out |= 0x01;
+	if (IS_PIN_DIGITAL(pin+1) && (bitmask & 0x02) && digitalRead(PIN_TO_DIGITAL(pin+1))) out |= 0x02;
+	if (IS_PIN_DIGITAL(pin+2) && (bitmask & 0x04) && digitalRead(PIN_TO_DIGITAL(pin+2))) out |= 0x04;
+	if (IS_PIN_DIGITAL(pin+3) && (bitmask & 0x08) && digitalRead(PIN_TO_DIGITAL(pin+3))) out |= 0x08;
+	if (IS_PIN_DIGITAL(pin+4) && (bitmask & 0x10) && digitalRead(PIN_TO_DIGITAL(pin+4))) out |= 0x10;
+	if (IS_PIN_DIGITAL(pin+5) && (bitmask & 0x20) && digitalRead(PIN_TO_DIGITAL(pin+5))) out |= 0x20;
+	if (IS_PIN_DIGITAL(pin+6) && (bitmask & 0x40) && digitalRead(PIN_TO_DIGITAL(pin+6))) out |= 0x40;
+	if (IS_PIN_DIGITAL(pin+7) && (bitmask & 0x80) && digitalRead(PIN_TO_DIGITAL(pin+7))) out |= 0x80;
+	return out;
+#endif
+}
+
+/*==============================================================================
+ * writePort() - Write an 8 bit port, only touch pins specified by a bitmask
+ *============================================================================*/
+
+static inline unsigned char writePort(byte, byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char writePort(byte port, byte value, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) {
+		bitmask = bitmask & 0xFC;  // do not touch Tx & Rx pins
+		byte valD = value & bitmask;
+		byte maskD = ~bitmask;
+		cli();
+		PORTD = (PORTD & maskD) | valD;
+		sei();
+	} else if (port == 1) {
+		byte valB = (value & bitmask) & 0x3F;
+		byte valC = (value & bitmask) >> 6;
+		byte maskB = ~(bitmask & 0x3F);
+		byte maskC = ~((bitmask & 0xC0) >> 6);
+		cli();
+		PORTB = (PORTB & maskB) | valB;
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	} else if (port == 2) {
+		bitmask = bitmask & 0x0F;
+		byte valC = (value & bitmask) << 2;
+		byte maskC = ~(bitmask << 2);
+		cli();
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	}
+#else
+	byte pin=port*8;
+	if ((bitmask & 0x01)) digitalWrite(PIN_TO_DIGITAL(pin+0), (value & 0x01));
+	if ((bitmask & 0x02)) digitalWrite(PIN_TO_DIGITAL(pin+1), (value & 0x02));
+	if ((bitmask & 0x04)) digitalWrite(PIN_TO_DIGITAL(pin+2), (value & 0x04));
+	if ((bitmask & 0x08)) digitalWrite(PIN_TO_DIGITAL(pin+3), (value & 0x08));
+	if ((bitmask & 0x10)) digitalWrite(PIN_TO_DIGITAL(pin+4), (value & 0x10));
+	if ((bitmask & 0x20)) digitalWrite(PIN_TO_DIGITAL(pin+5), (value & 0x20));
+	if ((bitmask & 0x40)) digitalWrite(PIN_TO_DIGITAL(pin+6), (value & 0x40));
+	if ((bitmask & 0x80)) digitalWrite(PIN_TO_DIGITAL(pin+7), (value & 0x80));
+#endif
+}
+
+
+
+
+#ifndef TOTAL_PORTS
+#define TOTAL_PORTS             ((TOTAL_PINS + 7) / 8)
+#endif
+
+
+#endif /* Firmata_Boards_h */
+
diff --git a/Firmware/Tiva C/StandardFirmata/Firmata.cpp b/Firmware/Tiva C/StandardFirmata/Firmata.cpp
new file mode 100644
index 0000000..8e48bc7
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/Firmata.cpp
@@ -0,0 +1,554 @@
+/*
+  Firmata.cpp - Firmata library v2.5.6 - 2017-03-18
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "Firmata.h"
+#include "HardwareSerial.h"
+
+#include <string.h>
+#include <stdlib.h>
+
+using namespace firmata;
+
+//******************************************************************************
+//* Static Members
+//******************************************************************************
+// make one instance for the user to use
+FirmataClass Firmata;
+
+/* callback functions */
+callbackFunction FirmataClass::currentAnalogCallback = (callbackFunction)NULL;
+callbackFunction FirmataClass::currentDigitalCallback = (callbackFunction)NULL;
+callbackFunction FirmataClass::currentPinModeCallback = (callbackFunction)NULL;
+callbackFunction FirmataClass::currentPinValueCallback = (callbackFunction)NULL;
+callbackFunction FirmataClass::currentReportAnalogCallback = (callbackFunction)NULL;
+callbackFunction FirmataClass::currentReportDigitalCallback = (callbackFunction)NULL;
+stringCallbackFunction FirmataClass::currentStringCallback = (stringCallbackFunction)NULL;
+sysexCallbackFunction FirmataClass::currentSysexCallback = (sysexCallbackFunction)NULL;
+systemCallbackFunction FirmataClass::currentSystemResetCallback = (systemCallbackFunction)NULL;
+
+//******************************************************************************
+//* Support Functions
+//******************************************************************************
+
+/**
+ * Split a 16-bit byte into two 7-bit values and write each value.
+ * @param value The 16-bit value to be split and written separately.
+ */
+void FirmataClass::sendValueAsTwo7bitBytes(int value)
+{
+  marshaller.encodeByteStream(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
+}
+
+/**
+ * A helper method to write the beginning of a Sysex message transmission.
+ */
+void FirmataClass::startSysex(void)
+{
+  FirmataStream->write(START_SYSEX);
+}
+
+/**
+ * A helper method to write the end of a Sysex message transmission.
+ */
+void FirmataClass::endSysex(void)
+{
+  FirmataStream->write(END_SYSEX);
+}
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+/**
+ * The Firmata class.
+ * An instance named "Firmata" is created automatically for the user.
+ */
+FirmataClass::FirmataClass()
+:
+  parser(FirmataParser(parserBuffer, MAX_DATA_BYTES))
+{
+  firmwareVersionCount = 0;
+  firmwareVersionVector = 0;
+  blinkVersionDisabled = false;
+
+  // Establish callback translation to parser callbacks
+  parser.attach(ANALOG_MESSAGE, (FirmataParser::callbackFunction)staticAnalogCallback, (void *)NULL);
+  parser.attach(DIGITAL_MESSAGE, (FirmataParser::callbackFunction)staticDigitalCallback, (void *)NULL);
+  parser.attach(REPORT_ANALOG, (FirmataParser::callbackFunction)staticReportAnalogCallback, (void *)NULL);
+  parser.attach(REPORT_DIGITAL, (FirmataParser::callbackFunction)staticReportDigitalCallback, (void *)NULL);
+  parser.attach(SET_PIN_MODE, (FirmataParser::callbackFunction)staticPinModeCallback, (void *)NULL);
+  parser.attach(SET_DIGITAL_PIN_VALUE, (FirmataParser::callbackFunction)staticPinValueCallback, (void *)NULL);
+  parser.attach(STRING_DATA, (FirmataParser::stringCallbackFunction)staticStringCallback, (void *)NULL);
+  parser.attach(START_SYSEX, (FirmataParser::sysexCallbackFunction)staticSysexCallback, (void *)NULL);
+  parser.attach(REPORT_FIRMWARE, (FirmataParser::versionCallbackFunction)staticReportFirmwareCallback, this);
+  parser.attach(REPORT_VERSION, (FirmataParser::systemCallbackFunction)staticReportVersionCallback, this);
+  parser.attach(SYSTEM_RESET, (FirmataParser::systemCallbackFunction)staticSystemResetCallback, (void *)NULL);
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+/**
+ * Initialize the default Serial transport at the default baud of 57600.
+ */
+void FirmataClass::begin(void)
+{
+  begin(57600);
+}
+
+/**
+ * Initialize the default Serial transport and override the default baud.
+ * Sends the protocol version to the host application followed by the firmware version and name.
+ * blinkVersion is also called. To skip the call to blinkVersion, call Firmata.disableBlinkVersion()
+ * before calling Firmata.begin(baud).
+ * @param speed The baud to use. 57600 baud is the default value.
+ */
+void FirmataClass::begin(long speed)
+{
+  Serial.begin(speed);
+  blinkVersion();
+  begin(Serial);
+}
+
+/**
+ * Reassign the Firmata stream transport.
+ * @param s A reference to the Stream transport object. This can be any type of
+ * transport that implements the Stream interface. Some examples include Ethernet, WiFi
+ * and other UARTs on the board (Serial1, Serial2, etc).
+ */
+void FirmataClass::begin(Stream &s)
+{
+  FirmataStream = &s;
+  marshaller.begin(s);
+  // do not call blinkVersion() here because some hardware such as the
+  // Ethernet shield use pin 13
+  printVersion();         // send the protocol version
+  printFirmwareVersion(); // send the firmware name and version
+}
+
+/**
+ * Send the Firmata protocol version to the Firmata host application.
+ */
+void FirmataClass::printVersion(void)
+{
+  marshaller.sendVersion(FIRMATA_PROTOCOL_MAJOR_VERSION, FIRMATA_PROTOCOL_MINOR_VERSION);
+}
+
+/**
+ * Blink the Firmata protocol version to the onboard LEDs (if the board has an onboard LED).
+ * If VERSION_BLINK_PIN is not defined in Boards.h for a particular board, then this method
+ * does nothing.
+ * The first series of flashes indicates the firmware major version (2 flashes = 2).
+ * The second series of flashes indicates the firmware minor version (5 flashes = 5).
+ */
+void FirmataClass::blinkVersion(void)
+{
+#if defined(VERSION_BLINK_PIN)
+  if (blinkVersionDisabled) return;
+  // flash the pin with the protocol version
+  pinMode(VERSION_BLINK_PIN, OUTPUT);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MAJOR_VERSION, 40, 210);
+  delay(250);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MINOR_VERSION, 40, 210);
+  delay(125);
+#endif
+}
+
+/**
+ * Provides a means to disable the version blink sequence on the onboard LED, trimming startup
+ * time by a couple of seconds.
+ * Call this before Firmata.begin(). It only applies when using the default Serial transport.
+ */
+void FirmataClass::disableBlinkVersion()
+{
+  blinkVersionDisabled = true;
+}
+
+/**
+ * Sends the firmware name and version to the Firmata host application. The major and minor version
+ * numbers are the first 2 bytes in the message. The following bytes are the characters of the
+ * firmware name.
+ */
+void FirmataClass::printFirmwareVersion(void)
+{
+  if (firmwareVersionCount) { // make sure that the name has been set before reporting
+    marshaller.sendFirmwareVersion(static_cast<uint8_t>(firmwareVersionVector[0]), static_cast<uint8_t>(firmwareVersionVector[1]), (firmwareVersionCount - 2), reinterpret_cast<uint8_t *>(&firmwareVersionVector[2]));
+  }
+}
+
+/**
+ * Sets the name and version of the firmware. This is not the same version as the Firmata protocol
+ * (although at times the firmware version and protocol version may be the same number).
+ * @param name A pointer to the name char array
+ * @param major The major version number
+ * @param minor The minor version number
+ */
+void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte minor)
+{
+  const char *firmwareName;
+  const char *extension;
+
+  // parse out ".cpp" and "applet/" that comes from using __FILE__
+  extension = strstr(name, ".cpp");
+  firmwareName = strrchr(name, '/');
+
+  if (!firmwareName) {
+    // windows
+    firmwareName = strrchr(name, '\\');
+  }
+  if (!firmwareName) {
+    // user passed firmware name
+    firmwareName = name;
+  } else {
+    firmwareName ++;
+  }
+
+  if (!extension) {
+    firmwareVersionCount = strlen(firmwareName) + 2;
+  } else {
+    firmwareVersionCount = extension - firmwareName + 2;
+  }
+
+  // in case anyone calls setFirmwareNameAndVersion more than once
+  free(firmwareVersionVector);
+
+  firmwareVersionVector = (byte *) malloc(firmwareVersionCount + 1);
+  firmwareVersionVector[firmwareVersionCount] = 0;
+  firmwareVersionVector[0] = major;
+  firmwareVersionVector[1] = minor;
+  strncpy((char *)firmwareVersionVector + 2, firmwareName, firmwareVersionCount - 2);
+}
+
+//------------------------------------------------------------------------------
+// Serial Receive Handling
+
+/**
+ * A wrapper for Stream::available()
+ * @return The number of bytes remaining in the input stream buffer.
+ */
+int FirmataClass::available(void)
+{
+  return FirmataStream->available();
+}
+
+/**
+ * Read a single int from the input stream. If the value is not = -1, pass it on to parse(byte)
+ */
+void FirmataClass::processInput(void)
+{
+  int inputData = FirmataStream->read(); // this is 'int' to handle -1 when no data
+  if (inputData != -1) {
+    parser.parse(inputData);
+  }
+}
+
+/**
+ * Parse data from the input stream.
+ * @param inputData A single byte to be added to the parser.
+ */
+void FirmataClass::parse(byte inputData)
+{
+    parser.parse(inputData);
+}
+
+/**
+ * @return Returns true if the parser is actively parsing data.
+ */
+boolean FirmataClass::isParsingMessage(void)
+{
+  return parser.isParsingMessage();
+}
+
+//------------------------------------------------------------------------------
+// Output Stream Handling
+
+/**
+ * Send an analog message to the Firmata host application. The range of pins is limited to [0..15]
+ * when using the ANALOG_MESSAGE. The maximum value of the ANALOG_MESSAGE is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin to send the value of (limited to pins 0 - 15).
+ * @param value The value of the analog pin (0 - 1024 for 10-bit analog, 0 - 4096 for 12-bit, etc).
+ * The maximum value is 14-bits (16384).
+ */
+void FirmataClass::sendAnalog(byte pin, int value)
+{
+  marshaller.sendAnalog(pin, value);
+}
+
+/* (intentionally left out asterix here)
+ * STUB - NOT IMPLEMENTED
+ * Send a single digital pin value to the Firmata host application.
+ * @param pin The digital pin to send the value of.
+ * @param value The value of the pin.
+ */
+void FirmataClass::sendDigital(byte pin, int value)
+{
+  (void)pin;
+  (void)value;
+  /* TODO add single pin digital messages to the protocol, this needs to
+   * track the last digital data sent so that it can be sure to change just
+   * one bit in the packet.  This is complicated by the fact that the
+   * numbering of the pins will probably differ on Arduino, Wiring, and
+   * other boards.
+   */
+
+  // TODO: the digital message should not be sent on the serial port every
+  // time sendDigital() is called.  Instead, it should add it to an int
+  // which will be sent on a schedule.  If a pin changes more than once
+  // before the digital message is sent on the serial port, it should send a
+  // digital message for each change.
+
+  //    if(value == 0)
+  //        sendDigitalPortPair();
+}
+
+
+/**
+ * Send an 8-bit port in a single digital message (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number to send. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ * @param portData The value of the port. The value of each pin in the port is represented by a bit.
+ */
+void FirmataClass::sendDigitalPort(byte portNumber, int portData)
+{
+  marshaller.sendDigitalPort(portNumber, portData);
+}
+
+/**
+ * Send a sysex message where all values after the command byte are packet as 2 7-bit bytes
+ * (this is not always the case so this function is not always used to send sysex messages).
+ * @param command The sysex command byte.
+ * @param bytec The number of data bytes in the message (excludes start, command and end bytes).
+ * @param bytev A pointer to the array of data bytes to send in the message.
+ */
+void FirmataClass::sendSysex(byte command, byte bytec, byte *bytev)
+{
+  marshaller.sendSysex(command, bytec, bytev);
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param command Must be STRING_DATA
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(byte command, const char *string)
+{
+  if (command == STRING_DATA) {
+    marshaller.sendString(string);
+  }
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(const char *string)
+{
+  marshaller.sendString(string);
+}
+
+/**
+ * A wrapper for Stream::available().
+ * Write a single byte to the output stream.
+ * @param c The byte to be written.
+ */
+void FirmataClass::write(byte c)
+{
+  FirmataStream->write(c);
+}
+
+/**
+ * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
+ * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ */
+void FirmataClass::attach(uint8_t command, ::callbackFunction newFunction)
+{
+  switch (command) {
+    case ANALOG_MESSAGE:
+      currentAnalogCallback = newFunction;
+      break;
+    case DIGITAL_MESSAGE:
+      currentDigitalCallback = newFunction;
+      break;
+    case REPORT_ANALOG:
+      currentReportAnalogCallback = newFunction;
+      break;
+    case REPORT_DIGITAL:
+      currentReportDigitalCallback = newFunction;
+      break;
+    case SET_PIN_MODE:
+      currentPinModeCallback = newFunction;
+      break;
+    case SET_DIGITAL_PIN_VALUE:
+      currentPinValueCallback = newFunction;
+      break;
+  }
+}
+
+/**
+ * Attach a callback function for the SYSTEM_RESET command.
+ * @param command Must be set to SYSTEM_RESET or it will be ignored.
+ * @param newFunction A reference to the system reset callback function to attach.
+ */
+void FirmataClass::attach(uint8_t command, systemCallbackFunction newFunction)
+{
+  switch (command) {
+    case SYSTEM_RESET:
+      currentSystemResetCallback = newFunction;
+      break;
+  }
+}
+
+/**
+ * Attach a callback function for the STRING_DATA command.
+ * @param command Must be set to STRING_DATA or it will be ignored.
+ * @param newFunction A reference to the string callback function to attach.
+ */
+void FirmataClass::attach(uint8_t command, stringCallbackFunction newFunction)
+{
+  switch (command) {
+    case STRING_DATA:
+      currentStringCallback = newFunction;
+      break;
+  }
+}
+
+/**
+ * Attach a generic sysex callback function to sysex command.
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the sysex callback function to attach.
+ */
+void FirmataClass::attach(uint8_t command, sysexCallbackFunction newFunction)
+{
+  (void)command;
+  currentSysexCallback = newFunction;
+}
+
+/**
+ * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
+ * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
+ * @param command The ID of the command to detatch the callback function from.
+ */
+void FirmataClass::detach(uint8_t command)
+{
+  switch (command) {
+    case SYSTEM_RESET:
+      attach(command, (systemCallbackFunction)NULL);
+      break;
+    case STRING_DATA:
+      attach(command, (stringCallbackFunction)NULL);
+      break;
+    case START_SYSEX:
+      attach(command, (sysexCallbackFunction)NULL);
+      break;
+    default:
+      attach(command, (callbackFunction)NULL);
+      break;
+  }
+}
+
+/**
+ * @param pin The pin to get the configuration of.
+ * @return The configuration of the specified pin.
+ */
+byte FirmataClass::getPinMode(byte pin)
+{
+  return pinConfig[pin];
+}
+
+/**
+ * Set the pin mode/configuration. The pin configuration (or mode) in Firmata represents the
+ * current function of the pin. Examples are digital input or output, analog input, pwm, i2c,
+ * serial (uart), etc.
+ * @param pin The pin to configure.
+ * @param config The configuration value for the specified pin.
+ */
+void FirmataClass::setPinMode(byte pin, byte config)
+{
+  if (pinConfig[pin] == PIN_MODE_IGNORE)
+    return;
+
+  pinConfig[pin] = config;
+}
+
+/**
+ * @param pin The pin to get the state of.
+ * @return The state of the specified pin.
+ */
+int FirmataClass::getPinState(byte pin)
+{
+  return pinState[pin];
+}
+
+/**
+ * Set the pin state. The pin state of an output pin is the pin value. The state of an
+ * input pin is 0, unless the pin has it's internal pull up resistor enabled, then the value is 1.
+ * @param pin The pin to set the state of
+ * @param state Set the state of the specified pin
+ */
+void FirmataClass::setPinState(byte pin, int state)
+{
+  pinState[pin] = state;
+}
+
+// sysex callbacks
+/*
+ * this is too complicated for analogReceive, but maybe for Sysex?
+ void FirmataClass::attachSysex(sysexFunction newFunction)
+ {
+ byte i;
+ byte tmpCount = analogReceiveFunctionCount;
+ analogReceiveFunction* tmpArray = analogReceiveFunctionArray;
+ analogReceiveFunctionCount++;
+ analogReceiveFunctionArray = (analogReceiveFunction*) calloc(analogReceiveFunctionCount, sizeof(analogReceiveFunction));
+ for(i = 0; i < tmpCount; i++) {
+ analogReceiveFunctionArray[i] = tmpArray[i];
+ }
+ analogReceiveFunctionArray[tmpCount] = newFunction;
+ free(tmpArray);
+ }
+*/
+
+//******************************************************************************
+//* Private Methods
+//******************************************************************************
+
+/**
+ * Flashing the pin for the version number
+ * @private
+ * @param pin The pin the LED is attached to.
+ * @param count The number of times to flash the LED.
+ * @param onInterval The number of milliseconds for the LED to be ON during each interval.
+ * @param offInterval The number of milliseconds for the LED to be OFF during each interval.
+ */
+void FirmataClass::strobeBlinkPin(byte pin, int count, int onInterval, int offInterval)
+{
+  byte i;
+  for (i = 0; i < count; i++) {
+    delay(offInterval);
+    digitalWrite(pin, HIGH);
+    delay(onInterval);
+    digitalWrite(pin, LOW);
+  }
+}
+
diff --git a/Firmware/Tiva C/StandardFirmata/Firmata.h b/Firmware/Tiva C/StandardFirmata/Firmata.h
new file mode 100644
index 0000000..ec95165
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/Firmata.h
@@ -0,0 +1,180 @@
+/*
+  Firmata.h - Firmata library v2.5.6 - 2017-03-18
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef Firmata_h
+#define Firmata_h
+
+#include "Boards.h"  /* Hardware Abstraction Layer + Wiring/Arduino */
+#include "FirmataDefines.h"
+#include "FirmataMarshaller.h"
+#include "FirmataParser.h"
+
+/* DEPRECATED as of Firmata v2.5.1. As of 2.5.1 there are separate version numbers for
+ * the protocol version and the firmware version.
+ */
+#define FIRMATA_MAJOR_VERSION           2 // same as FIRMATA_PROTOCOL_MAJOR_VERSION
+#define FIRMATA_MINOR_VERSION           5 // same as FIRMATA_PROTOCOL_MINOR_VERSION
+#define FIRMATA_BUGFIX_VERSION          1 // same as FIRMATA_PROTOCOL_BUGFIX_VERSION
+
+// extended command set using sysex (0-127/0x00-0x7F)
+/* 0x00-0x0F reserved for user-defined commands */
+// these are DEPRECATED to make the naming more consistent
+#define FIRMATA_STRING          0x71 // same as STRING_DATA
+#define SYSEX_I2C_REQUEST       0x76 // same as I2C_REQUEST
+#define SYSEX_I2C_REPLY         0x77 // same as I2C_REPLY
+#define SYSEX_SAMPLING_INTERVAL 0x7A // same as SAMPLING_INTERVAL
+
+// pin modes
+//#define INPUT                 0x00 // defined in Arduino.h
+//#define OUTPUT                0x01 // defined in Arduino.h
+// DEPRECATED as of Firmata v2.5
+#define ANALOG                  0x02 // same as PIN_MODE_ANALOG
+#define PWM                     0x03 // same as PIN_MODE_PWM
+#define SERVO                   0x04 // same as PIN_MODE_SERVO
+#define SHIFT                   0x05 // same as PIN_MODE_SHIFT
+#define I2C                     0x06 // same as PIN_MODE_I2C
+#define ONEWIRE                 0x07 // same as PIN_MODE_ONEWIRE
+#define STEPPER                 0x08 // same as PIN_MODE_STEPPER
+#define ENCODER                 0x09 // same as PIN_MODE_ENCODER
+#define IGNORE                  0x7F // same as PIN_MODE_IGNORE
+
+namespace firmata {
+
+// TODO make it a subclass of a generic Serial/Stream base class
+class FirmataClass
+{
+  public:
+    typedef void (*callbackFunction)(uint8_t, int);
+    typedef void (*systemCallbackFunction)(void);
+    typedef void (*stringCallbackFunction)(char *);
+    typedef void (*sysexCallbackFunction)(uint8_t command, uint8_t argc, uint8_t *argv);
+
+    FirmataClass();
+
+    /* Arduino constructors */
+    void begin();
+    void begin(long);
+    void begin(Stream &s);
+
+    /* querying functions */
+    void printVersion(void);
+    void blinkVersion(void);
+    void printFirmwareVersion(void);
+
+    //void setFirmwareVersion(byte major, byte minor);  // see macro below
+    void setFirmwareNameAndVersion(const char *name, byte major, byte minor);
+    void disableBlinkVersion();
+
+    /* serial receive handling */
+    int available(void);
+    void processInput(void);
+    void parse(unsigned char value);
+    boolean isParsingMessage(void);
+
+    /* serial send handling */
+    void sendAnalog(byte pin, int value);
+    void sendDigital(byte pin, int value); // TODO implement this
+    void sendDigitalPort(byte portNumber, int portData);
+    void sendString(const char *string);
+    void sendString(byte command, const char *string);
+    void sendSysex(byte command, byte bytec, byte *bytev);
+    void write(byte c);
+
+    /* attach & detach callback functions to messages */
+    void attach(uint8_t command, callbackFunction newFunction);
+    void attach(uint8_t command, systemCallbackFunction newFunction);
+    void attach(uint8_t command, stringCallbackFunction newFunction);
+    void attach(uint8_t command, sysexCallbackFunction newFunction);
+    void detach(uint8_t command);
+
+    /* access pin state and config */
+    byte getPinMode(byte pin);
+    void setPinMode(byte pin, byte config);
+
+    /* access pin state */
+    int getPinState(byte pin);
+    void setPinState(byte pin, int state);
+
+    /* utility methods */
+    void sendValueAsTwo7bitBytes(int value);
+    void startSysex(void);
+    void endSysex(void);
+
+  private:
+    uint8_t parserBuffer[MAX_DATA_BYTES];
+    FirmataMarshaller marshaller;
+    FirmataParser parser;
+    Stream *FirmataStream;
+
+    /* firmware name and version */
+    byte firmwareVersionCount;
+    byte *firmwareVersionVector;
+
+    /* pin configuration */
+    byte pinConfig[TOTAL_PINS];
+    int pinState[TOTAL_PINS];
+
+    boolean blinkVersionDisabled;
+
+    /* private methods ------------------------------ */
+    void strobeBlinkPin(byte pin, int count, int onInterval, int offInterval);
+    friend void FirmataMarshaller::encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
+
+    /* callback functions */
+    static callbackFunction currentAnalogCallback;
+    static callbackFunction currentDigitalCallback;
+    static callbackFunction currentPinModeCallback;
+    static callbackFunction currentPinValueCallback;
+    static callbackFunction currentReportAnalogCallback;
+    static callbackFunction currentReportDigitalCallback;
+    static stringCallbackFunction currentStringCallback;
+    static sysexCallbackFunction currentSysexCallback;
+    static systemCallbackFunction currentSystemResetCallback;
+
+    /* static callbacks */
+    inline static void staticAnalogCallback (void *, uint8_t command, uint16_t value) { if ( currentAnalogCallback ) { currentAnalogCallback(command,(int)value); } }
+    inline static void staticDigitalCallback (void *, uint8_t command, uint16_t value) { if ( currentDigitalCallback ) { currentDigitalCallback(command, (int)value); } }
+    inline static void staticPinModeCallback (void *, uint8_t command, uint16_t value) { if ( currentPinModeCallback ) { currentPinModeCallback(command, (int)value); } }
+    inline static void staticPinValueCallback (void *, uint8_t command, uint16_t value) { if ( currentPinValueCallback ) { currentPinValueCallback(command, (int)value); } }
+    inline static void staticReportAnalogCallback (void *, uint8_t command, uint16_t value) { if ( currentReportAnalogCallback ) { currentReportAnalogCallback(command, (int)value); } }
+    inline static void staticReportDigitalCallback (void *, uint8_t command, uint16_t value) { if ( currentReportDigitalCallback ) { currentReportDigitalCallback(command, (int)value); } }
+    inline static void staticStringCallback (void *, const char * c_str) { if ( currentStringCallback ) { currentStringCallback((char *)c_str); } }
+    inline static void staticSysexCallback (void *, uint8_t command, size_t argc, uint8_t *argv) { if ( currentSysexCallback ) { currentSysexCallback(command, (uint8_t)argc, argv); } }
+    inline static void staticReportFirmwareCallback (void * context, size_t, size_t, const char *) { if ( context ) { ((FirmataClass *)context)->printFirmwareVersion(); } }
+    inline static void staticReportVersionCallback (void * context) { if ( context ) { ((FirmataClass *)context)->printVersion(); } }
+    inline static void staticSystemResetCallback (void *) { if ( currentSystemResetCallback ) { currentSystemResetCallback(); } }
+};
+
+} // namespace firmata
+
+extern "C" {
+  // callback function types
+  typedef firmata::FirmataClass::callbackFunction callbackFunction;
+  typedef firmata::FirmataClass::systemCallbackFunction systemCallbackFunction;
+  typedef firmata::FirmataClass::stringCallbackFunction stringCallbackFunction;
+  typedef firmata::FirmataClass::sysexCallbackFunction sysexCallbackFunction;
+}
+
+extern firmata::FirmataClass Firmata;
+
+/*==============================================================================
+ * MACROS
+ *============================================================================*/
+
+/* shortcut for setFirmwareNameAndVersion() that uses __FILE__ to set the
+ * firmware name.  It needs to be a macro so that __FILE__ is included in the
+ * firmware source file rather than the library source file.
+ */
+#define setFirmwareVersion(x, y)   setFirmwareNameAndVersion(__FILE__, x, y)
+
+#endif /* Firmata_h */
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataConstants.h b/Firmware/Tiva C/StandardFirmata/FirmataConstants.h
new file mode 100644
index 0000000..e8d2181
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataConstants.h
@@ -0,0 +1,97 @@
+/*
+  FirmataConstants.h
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2017 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataConstants_h
+#define FirmataConstants_h
+
+namespace firmata {
+/* Version numbers for the Firmata library.
+ * The firmware version will not always equal the protocol version going forward.
+ * Query using the REPORT_FIRMWARE message.
+ */
+static const int FIRMWARE_MAJOR_VERSION =  2;
+static const int FIRMWARE_MINOR_VERSION =  5;
+static const int FIRMWARE_BUGFIX_VERSION = 6;
+
+/* Version numbers for the protocol.  The protocol is still changing, so these
+ * version numbers are important.
+ * Query using the REPORT_VERSION message.
+ */
+static const int PROTOCOL_MAJOR_VERSION =  2; // for non-compatible changes
+static const int PROTOCOL_MINOR_VERSION =  5; // for backwards compatible changes
+static const int PROTOCOL_BUGFIX_VERSION = 1; // for bugfix releases
+
+static const int MAX_DATA_BYTES =          64; // max number of data bytes in incoming messages
+
+// message command bytes (128-255/0x80-0xFF)
+
+static const int DIGITAL_MESSAGE =         0x90; // send data for a digital port (collection of 8 pins)
+static const int ANALOG_MESSAGE =          0xE0; // send data for an analog pin (or PWM)
+static const int REPORT_ANALOG =           0xC0; // enable analog input by pin #
+static const int REPORT_DIGITAL =          0xD0; // enable digital input by port pair
+//
+static const int SET_PIN_MODE =            0xF4; // set a pin to INPUT/OUTPUT/PWM/etc
+static const int SET_DIGITAL_PIN_VALUE =   0xF5; // set value of an individual digital pin
+//
+static const int REPORT_VERSION =          0xF9; // report protocol version
+static const int SYSTEM_RESET =            0xFF; // reset from MIDI
+//
+static const int START_SYSEX =             0xF0; // start a MIDI Sysex message
+static const int END_SYSEX =               0xF7; // end a MIDI Sysex message
+
+// extended command set using sysex (0-127/0x00-0x7F)
+/* 0x00-0x0F reserved for user-defined commands */
+
+static const int SERIAL_DATA =             0x60; // communicate with serial devices, including other boards
+static const int ENCODER_DATA =            0x61; // reply with encoders current positions
+static const int SERVO_CONFIG =            0x70; // set max angle, minPulse, maxPulse, freq
+static const int STRING_DATA =             0x71; // a string message with 14-bits per char
+static const int STEPPER_DATA =            0x72; // control a stepper motor
+static const int ONEWIRE_DATA =            0x73; // send an OneWire read/write/reset/select/skip/search request
+static const int SHIFT_DATA =              0x75; // a bitstream to/from a shift register
+static const int I2C_REQUEST =             0x76; // send an I2C read/write request
+static const int I2C_REPLY =               0x77; // a reply to an I2C read request
+static const int I2C_CONFIG =              0x78; // config I2C settings such as delay times and power pins
+static const int REPORT_FIRMWARE =         0x79; // report name and version of the firmware
+static const int EXTENDED_ANALOG =         0x6F; // analog write (PWM, Servo, etc) to any pin
+static const int PIN_STATE_QUERY =         0x6D; // ask for a pin's current mode and value
+static const int PIN_STATE_RESPONSE =      0x6E; // reply with pin's current mode and value
+static const int CAPABILITY_QUERY =        0x6B; // ask for supported modes and resolution of all pins
+static const int CAPABILITY_RESPONSE =     0x6C; // reply with supported modes and resolution
+static const int ANALOG_MAPPING_QUERY =    0x69; // ask for mapping of analog to pin numbers
+static const int ANALOG_MAPPING_RESPONSE = 0x6A; // reply with mapping info
+static const int SAMPLING_INTERVAL =       0x7A; // set the poll rate of the main loop
+static const int SCHEDULER_DATA =          0x7B; // send a createtask/deletetask/addtotask/schedule/querytasks/querytask request to the scheduler
+static const int SYSEX_NON_REALTIME =      0x7E; // MIDI Reserved for non-realtime messages
+static const int SYSEX_REALTIME =          0x7F; // MIDI Reserved for realtime messages
+
+// pin modes
+static const int PIN_MODE_INPUT =          0x00; // same as INPUT defined in Arduino.h
+static const int PIN_MODE_OUTPUT =         0x01; // same as OUTPUT defined in Arduino.h
+static const int PIN_MODE_ANALOG =         0x02; // analog pin in analogInput mode
+static const int PIN_MODE_PWM =            0x03; // digital pin in PWM output mode
+static const int PIN_MODE_SERVO =          0x04; // digital pin in Servo output mode
+static const int PIN_MODE_SHIFT =          0x05; // shiftIn/shiftOut mode
+static const int PIN_MODE_I2C =            0x06; // pin included in I2C setup
+static const int PIN_MODE_ONEWIRE =        0x07; // pin configured for 1-wire
+static const int PIN_MODE_STEPPER =        0x08; // pin configured for stepper motor
+static const int PIN_MODE_ENCODER =        0x09; // pin configured for rotary encoders
+static const int PIN_MODE_SERIAL =         0x0A; // pin configured for serial communication
+static const int PIN_MODE_PULLUP =         0x0B; // enable internal pull-up resistor for pin
+static const int PIN_MODE_IGNORE =         0x7F; // pin configured to be ignored by digitalWrite and capabilityResponse
+
+static const int TOTAL_PIN_MODES =         13;
+
+} // namespace firmata
+
+#endif // FirmataConstants_h
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataDefines.h b/Firmware/Tiva C/StandardFirmata/FirmataDefines.h
new file mode 100644
index 0000000..fb95fb5
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataDefines.h
@@ -0,0 +1,283 @@
+/*
+  FirmataDefines.h
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataDefines_h
+#define FirmataDefines_h
+
+#include "FirmataConstants.h"
+
+/* Version numbers for the Firmata library.
+ * The firmware version will not always equal the protocol version going forward.
+ * Query using the REPORT_FIRMWARE message.
+ */
+#define FIRMATA_FIRMWARE_MAJOR_VERSION  firmata::FIRMWARE_MAJOR_VERSION
+#define FIRMATA_FIRMWARE_MINOR_VERSION  firmata::FIRMWARE_MINOR_VERSION
+#define FIRMATA_FIRMWARE_BUGFIX_VERSION firmata::FIRMWARE_BUGFIX_VERSION
+
+/* Version numbers for the protocol.  The protocol is still changing, so these
+ * version numbers are important.
+ * Query using the REPORT_VERSION message.
+ */
+#define FIRMATA_PROTOCOL_MAJOR_VERSION  firmata::PROTOCOL_MAJOR_VERSION // for non-compatible changes
+#define FIRMATA_PROTOCOL_MINOR_VERSION  firmata::PROTOCOL_MINOR_VERSION // for backwards compatible changes
+#define FIRMATA_PROTOCOL_BUGFIX_VERSION firmata::PROTOCOL_BUGFIX_VERSION // for bugfix releases
+
+#ifdef MAX_DATA_BYTES
+#undef MAX_DATA_BYTES
+#endif
+#define MAX_DATA_BYTES          firmata::MAX_DATA_BYTES // max number of data bytes in incoming messages
+
+// message command bytes (128-255/0x80-0xFF)
+
+#ifdef DIGITAL_MESSAGE
+#undef DIGITAL_MESSAGE
+#endif
+#define DIGITAL_MESSAGE         firmata::DIGITAL_MESSAGE // send data for a digital port (collection of 8 pins)
+
+#ifdef ANALOG_MESSAGE
+#undef ANALOG_MESSAGE
+#endif
+#define ANALOG_MESSAGE          firmata::ANALOG_MESSAGE // send data for an analog pin (or PWM)
+
+#ifdef REPORT_ANALOG
+#undef REPORT_ANALOG
+#endif
+#define REPORT_ANALOG           firmata::REPORT_ANALOG // enable analog input by pin #
+
+#ifdef REPORT_DIGITAL
+#undef REPORT_DIGITAL
+#endif
+#define REPORT_DIGITAL          firmata::REPORT_DIGITAL // enable digital input by port pair
+
+//
+
+#ifdef SET_PIN_MODE
+#undef SET_PIN_MODE
+#endif
+#define SET_PIN_MODE            firmata::SET_PIN_MODE // set a pin to INPUT/OUTPUT/PWM/etc
+
+#ifdef SET_DIGITAL_PIN_VALUE
+#undef SET_DIGITAL_PIN_VALUE
+#endif
+#define SET_DIGITAL_PIN_VALUE   firmata::SET_DIGITAL_PIN_VALUE // set value of an individual digital pin
+
+//
+
+#ifdef REPORT_VERSION
+#undef REPORT_VERSION
+#endif
+#define REPORT_VERSION          firmata::REPORT_VERSION // report protocol version
+
+#ifdef SYSTEM_RESET
+#undef SYSTEM_RESET
+#endif
+#define SYSTEM_RESET            firmata::SYSTEM_RESET // reset from MIDI
+
+//
+
+#ifdef START_SYSEX
+#undef START_SYSEX
+#endif
+#define START_SYSEX             firmata::START_SYSEX // start a MIDI Sysex message
+
+#ifdef END_SYSEX
+#undef END_SYSEX
+#endif
+#define END_SYSEX               firmata::END_SYSEX // end a MIDI Sysex message
+
+// extended command set using sysex (0-127/0x00-0x7F)
+/* 0x00-0x0F reserved for user-defined commands */
+
+#ifdef SERIAL_MESSAGE
+#undef SERIAL_MESSAGE
+#endif
+#define SERIAL_MESSAGE          firmata::SERIAL_DATA // communicate with serial devices, including other boards
+
+#ifdef ENCODER_DATA
+#undef ENCODER_DATA
+#endif
+#define ENCODER_DATA            firmata::ENCODER_DATA // reply with encoders current positions
+
+#ifdef SERVO_CONFIG
+#undef SERVO_CONFIG
+#endif
+#define SERVO_CONFIG            firmata::SERVO_CONFIG // set max angle, minPulse, maxPulse, freq
+
+#ifdef STRING_DATA
+#undef STRING_DATA
+#endif
+#define STRING_DATA             firmata::STRING_DATA // a string message with 14-bits per char
+
+#ifdef STEPPER_DATA
+#undef STEPPER_DATA
+#endif
+#define STEPPER_DATA            firmata::STEPPER_DATA // control a stepper motor
+
+#ifdef ONEWIRE_DATA
+#undef ONEWIRE_DATA
+#endif
+#define ONEWIRE_DATA            firmata::ONEWIRE_DATA // send an OneWire read/write/reset/select/skip/search request
+
+#ifdef SHIFT_DATA
+#undef SHIFT_DATA
+#endif
+#define SHIFT_DATA              firmata::SHIFT_DATA // a bitstream to/from a shift register
+
+#ifdef I2C_REQUEST
+#undef I2C_REQUEST
+#endif
+#define I2C_REQUEST             firmata::I2C_REQUEST // send an I2C read/write request
+
+#ifdef I2C_REPLY
+#undef I2C_REPLY
+#endif
+#define I2C_REPLY               firmata::I2C_REPLY // a reply to an I2C read request
+
+#ifdef I2C_CONFIG
+#undef I2C_CONFIG
+#endif
+#define I2C_CONFIG              firmata::I2C_CONFIG // config I2C settings such as delay times and power pins
+
+#ifdef REPORT_FIRMWARE
+#undef REPORT_FIRMWARE
+#endif
+#define REPORT_FIRMWARE         firmata::REPORT_FIRMWARE // report name and version of the firmware
+
+#ifdef EXTENDED_ANALOG
+#undef EXTENDED_ANALOG
+#endif
+#define EXTENDED_ANALOG         firmata::EXTENDED_ANALOG // analog write (PWM, Servo, etc) to any pin
+
+#ifdef PIN_STATE_QUERY
+#undef PIN_STATE_QUERY
+#endif
+#define PIN_STATE_QUERY         firmata::PIN_STATE_QUERY // ask for a pin's current mode and value
+
+#ifdef PIN_STATE_RESPONSE
+#undef PIN_STATE_RESPONSE
+#endif
+#define PIN_STATE_RESPONSE      firmata::PIN_STATE_RESPONSE // reply with pin's current mode and value
+
+#ifdef CAPABILITY_QUERY
+#undef CAPABILITY_QUERY
+#endif
+#define CAPABILITY_QUERY        firmata::CAPABILITY_QUERY // ask for supported modes and resolution of all pins
+
+#ifdef CAPABILITY_RESPONSE
+#undef CAPABILITY_RESPONSE
+#endif
+#define CAPABILITY_RESPONSE     firmata::CAPABILITY_RESPONSE // reply with supported modes and resolution
+
+#ifdef ANALOG_MAPPING_QUERY
+#undef ANALOG_MAPPING_QUERY
+#endif
+#define ANALOG_MAPPING_QUERY    firmata::ANALOG_MAPPING_QUERY // ask for mapping of analog to pin numbers
+
+#ifdef ANALOG_MAPPING_RESPONSE
+#undef ANALOG_MAPPING_RESPONSE
+#endif
+#define ANALOG_MAPPING_RESPONSE firmata::ANALOG_MAPPING_RESPONSE // reply with mapping info
+
+#ifdef SAMPLING_INTERVAL
+#undef SAMPLING_INTERVAL
+#endif
+#define SAMPLING_INTERVAL       firmata::SAMPLING_INTERVAL // set the poll rate of the main loop
+
+#ifdef SCHEDULER_DATA
+#undef SCHEDULER_DATA
+#endif
+#define SCHEDULER_DATA          firmata::SCHEDULER_DATA // send a createtask/deletetask/addtotask/schedule/querytasks/querytask request to the scheduler
+
+#ifdef SYSEX_NON_REALTIME
+#undef SYSEX_NON_REALTIME
+#endif
+#define SYSEX_NON_REALTIME      firmata::SYSEX_NON_REALTIME // MIDI Reserved for non-realtime messages
+
+#ifdef SYSEX_REALTIME
+#undef SYSEX_REALTIME
+#endif
+#define SYSEX_REALTIME          firmata::SYSEX_REALTIME // MIDI Reserved for realtime messages
+
+// pin modes
+
+#ifdef PIN_MODE_INPUT
+#undef PIN_MODE_INPUT
+#endif
+#define PIN_MODE_INPUT          firmata::PIN_MODE_INPUT // same as INPUT defined in Arduino.h
+
+#ifdef PIN_MODE_OUTPUT
+#undef PIN_MODE_OUTPUT
+#endif
+#define PIN_MODE_OUTPUT         firmata::PIN_MODE_OUTPUT // same as OUTPUT defined in Arduino.h
+
+#ifdef PIN_MODE_ANALOG
+#undef PIN_MODE_ANALOG
+#endif
+#define PIN_MODE_ANALOG         firmata::PIN_MODE_ANALOG // analog pin in analogInput mode
+
+#ifdef PIN_MODE_PWM
+#undef PIN_MODE_PWM
+#endif
+#define PIN_MODE_PWM            firmata::PIN_MODE_PWM // digital pin in PWM output mode
+
+#ifdef PIN_MODE_SERVO
+#undef PIN_MODE_SERVO
+#endif
+#define PIN_MODE_SERVO          firmata::PIN_MODE_SERVO // digital pin in Servo output mode
+
+#ifdef PIN_MODE_SHIFT
+#undef PIN_MODE_SHIFT
+#endif
+#define PIN_MODE_SHIFT          firmata::PIN_MODE_SHIFT // shiftIn/shiftOut mode
+
+#ifdef PIN_MODE_I2C
+#undef PIN_MODE_I2C
+#endif
+#define PIN_MODE_I2C            firmata::PIN_MODE_I2C // pin included in I2C setup
+
+#ifdef PIN_MODE_ONEWIRE
+#undef PIN_MODE_ONEWIRE
+#endif
+#define PIN_MODE_ONEWIRE        firmata::PIN_MODE_ONEWIRE // pin configured for 1-wire
+
+#ifdef PIN_MODE_STEPPER
+#undef PIN_MODE_STEPPER
+#endif
+#define PIN_MODE_STEPPER        firmata::PIN_MODE_STEPPER // pin configured for stepper motor
+
+#ifdef PIN_MODE_ENCODER
+#undef PIN_MODE_ENCODER
+#endif
+#define PIN_MODE_ENCODER        firmata::PIN_MODE_ENCODER // pin configured for rotary encoders
+
+#ifdef PIN_MODE_SERIAL
+#undef PIN_MODE_SERIAL
+#endif
+#define PIN_MODE_SERIAL         firmata::PIN_MODE_SERIAL // pin configured for serial communication
+
+#ifdef PIN_MODE_PULLUP
+#undef PIN_MODE_PULLUP
+#endif
+#define PIN_MODE_PULLUP         firmata::PIN_MODE_PULLUP // enable internal pull-up resistor for pin
+
+#ifdef PIN_MODE_IGNORE
+#undef PIN_MODE_IGNORE
+#endif
+#define PIN_MODE_IGNORE         firmata::PIN_MODE_IGNORE // pin configured to be ignored by digitalWrite and capabilityResponse
+
+#ifdef TOTAL_PIN_MODES
+#undef TOTAL_PIN_MODES
+#endif
+#define TOTAL_PIN_MODES         firmata::TOTAL_PIN_MODES
+
+#endif // FirmataConstants_h
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.cpp b/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.cpp
new file mode 100644
index 0000000..6cb5200
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.cpp
@@ -0,0 +1,431 @@
+/*
+  FirmataMarshaller.cpp
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "FirmataMarshaller.h"
+
+#if defined(__cplusplus) && !defined(ARDUINO)
+  #include <cstring>
+#else
+  #include <string.h>
+#endif
+
+#include "FirmataConstants.h"
+
+using namespace firmata;
+
+//******************************************************************************
+//* Support Functions
+//******************************************************************************
+
+/**
+ * Request or halt a stream of analog readings from the Firmata host application. The range of pins is
+ * limited to [0..15] when using the REPORT_ANALOG. The maximum result of the REPORT_ANALOG is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin for which to request the value (limited to pins 0 - 15).
+ * @param stream_enable A zero value will disable the stream, a non-zero will enable the stream
+ * @note The maximum resulting value is 14-bits (16384).
+ */
+void FirmataMarshaller::reportAnalog(uint8_t pin, bool stream_enable)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  // pin can only be 0-15, so chop higher bits
+  FirmataStream->write(REPORT_ANALOG | (pin & 0xF));
+  FirmataStream->write(stream_enable);
+}
+
+/**
+ * Request or halt an 8-bit port stream from the Firmata host application (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number for which to request the value. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ * @param stream_enable A zero value will disable the stream, a non-zero will enable the stream
+ */
+void FirmataMarshaller::reportDigitalPort(uint8_t portNumber, bool stream_enable)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(REPORT_DIGITAL | (portNumber & 0xF));
+  FirmataStream->write(stream_enable);
+}
+
+/**
+ * An alternative to the normal analog message, this extended version allows addressing beyond
+ * pin 15 and supports sending analog values with any number of bits.
+ * @param pin The analog pin to which the value is sent.
+ * @param bytec The size of the storage for the analog value
+ * @param bytev The pointer to the location of the analog value
+ */
+void FirmataMarshaller::sendExtendedAnalog(uint8_t pin, size_t bytec, uint8_t * bytev)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(START_SYSEX);
+  FirmataStream->write(EXTENDED_ANALOG);
+  FirmataStream->write(pin);
+  encodeByteStream(bytec, bytev, bytec);
+  FirmataStream->write(END_SYSEX);
+}
+
+/**
+ * Transform 8-bit stream into 7-bit message
+ * @param bytec The number of data bytes in the message.
+ * @param bytev A pointer to the array of data bytes to send in the message.
+ * @param max_bytes Force message to be n bytes, regardless of data bits.
+ */
+void FirmataMarshaller::encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes)
+const
+{
+  static const size_t transmit_bits = 7;
+  static const uint8_t transmit_mask = ((1 << transmit_bits) - 1);
+
+  size_t bytes_sent = 0;
+  size_t outstanding_bits = 0;
+  uint8_t outstanding_bit_cache = *bytev;
+
+  if ( !max_bytes ) { max_bytes = static_cast<size_t>(-1); }
+  for (size_t i = 0 ; (i < bytec) && (bytes_sent < max_bytes) ; ++i) {
+    uint8_t transmit_byte = (outstanding_bit_cache|(bytev[i] << outstanding_bits));
+    FirmataStream->write(transmit_mask & transmit_byte);
+    ++bytes_sent;
+    outstanding_bit_cache = (bytev[i] >> (transmit_bits - outstanding_bits));
+    outstanding_bits = (outstanding_bits + (8 - transmit_bits));
+    for ( ; (outstanding_bits >= transmit_bits) && (bytes_sent < max_bytes) ; ) {
+      transmit_byte = outstanding_bit_cache;
+    FirmataStream->write(transmit_mask & transmit_byte);
+      ++bytes_sent;
+      outstanding_bit_cache >>= transmit_bits;
+      outstanding_bits -= transmit_bits;
+    }
+  }
+  if ( outstanding_bits && (bytes_sent < max_bytes) ) {
+    FirmataStream->write(static_cast<uint8_t>((1 << outstanding_bits) - 1) & outstanding_bit_cache);
+  }
+}
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+/**
+ * The FirmataMarshaller class.
+ */
+FirmataMarshaller::FirmataMarshaller()
+:
+  FirmataStream((Stream *)NULL)
+{
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+/**
+ * Reassign the Firmata stream transport.
+ * @param s A reference to the Stream transport object. This can be any type of
+ * transport that implements the Stream interface. Some examples include Ethernet, WiFi
+ * and other UARTs on the board (Serial1, Serial2, etc).
+ */
+void FirmataMarshaller::begin(Stream &s)
+{
+  FirmataStream = &s;
+}
+
+/**
+ * Closes the FirmataMarshaller stream by setting its stream reference to `(Stream *)NULL`
+ */
+void FirmataMarshaller::end(void)
+{
+  FirmataStream = (Stream *)NULL;
+}
+
+//******************************************************************************
+//* Output Stream Handling
+//******************************************************************************
+
+/**
+ * Query the target's firmware name and version
+ */
+void FirmataMarshaller::queryFirmwareVersion(void)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(START_SYSEX);
+  FirmataStream->write(REPORT_FIRMWARE);
+  FirmataStream->write(END_SYSEX);
+}
+
+/**
+ * Query the target's Firmata protocol version
+ */
+void FirmataMarshaller::queryVersion(void)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(REPORT_VERSION);
+}
+
+/**
+ * Halt the stream of analog readings from the Firmata host application. The range of pins is
+ * limited to [0..15] when using the REPORT_ANALOG. The maximum result of the REPORT_ANALOG is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin for which to request the value (limited to pins 0 - 15).
+ */
+void FirmataMarshaller::reportAnalogDisable(uint8_t pin)
+const
+{
+  reportAnalog(pin, false);
+}
+
+/**
+ * Request a stream of analog readings from the Firmata host application. The range of pins is
+ * limited to [0..15] when using the REPORT_ANALOG. The maximum result of the REPORT_ANALOG is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin for which to request the value (limited to pins 0 - 15).
+ */
+void FirmataMarshaller::reportAnalogEnable(uint8_t pin)
+const
+{
+  reportAnalog(pin, true);
+}
+
+/**
+ * Halt an 8-bit port stream from the Firmata host application (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number for which to request the value. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ */
+void FirmataMarshaller::reportDigitalPortDisable(uint8_t portNumber)
+const
+{
+  reportDigitalPort(portNumber, false);
+}
+
+/**
+ * Request an 8-bit port stream from the Firmata host application (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number for which to request the value. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ */
+void FirmataMarshaller::reportDigitalPortEnable(uint8_t portNumber)
+const
+{
+  reportDigitalPort(portNumber, true);
+}
+
+/**
+ * Send an analog message to the Firmata host application. The range of pins is limited to [0..15]
+ * when using the ANALOG_MESSAGE. The maximum value of the ANALOG_MESSAGE is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin to which the value is sent.
+ * @param value The value of the analog pin (0 - 1024 for 10-bit analog, 0 - 4096 for 12-bit, etc).
+ * @note The maximum value is 14-bits (16384).
+ */
+void FirmataMarshaller::sendAnalog(uint8_t pin, uint16_t value)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  if ( (0xF >= pin) && (0x3FFF >= value) ) {
+    FirmataStream->write(ANALOG_MESSAGE|pin);
+    encodeByteStream(sizeof(value), reinterpret_cast<uint8_t *>(&value), sizeof(value));
+  } else {
+    sendExtendedAnalog(pin, sizeof(value), reinterpret_cast<uint8_t *>(&value));
+  }
+}
+
+/**
+ * Send an analog mapping query to the Firmata host application. The resulting sysex message will
+ * have an ANALOG_MAPPING_RESPONSE command byte, followed by a list of pins [0-n]; where each
+ * pin will specify its corresponding analog pin number or 0x7F (127) if not applicable.
+ */
+void FirmataMarshaller::sendAnalogMappingQuery(void)
+const
+{
+  sendSysex(ANALOG_MAPPING_QUERY, 0, NULL);
+}
+
+/**
+ * Send a capability query to the Firmata host application. The resulting sysex message will have
+ * a CAPABILITY_RESPONSE command byte, followed by a list of byte tuples (mode and mode resolution)
+ * for each pin; where each pin list is terminated by 0x7F (127).
+ */
+void FirmataMarshaller::sendCapabilityQuery(void)
+const
+{
+  sendSysex(CAPABILITY_QUERY, 0, NULL);
+}
+
+/**
+ * Send a single digital pin value to the Firmata host application.
+ * @param pin The digital pin to send the value of.
+ * @param value The value of the pin.
+ */
+void FirmataMarshaller::sendDigital(uint8_t pin, uint8_t value)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(SET_DIGITAL_PIN_VALUE);
+  FirmataStream->write(pin & 0x7F);
+  FirmataStream->write(value != 0);
+}
+
+
+/**
+ * Send an 8-bit port in a single digital message (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number to send. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ * @param portData The value of the port. The value of each pin in the port is represented by a bit.
+ */
+void FirmataMarshaller::sendDigitalPort(uint8_t portNumber, uint16_t portData)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(DIGITAL_MESSAGE | (portNumber & 0xF));
+  // Tx bits  0-6 (protocol v1 and higher)
+  // Tx bits 7-13 (bit 7 only for protocol v2 and higher)
+  encodeByteStream(sizeof(portData), reinterpret_cast<uint8_t *>(&portData), sizeof(portData));
+}
+
+/**
+ * Sends the firmware name and version to the Firmata host application.
+ * @param major The major verison number
+ * @param minor The minor version number
+ * @param bytec The length of the firmware name
+ * @param bytev The firmware name array
+ */
+void FirmataMarshaller::sendFirmwareVersion(uint8_t major, uint8_t minor, size_t bytec, uint8_t *bytev)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  size_t i;
+  FirmataStream->write(START_SYSEX);
+  FirmataStream->write(REPORT_FIRMWARE);
+  FirmataStream->write(major);
+  FirmataStream->write(minor);
+  for (i = 0; i < bytec; ++i) {
+    encodeByteStream(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]));
+  }
+  FirmataStream->write(END_SYSEX);
+}
+
+/**
+ * Send the Firmata protocol version to the Firmata host application.
+ * @param major The major verison number
+ * @param minor The minor version number
+ */
+void FirmataMarshaller::sendVersion(uint8_t major, uint8_t minor)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(REPORT_VERSION);
+  FirmataStream->write(major);
+  FirmataStream->write(minor);
+}
+
+/**
+ * Send the pin mode/configuration. The pin configuration (or mode) in Firmata represents the
+ * current function of the pin. Examples are digital input or output, analog input, pwm, i2c,
+ * serial (uart), etc.
+ * @param pin The pin to configure.
+ * @param config The configuration value for the specified pin.
+ */
+void FirmataMarshaller::sendPinMode(uint8_t pin, uint8_t config)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(SET_PIN_MODE);
+  FirmataStream->write(pin);
+  FirmataStream->write(config);
+}
+
+/**
+ * Send a pin state query to the Firmata host application. The resulting sysex message will have
+ * a PIN_STATE_RESPONSE command byte, followed by the pin number, the pin mode and a stream of
+ * bits to indicate any *data* written to the pin (pin state).
+ * @param pin The pin to query
+ * @note The pin state is any data written to the pin (i.e. pin state != pin value)
+ */
+void FirmataMarshaller::sendPinStateQuery(uint8_t pin)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(START_SYSEX);
+  FirmataStream->write(PIN_STATE_QUERY);
+  FirmataStream->write(pin);
+  FirmataStream->write(END_SYSEX);
+}
+
+/**
+ * Send a sysex message where all values after the command byte are packet as 2 7-bit bytes
+ * (this is not always the case so this function is not always used to send sysex messages).
+ * @param command The sysex command byte.
+ * @param bytec The number of data bytes in the message (excludes start, command and end bytes).
+ * @param bytev A pointer to the array of data bytes to send in the message.
+ */
+void FirmataMarshaller::sendSysex(uint8_t command, size_t bytec, uint8_t *bytev)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  size_t i;
+  FirmataStream->write(START_SYSEX);
+  FirmataStream->write(command);
+  for (i = 0; i < bytec; ++i) {
+    encodeByteStream(sizeof(bytev[i]), reinterpret_cast<uint8_t *>(&bytev[i]));
+  }
+  FirmataStream->write(END_SYSEX);
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param string A pointer to the char string
+ */
+void FirmataMarshaller::sendString(const char *string)
+const
+{
+  sendSysex(STRING_DATA, strlen(string), reinterpret_cast<uint8_t *>(const_cast<char *>(string)));
+}
+
+/**
+ * The sampling interval sets how often analog data and i2c data is reported to the client.
+ * @param interval_ms The interval (in milliseconds) at which to sample
+ * @note The default sampling interval is 19ms
+ */
+void FirmataMarshaller::setSamplingInterval(uint16_t interval_ms)
+const
+{
+  sendSysex(SAMPLING_INTERVAL, sizeof(interval_ms), reinterpret_cast<uint8_t *>(&interval_ms));
+}
+
+/**
+ * Perform a software reset on the target. For example, StandardFirmata.ino will initialize
+ * everything to a known state and reset the parsing buffer.
+ */
+void FirmataMarshaller::systemReset(void)
+const
+{
+  if ( (Stream *)NULL == FirmataStream ) { return; }
+  FirmataStream->write(SYSTEM_RESET);
+}
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.h b/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.h
new file mode 100644
index 0000000..3fa83f6
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataMarshaller.h
@@ -0,0 +1,75 @@
+/*
+  FirmataMarshaller.h
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataMarshaller_h
+#define FirmataMarshaller_h
+
+#if defined(__cplusplus) && !defined(ARDUINO)
+  #include <cstddef>
+  #include <cstdint>
+#else
+  #include <stddef.h>
+  #include <stdint.h>
+#endif
+
+#include <Stream.h>
+
+namespace firmata {
+
+class FirmataMarshaller
+{
+    friend class FirmataClass;
+
+  public:
+    /* constructors */
+    FirmataMarshaller();
+
+    /* public methods */
+    void begin(Stream &s);
+    void end();
+
+    /* serial send handling */
+    void queryFirmwareVersion(void) const;
+    void queryVersion(void) const;
+    void reportAnalogDisable(uint8_t pin) const;
+    void reportAnalogEnable(uint8_t pin) const;
+    void reportDigitalPortDisable(uint8_t portNumber) const;
+    void reportDigitalPortEnable(uint8_t portNumber) const;
+    void sendAnalog(uint8_t pin, uint16_t value) const;
+    void sendAnalogMappingQuery(void) const;
+    void sendCapabilityQuery(void) const;
+    void sendDigital(uint8_t pin, uint8_t value) const;
+    void sendDigitalPort(uint8_t portNumber, uint16_t portData) const;
+    void sendFirmwareVersion(uint8_t major, uint8_t minor, size_t bytec, uint8_t *bytev) const;
+    void sendVersion(uint8_t major, uint8_t minor) const;
+    void sendPinMode(uint8_t pin, uint8_t config) const;
+    void sendPinStateQuery(uint8_t pin) const;
+    void sendString(const char *string) const;
+    void sendSysex(uint8_t command, size_t bytec, uint8_t *bytev) const;
+    void setSamplingInterval(uint16_t interval_ms) const;
+    void systemReset(void) const;
+
+  private:
+    /* utility methods */
+    void reportAnalog(uint8_t pin, bool stream_enable) const;
+    void reportDigitalPort(uint8_t portNumber, bool stream_enable) const;
+    void sendExtendedAnalog(uint8_t pin, size_t bytec, uint8_t * bytev) const;
+    void encodeByteStream (size_t bytec, uint8_t * bytev, size_t max_bytes = 0) const;
+
+    Stream * FirmataStream;
+};
+
+} // namespace firmata
+
+#endif /* FirmataMarshaller_h */
+
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataParser.cpp b/Firmware/Tiva C/StandardFirmata/FirmataParser.cpp
new file mode 100644
index 0000000..d402fdf
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataParser.cpp
@@ -0,0 +1,480 @@
+/*
+  FirmataParser.cpp
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "FirmataParser.h"
+
+#include "FirmataConstants.h"
+
+using namespace firmata;
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+/**
+ * The FirmataParser class.
+ * @param dataBuffer A pointer to an external buffer used to store parsed data
+ * @param dataBufferSize The size of the external buffer
+ */
+FirmataParser::FirmataParser(uint8_t * const dataBuffer, size_t dataBufferSize)
+:
+  dataBuffer(dataBuffer),
+  dataBufferSize(dataBufferSize),
+  executeMultiByteCommand(0),
+  multiByteChannel(0),
+  waitForData(0),
+  parsingSysex(false),
+  sysexBytesRead(0),
+  currentAnalogCallbackContext((void *)NULL),
+  currentDigitalCallbackContext((void *)NULL),
+  currentReportAnalogCallbackContext((void *)NULL),
+  currentReportDigitalCallbackContext((void *)NULL),
+  currentPinModeCallbackContext((void *)NULL),
+  currentPinValueCallbackContext((void *)NULL),
+  currentReportFirmwareCallbackContext((void *)NULL),
+  currentReportVersionCallbackContext((void *)NULL),
+  currentDataBufferOverflowCallbackContext((void *)NULL),
+  currentStringCallbackContext((void *)NULL),
+  currentSysexCallbackContext((void *)NULL),
+  currentSystemResetCallbackContext((void *)NULL),
+  currentAnalogCallback((callbackFunction)NULL),
+  currentDigitalCallback((callbackFunction)NULL),
+  currentReportAnalogCallback((callbackFunction)NULL),
+  currentReportDigitalCallback((callbackFunction)NULL),
+  currentPinModeCallback((callbackFunction)NULL),
+  currentPinValueCallback((callbackFunction)NULL),
+  currentDataBufferOverflowCallback((dataBufferOverflowCallbackFunction)NULL),
+  currentStringCallback((stringCallbackFunction)NULL),
+  currentSysexCallback((sysexCallbackFunction)NULL),
+  currentReportFirmwareCallback((versionCallbackFunction)NULL),
+  currentReportVersionCallback((systemCallbackFunction)NULL),
+  currentSystemResetCallback((systemCallbackFunction)NULL)
+{
+    allowBufferUpdate = ((uint8_t *)NULL == dataBuffer);
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+//------------------------------------------------------------------------------
+// Serial Receive Handling
+
+/**
+ * Parse data from the input stream.
+ * @param inputData A single byte to be added to the parser.
+ */
+void FirmataParser::parse(uint8_t inputData)
+{
+  uint8_t command;
+
+  if (parsingSysex) {
+    if (inputData == END_SYSEX) {
+      //stop sysex byte
+      parsingSysex = false;
+      //fire off handler function
+      processSysexMessage();
+    } else {
+      //normal data byte - add to buffer
+      bufferDataAtPosition(inputData, sysexBytesRead);
+      ++sysexBytesRead;
+    }
+  } else if ( (waitForData > 0) && (inputData < 128) ) {
+    --waitForData;
+    bufferDataAtPosition(inputData, waitForData);
+    if ( (waitForData == 0) && executeMultiByteCommand ) { // got the whole message
+      switch (executeMultiByteCommand) {
+        case ANALOG_MESSAGE:
+          if (currentAnalogCallback) {
+            (*currentAnalogCallback)(currentAnalogCallbackContext,
+                                     multiByteChannel,
+                                     (dataBuffer[0] << 7)
+                                     + dataBuffer[1]);
+          }
+          break;
+        case DIGITAL_MESSAGE:
+          if (currentDigitalCallback) {
+            (*currentDigitalCallback)(currentDigitalCallbackContext,
+                                      multiByteChannel,
+                                      (dataBuffer[0] << 7)
+                                      + dataBuffer[1]);
+          }
+          break;
+        case SET_PIN_MODE:
+          if (currentPinModeCallback)
+            (*currentPinModeCallback)(currentPinModeCallbackContext, dataBuffer[1], dataBuffer[0]);
+          break;
+        case SET_DIGITAL_PIN_VALUE:
+          if (currentPinValueCallback)
+            (*currentPinValueCallback)(currentPinValueCallbackContext, dataBuffer[1], dataBuffer[0]);
+          break;
+        case REPORT_ANALOG:
+          if (currentReportAnalogCallback)
+            (*currentReportAnalogCallback)(currentReportAnalogCallbackContext, multiByteChannel, dataBuffer[0]);
+          break;
+        case REPORT_DIGITAL:
+          if (currentReportDigitalCallback)
+            (*currentReportDigitalCallback)(currentReportDigitalCallbackContext, multiByteChannel, dataBuffer[0]);
+          break;
+      }
+      executeMultiByteCommand = 0;
+    }
+  } else {
+    // remove channel info from command byte if less than 0xF0
+    if (inputData < 0xF0) {
+      command = inputData & 0xF0;
+      multiByteChannel = inputData & 0x0F;
+    } else {
+      command = inputData;
+      // commands in the 0xF* range don't use channel data
+    }
+    switch (command) {
+      case ANALOG_MESSAGE:
+      case DIGITAL_MESSAGE:
+      case SET_PIN_MODE:
+      case SET_DIGITAL_PIN_VALUE:
+        waitForData = 2; // two data bytes needed
+        executeMultiByteCommand = command;
+        break;
+      case REPORT_ANALOG:
+      case REPORT_DIGITAL:
+        waitForData = 1; // one data byte needed
+        executeMultiByteCommand = command;
+        break;
+      case START_SYSEX:
+        parsingSysex = true;
+        sysexBytesRead = 0;
+        break;
+      case SYSTEM_RESET:
+        systemReset();
+        break;
+      case REPORT_VERSION:
+        if (currentReportVersionCallback)
+          (*currentReportVersionCallback)(currentReportVersionCallbackContext);
+        break;
+    }
+  }
+}
+
+/**
+ * @return Returns true if the parser is actively parsing data.
+ */
+bool FirmataParser::isParsingMessage(void)
+const
+{
+  return (waitForData > 0 || parsingSysex);
+}
+
+/**
+ * Provides a mechanism to either set or update the working buffer of the parser.
+ * The method will be enabled when no buffer has been provided, or an overflow
+ * condition exists.
+ * @param dataBuffer A pointer to an external buffer used to store parsed data
+ * @param dataBufferSize The size of the external buffer
+ */
+int FirmataParser::setDataBufferOfSize(uint8_t * dataBuffer, size_t dataBufferSize)
+{
+    int result;
+
+    if ( !allowBufferUpdate ) {
+      result = __LINE__;
+    } else if ((uint8_t *)NULL == dataBuffer) {
+      result = __LINE__;
+    } else {
+      this->dataBuffer = dataBuffer;
+      this->dataBufferSize = dataBufferSize;
+      allowBufferUpdate = false;
+      result = 0;
+    }
+
+    return result;
+}
+
+/**
+ * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
+ * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, callbackFunction newFunction, void * context)
+{
+  switch (command) {
+    case ANALOG_MESSAGE:
+      currentAnalogCallback = newFunction;
+      currentAnalogCallbackContext = context;
+      break;
+    case DIGITAL_MESSAGE:
+      currentDigitalCallback = newFunction;
+      currentDigitalCallbackContext = context;
+      break;
+    case REPORT_ANALOG:
+      currentReportAnalogCallback = newFunction;
+      currentReportAnalogCallbackContext = context;
+      break;
+    case REPORT_DIGITAL:
+      currentReportDigitalCallback = newFunction;
+      currentReportDigitalCallbackContext = context;
+      break;
+    case SET_PIN_MODE:
+      currentPinModeCallback = newFunction;
+      currentPinModeCallbackContext = context;
+      break;
+    case SET_DIGITAL_PIN_VALUE:
+      currentPinValueCallback = newFunction;
+      currentPinValueCallbackContext = context;
+      break;
+  }
+}
+
+/**
+ * Attach a version callback function (supported option: REPORT_FIRMWARE).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, versionCallbackFunction newFunction, void * context)
+{
+  switch (command) {
+    case REPORT_FIRMWARE:
+      currentReportFirmwareCallback = newFunction;
+      currentReportFirmwareCallbackContext = context;
+      break;
+  }
+}
+
+/**
+ * Attach a system callback function (supported options are: SYSTEM_RESET, REPORT_VERSION).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, systemCallbackFunction newFunction, void * context)
+{
+  switch (command) {
+    case REPORT_VERSION:
+      currentReportVersionCallback = newFunction;
+      currentReportVersionCallbackContext = context;
+      break;
+    case SYSTEM_RESET:
+      currentSystemResetCallback = newFunction;
+      currentSystemResetCallbackContext = context;
+      break;
+  }
+}
+
+/**
+ * Attach a callback function for the STRING_DATA command.
+ * @param command Must be set to STRING_DATA or it will be ignored.
+ * @param newFunction A reference to the string callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, stringCallbackFunction newFunction, void * context)
+{
+  switch (command) {
+    case STRING_DATA:
+      currentStringCallback = newFunction;
+      currentStringCallbackContext = context;
+      break;
+  }
+}
+
+/**
+ * Attach a generic sysex callback function to sysex command.
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the sysex callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(uint8_t command, sysexCallbackFunction newFunction, void * context)
+{
+  (void)command;
+  currentSysexCallback = newFunction;
+  currentSysexCallbackContext = context;
+}
+
+/**
+ * Attach a buffer overflow callback
+ * @param newFunction A reference to the buffer overflow callback function to attach.
+ * @param context An optional context to be provided to the callback function (NULL by default).
+ * @note The context parameter is provided so you can pass a parameter, by reference, to
+ *       your callback function.
+ */
+void FirmataParser::attach(dataBufferOverflowCallbackFunction newFunction, void * context)
+{
+  currentDataBufferOverflowCallback = newFunction;
+  currentDataBufferOverflowCallbackContext = context;
+}
+
+/**
+ * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
+ * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
+ * @param command The ID of the command to detatch the callback function from.
+ */
+void FirmataParser::detach(uint8_t command)
+{
+  switch (command) {
+    case REPORT_FIRMWARE:
+      attach(command, (versionCallbackFunction)NULL, NULL);
+      break;
+    case REPORT_VERSION:
+    case SYSTEM_RESET:
+      attach(command, (systemCallbackFunction)NULL, NULL);
+      break;
+    case STRING_DATA:
+      attach(command, (stringCallbackFunction)NULL, NULL);
+      break;
+    case START_SYSEX:
+      attach(command, (sysexCallbackFunction)NULL, NULL);
+      break;
+    default:
+      attach(command, (callbackFunction)NULL, NULL);
+      break;
+  }
+}
+
+/**
+ * Detach the buffer overflow callback
+ * @param <unused> Any pointer of type dataBufferOverflowCallbackFunction.
+ */
+void FirmataParser::detach(dataBufferOverflowCallbackFunction)
+{
+  currentDataBufferOverflowCallback = (dataBufferOverflowCallbackFunction)NULL;
+  currentDataBufferOverflowCallbackContext = (void *)NULL;
+}
+
+//******************************************************************************
+//* Private Methods
+//******************************************************************************
+
+/**
+ * Buffer abstraction to prevent memory corruption
+ * @param data The byte to put into the buffer
+ * @param pos The position to insert the byte into the buffer
+ * @return writeError A boolean to indicate if an error occured
+ * @private
+ */
+bool FirmataParser::bufferDataAtPosition(const uint8_t data, const size_t pos)
+{
+  bool bufferOverflow = (pos >= dataBufferSize);
+
+  // Notify of overflow condition
+  if ( bufferOverflow
+  && ((dataBufferOverflowCallbackFunction)NULL != currentDataBufferOverflowCallback) )
+  {
+    allowBufferUpdate = true;
+    currentDataBufferOverflowCallback(currentDataBufferOverflowCallbackContext);
+    // Check if overflow was resolved during callback
+    bufferOverflow = (pos >= dataBufferSize);
+  }
+
+  // Write data to buffer if no overflow condition persist
+  if ( !bufferOverflow )
+  {
+    dataBuffer[pos] = data;
+  }
+
+  return bufferOverflow;
+}
+
+/**
+ * Transform 7-bit firmata message into 8-bit stream
+ * @param bytec The encoded data byte length of the message (max: 16383).
+ * @param bytev A pointer to the encoded array of data bytes.
+ * @return The length of the decoded data.
+ * @note The conversion will be done in place on the provided buffer.
+ * @private
+ */
+size_t FirmataParser::decodeByteStream(size_t bytec, uint8_t * bytev) {
+  size_t decoded_bytes, i;
+
+  for ( i = 0, decoded_bytes = 0 ; i < bytec ; ++decoded_bytes, ++i ) {
+    bytev[decoded_bytes] = bytev[i];
+    bytev[decoded_bytes] |= (uint8_t)(bytev[++i] << 7);
+  }
+
+  return decoded_bytes;
+}
+
+/**
+ * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
+ * Calls callback function for STRING_DATA and all other sysex messages.
+ * @private
+ */
+void FirmataParser::processSysexMessage(void)
+{
+  switch (dataBuffer[0]) { //first byte in buffer is command
+    case REPORT_FIRMWARE:
+      if (currentReportFirmwareCallback) {
+        const size_t major_version_offset = 1;
+        const size_t minor_version_offset = 2;
+        const size_t string_offset = 3;
+        // Test for malformed REPORT_FIRMWARE message (used to query firmware prior to Firmata v3.0.0)
+        if ( 3 > sysexBytesRead ) {
+          (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, 0, 0, (const char *)NULL);
+        } else {
+          const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
+          bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
+          (*currentReportFirmwareCallback)(currentReportFirmwareCallbackContext, (size_t)dataBuffer[major_version_offset], (size_t)dataBuffer[minor_version_offset], (const char *)&dataBuffer[string_offset]);
+        }
+      }
+      break;
+    case STRING_DATA:
+      if (currentStringCallback) {
+        const size_t string_offset = 1;
+        const size_t end_of_string = (string_offset + decodeByteStream((sysexBytesRead - string_offset), &dataBuffer[string_offset]));
+        bufferDataAtPosition('\0', end_of_string); // NULL terminate the string
+        (*currentStringCallback)(currentStringCallbackContext, (const char *)&dataBuffer[string_offset]);
+      }
+      break;
+    default:
+      if (currentSysexCallback)
+        (*currentSysexCallback)(currentSysexCallbackContext, dataBuffer[0], sysexBytesRead - 1, dataBuffer + 1);
+  }
+}
+
+/**
+ * Resets the system state upon a SYSTEM_RESET message from the host software.
+ * @private
+ */
+void FirmataParser::systemReset(void)
+{
+  size_t i;
+
+  waitForData = 0; // this flag says the next serial input will be data
+  executeMultiByteCommand = 0; // execute this after getting multi-byte data
+  multiByteChannel = 0; // channel data for multiByteCommands
+
+  for (i = 0; i < dataBufferSize; ++i) {
+    dataBuffer[i] = 0;
+  }
+
+  parsingSysex = false;
+  sysexBytesRead = 0;
+
+  if (currentSystemResetCallback)
+    (*currentSystemResetCallback)(currentSystemResetCallbackContext);
+}
diff --git a/Firmware/Tiva C/StandardFirmata/FirmataParser.h b/Firmware/Tiva C/StandardFirmata/FirmataParser.h
new file mode 100644
index 0000000..bb0c8be
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/FirmataParser.h
@@ -0,0 +1,105 @@
+/*
+  FirmataParser.h
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataParser_h
+#define FirmataParser_h
+
+#if defined(__cplusplus) && !defined(ARDUINO)
+  #include <cstddef>
+  #include <cstdint>
+#else
+  #include <stddef.h>
+  #include <stdint.h>
+#endif
+
+namespace firmata {
+
+class FirmataParser
+{
+  public:
+    /* callback function types */
+    typedef void (*callbackFunction)(void * context, uint8_t command, uint16_t value);
+    typedef void (*dataBufferOverflowCallbackFunction)(void * context);
+    typedef void (*stringCallbackFunction)(void * context, const char * c_str);
+    typedef void (*sysexCallbackFunction)(void * context, uint8_t command, size_t argc, uint8_t * argv);
+    typedef void (*systemCallbackFunction)(void * context);
+    typedef void (*versionCallbackFunction)(void * context, size_t sv_major, size_t sv_minor, const char * firmware);
+
+    FirmataParser(uint8_t * dataBuffer = (uint8_t *)NULL, size_t dataBufferSize = 0);
+
+    /* serial receive handling */
+    void parse(uint8_t value);
+    bool isParsingMessage(void) const;
+    int setDataBufferOfSize(uint8_t * dataBuffer, size_t dataBufferSize);
+
+    /* attach & detach callback functions to messages */
+    void attach(uint8_t command, callbackFunction newFunction, void * context = NULL);
+    void attach(dataBufferOverflowCallbackFunction newFunction, void * context = NULL);
+    void attach(uint8_t command, stringCallbackFunction newFunction, void * context = NULL);
+    void attach(uint8_t command, sysexCallbackFunction newFunction, void * context = NULL);
+    void attach(uint8_t command, systemCallbackFunction newFunction, void * context = NULL);
+    void attach(uint8_t command, versionCallbackFunction newFunction, void * context = NULL);
+    void detach(uint8_t command);
+    void detach(dataBufferOverflowCallbackFunction);
+
+  private:
+    /* input message handling */
+    bool allowBufferUpdate;
+    uint8_t * dataBuffer; // multi-byte data
+    size_t dataBufferSize;
+    uint8_t executeMultiByteCommand; // execute this after getting multi-byte data
+    uint8_t multiByteChannel; // channel data for multiByteCommands
+    size_t waitForData; // this flag says the next serial input will be data
+
+    /* sysex */
+    bool parsingSysex;
+    size_t sysexBytesRead;
+
+    /* callback context */
+    void * currentAnalogCallbackContext;
+    void * currentDigitalCallbackContext;
+    void * currentReportAnalogCallbackContext;
+    void * currentReportDigitalCallbackContext;
+    void * currentPinModeCallbackContext;
+    void * currentPinValueCallbackContext;
+    void * currentReportFirmwareCallbackContext;
+    void * currentReportVersionCallbackContext;
+    void * currentDataBufferOverflowCallbackContext;
+    void * currentStringCallbackContext;
+    void * currentSysexCallbackContext;
+    void * currentSystemResetCallbackContext;
+
+    /* callback functions */
+    callbackFunction currentAnalogCallback;
+    callbackFunction currentDigitalCallback;
+    callbackFunction currentReportAnalogCallback;
+    callbackFunction currentReportDigitalCallback;
+    callbackFunction currentPinModeCallback;
+    callbackFunction currentPinValueCallback;
+    dataBufferOverflowCallbackFunction currentDataBufferOverflowCallback;
+    stringCallbackFunction currentStringCallback;
+    sysexCallbackFunction currentSysexCallback;
+    versionCallbackFunction currentReportFirmwareCallback;
+    systemCallbackFunction currentReportVersionCallback;
+    systemCallbackFunction currentSystemResetCallback;
+
+    /* private methods ------------------------------ */
+    bool bufferDataAtPosition(const uint8_t data, const size_t pos);
+    size_t decodeByteStream(size_t bytec, uint8_t * bytev);
+    void processSysexMessage(void);
+    void systemReset(void);
+};
+
+} // firmata
+
+#endif /* FirmataParser_h */
diff --git a/Firmware/Tiva C/StandardFirmata/Servo.cpp b/Firmware/Tiva C/StandardFirmata/Servo.cpp
new file mode 100644
index 0000000..2216219
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/Servo.cpp
@@ -0,0 +1,234 @@
+//#include "Energia.h"
+#include "Servo.h"
+
+#include "inc/hw_ints.h"
+#include "inc/hw_memmap.h"
+#include "inc/hw_types.h"
+#include "driverlib/debug.h"
+#include "driverlib/gpio.h"
+#include "driverlib/pin_map.h"
+#include "driverlib/rom.h"
+#include "driverlib/sysctl.h"
+#include "driverlib/timer.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+
+
+
+/** variables and functions common to all Servo instances **/
+
+volatile unsigned long ticksPerMicrosecond;  // Holds the calculated value
+unsigned int servoAssignedMask;
+static servo_t servos[SERVOS_PER_TIMER];
+unsigned int remainderPulseWidth;
+volatile int currentServo;
+bool servoInitialized = false;
+
+// Calculate the new period remainder
+static void calculatePeriodRemainder(void)
+{
+	unsigned long servoPeriodSum = 0;
+	for (int i = 0; i < SERVOS_PER_TIMER; i++){
+		servoPeriodSum += servos[i].pulse_width;
+	}
+	remainderPulseWidth = REFRESH_INTERVAL - servoPeriodSum;
+}
+
+static void initServo(void) {
+
+	/* Work around for clock not yet up in the constructor */
+#ifdef TARGET_IS_BLIZZARD_RB1
+	ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);
+#endif
+
+	// Initialize global variables
+	ticksPerMicrosecond = 0;
+	servoAssignedMask = 0;
+	remainderPulseWidth = 0;
+	currentServo = 0;
+
+	for(int i = 0; i < SERVOS_PER_TIMER; i++)
+	{
+		servos[i].pin_number = 0;
+		servos[i].pulse_width = DEFAULT_SERVO_PULSE_WIDTH;
+		servos[i].enabled = false;
+	}
+
+	calculatePeriodRemainder();
+
+	// Enable TIMER
+	ROM_SysCtlPeripheralEnable(SERVO_TIMER_PERIPH);
+
+	// Enable processor interrupts.
+	ROM_IntMasterEnable();
+
+	TimerIntRegister(SERVO_TIMER, SERVO_TIMER_A, ServoIntHandler);
+	// Configure the TIMER
+	ROM_TimerConfigure(SERVO_TIMER, SERVO_TIME_CFG);
+
+	// Calculate the number of timer counts/microsecond
+	ticksPerMicrosecond = F_CPU / 1000000;
+
+	// Initially load the timer with 20ms interval time
+	ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * REFRESH_INTERVAL);
+
+	// Setup the interrupt for the TIMER1A timeout.
+	ROM_IntEnable(SERVO_TIMER_INTERRUPT);
+	ROM_TimerIntEnable(SERVO_TIMER, SERVO_TIMER_TRIGGER);
+
+	// Enable the timer.
+	ROM_TimerEnable(SERVO_TIMER, SERVO_TIMER_A);
+
+}
+
+/** end of static functions **/
+
+/*
+ * When a new servo is created:
+ * Initialize the servo module if it has not been initialized already.
+ * Add the servo to the assigned servos mask with a new index.
+ */
+Servo::Servo()
+{
+	// If the module has not been initialized
+	if(!servoInitialized)
+	{
+		// Initialize it.
+		initServo();
+		// It has been initialized, prevent further calls to initServo().
+		servoInitialized = true;
+	}
+
+	this->index = INVALID_SERVO;
+
+	// Look for a free servo index.
+	for (int i = 0; i < SERVOS_PER_TIMER; i++)
+	{
+		if (((servoAssignedMask >> i) & 1) == 0)
+		{
+			// Save the index for this instance of Servo.
+			this->index = i;
+
+			// Mark the spot in the mask.
+			servoAssignedMask |= (1 << i);
+
+			// Stop searching for free slots.
+			break;
+		}
+	}
+}
+
+//! Write a pulse width of the given number of microseconds to the Servo's pin
+void Servo::writeMicroseconds(int value)
+{
+	if(value < this->min) value = this->min;
+	if(value > this->max) value = this->max;
+
+	servos[this->index].pulse_width = value;
+
+	calculatePeriodRemainder();
+}
+
+//! Write a pulse width of the given degrees (if in the appropriate range to be degrees)
+//! or of the specified number of microseconds (if in the appropriate range to be microseconds)
+void Servo::write(int value)
+{
+	// treat values less than the min pulse width as angles in degrees (valid values in microseconds are handled as microseconds)
+	if(value < MIN_SERVO_PULSE_WIDTH)
+	{
+		if(value < 0) value = 0;
+		if(value > 180) value = 180;
+
+		value = map(value, 0, 180, this->min,  this->max);
+	}
+	this->writeMicroseconds(value);
+}
+
+//! Returns the current pulse width of the Servo's signal, in microseconds
+int Servo::readMicroseconds()
+{
+	return servos[this->index].pulse_width;
+}
+
+//! Returns the current position of the Servo, in degrees
+int Servo::read() // return the value as degrees
+{
+  return  map( this->readMicroseconds()+1, this->min, this->max, 0, 180);
+}
+
+//! Attach the Servo to the given pin (and, if specified, with the given range of legal pulse widths)
+unsigned int Servo::attach(unsigned int pin, int min, int max)
+{
+	this->min = min;
+	this->max = max;
+
+	servos[this->index].pin_number = pin;
+
+	pinMode(pin, OUTPUT);
+	digitalWrite(pin, LOW);
+
+	calculatePeriodRemainder();
+
+	servos[this->index].enabled = true;
+
+	return this->index;
+}
+
+//! Detach the Servo from its pin
+void Servo::detach()
+{
+    // Disable, clean up
+	servos[this->index].enabled = false;
+	servos[this->index].pulse_width = DEFAULT_SERVO_PULSE_WIDTH;
+	calculatePeriodRemainder();
+
+	digitalWrite(servos[this->index].pin_number, LOW);
+}
+
+bool Servo::attached(){
+	return servos[this->index].enabled;
+}
+
+//! ISR for generating the pulse widths
+void ServoIntHandler(void)
+{
+	// Clear the timer interrupt.
+	ROM_TimerIntClear(SERVO_TIMER, SERVO_TIMER_TRIGGER);
+
+	// Get the pulse width value for the current servo from the array
+	// and reload the timer with the new pulse width count value
+	// if we have already serviced all servos (currentServo = MAX_SERVO_NO)
+	// then this value should be the 20ms period value
+	if(currentServo < SERVOS_PER_TIMER)
+	{
+		ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * servos[currentServo].pulse_width);
+	}
+	else
+	{
+		ROM_TimerLoadSet(SERVO_TIMER, SERVO_TIMER_A, ticksPerMicrosecond * remainderPulseWidth);
+	}
+
+	// End the servo pulse set previously (if any)
+	if(currentServo > 0)  // If not the 1st Servo....
+	{
+		if (servos[currentServo - 1].enabled)
+		{
+			digitalWrite(servos[currentServo - 1].pin_number, LOW);
+		}
+	}
+
+	// Set the current servo pin HIGH
+	if(currentServo < SERVOS_PER_TIMER)
+	{
+		if (servos[currentServo - 1].enabled)
+		{
+			digitalWrite(servos[currentServo].pin_number, HIGH);
+		}
+		currentServo++;  // Advance to next servo for processing next time
+	}
+	else
+	{
+		currentServo = 0; // Start all over again
+	}
+}
diff --git a/Firmware/Tiva C/StandardFirmata/Servo.h b/Firmware/Tiva C/StandardFirmata/Servo.h
new file mode 100644
index 0000000..1ed6c78
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/Servo.h
@@ -0,0 +1,54 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include "Energia.h"
+#include <inttypes.h>
+
+// Hardware limitations information
+#define MIN_SERVO_PULSE_WIDTH 		544
+#define MAX_SERVO_PULSE_WIDTH 		2400
+#define DEFAULT_SERVO_PULSE_WIDTH   1500
+#define REFRESH_INTERVAL 		    20000
+
+// Aliases for timer config and loading
+#define SERVO_TIMER				TIMER2_BASE
+#define SERVO_TIME_CFG			TIMER_CFG_PERIODIC
+#define SERVO_TIMER_TRIGGER		TIMER_TIMA_TIMEOUT
+#define SERVO_TIMER_INTERRUPT	INT_TIMER2A
+#define SERVO_TIMER_A			TIMER_A
+#define SERVO_TIMER_PERIPH		SYSCTL_PERIPH_TIMER2
+
+// Other defines
+#define SERVOS_PER_TIMER 	8
+#define INVALID_SERVO 		255
+#define MAX_SERVOS 8
+
+
+typedef struct
+{
+    unsigned int pin_number;
+    unsigned int pulse_width;
+    bool enabled;
+} servo_t;
+
+class Servo
+{
+private:
+    unsigned int index;
+    int min;
+    int max;
+public:
+    Servo();
+    unsigned int attach(unsigned int pin, int min = MIN_SERVO_PULSE_WIDTH, int max = MAX_SERVO_PULSE_WIDTH);
+    void detach();
+    void writeMicroseconds(int value);
+    int readMicroseconds();
+    void write(int value);
+    int read();
+    bool attached();
+
+};
+
+extern "C" void ServoIntHandler(void);
+
+#endif // SERVO_H
diff --git a/Firmware/Tiva C/StandardFirmata/StandardFirmata.ino b/Firmware/Tiva C/StandardFirmata/StandardFirmata.ino
new file mode 100644
index 0000000..84e51b8
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/StandardFirmata.ino
@@ -0,0 +1,864 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please click on the following link
+  to open the list of Firmata client libraries in your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated October 16th, 2016
+*/
+
+#include "Servo.h"
+#include <Wire.h>
+#include "Firmata.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+// Forward declare a few functions to avoid compiler errors with older versions
+// of the Arduino IDE.
+void setPinModeCallback(byte, int);
+void reportAnalogCallback(byte analogPin, int value);
+void sysexCallback(byte, byte, byte*);
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin)/*, minPulse, maxPulse*/);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
+          // Disable PWM if pin mode was previously set to PWM.
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW);
+        }
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_SERVO(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(/*DEFAULT_PWM_RESOLUTION*/8);
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          switch(analogPin){
+            case 0:
+              Firmata.sendAnalog(analogPin, analogRead(A0));
+              break;
+            case 1:
+              Firmata.sendAnalog(analogPin, analogRead(A1));
+              break;
+            case 2:
+              Firmata.sendAnalog(analogPin, analogRead(A2));
+              break;
+           case 3:
+              Firmata.sendAnalog(analogPin, analogRead(A3));
+              break;
+           case 4:
+              Firmata.sendAnalog(analogPin, analogRead(A4));
+              break;
+           case 5:
+              Firmata.sendAnalog(analogPin, analogRead(A5));
+              break;
+           case 6:
+              Firmata.sendAnalog(analogPin, analogRead(A6));
+              break;
+           case 7:
+              Firmata.sendAnalog(analogPin, analogRead(A7));
+              break;
+           case 8:
+              Firmata.sendAnalog(analogPin, analogRead(A8));
+              break;
+           case 9:
+              Firmata.sendAnalog(analogPin, analogRead(A9));
+              break;         
+            case 10:
+              Firmata.sendAnalog(analogPin, analogRead(A10));
+              break;
+            case 11:
+              Firmata.sendAnalog(analogPin, analogRead(A11));
+              break;  
+            default:
+              Firmata.sendAnalog(analogPin, analogRead(analogPin));
+              break;  
+           }
+          
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
diff --git a/Firmware/Tiva C/StandardFirmata/TM4C123GH6PM_PinDiagram.jpeg b/Firmware/Tiva C/StandardFirmata/TM4C123GH6PM_PinDiagram.jpeg
new file mode 100644
index 0000000..7bb051d
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/TM4C123GH6PM_PinDiagram.jpeg
diff --git a/Firmware/Tiva C/StandardFirmata/pinout_tm4c123g.txt b/Firmware/Tiva C/StandardFirmata/pinout_tm4c123g.txt
new file mode 100644
index 0000000..ee61b0d
--- /dev/null
+++ b/Firmware/Tiva C/StandardFirmata/pinout_tm4c123g.txt
@@ -0,0 +1,88 @@
+
+	PINOUT FOR TIVA C SERIES TM4C123G LAUNCHPAD
+
+===============================================================================================================
+|	PIN	|	HARDWARE	|	DIGITAL READ/WRITE	|	ANALOG READ	|	ANALOG WRITE	|	I2C (TWI)	|		SPI		|	HARDWARE SERIAL	|
+---------------------------------------------------------------------------------------------------------------
+|	1  	|	+3.3V   	|											|							|								|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	2  	|						|					PB_5				|			A11			|								|						|	CS(2)		|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	3  	|						|					PB_0				|							|								|						|					|				RX(1)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	4  	|						|					PB_1				|							|								|						|					|				TX(1)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	5  	|						|					PE_4				|			A9			|			PE_4			|		SCL(2)	|					|				RX(5)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	6  	|						|					PE_5				|			A8			|			PE_5			|		SDA(2)	|					|				TX(5)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	7  	|						|					PB_4				|			A10			|								|						|	SCK(2)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	8  	|						|					PA_5				|							|			PA_5			|						|	MOSI(0)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	9  	|						|					PA_6				|							|			PA_6			|		SCL(1)	|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	10 	|						|					PA_7				|							|			PA_7			|		SDA(1)	|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	11 	|						|					PA_2				|							|			PA_2			|						|	SCK(0)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	12 	|						|					PA_3				|							|			PA_3			|						|	CS(0)		|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	13 	|						|					PA_4				|							|			PA_4			|						|	MISO(0)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	14 	|						|					PB_6				|							|								|						|	MISO(2)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	15 	|						|					PB_7				|							|								|						|	MOSI(2)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	16 	|	RESET  		|											|							|								|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	17 	|	PUSH2   	|					PF_0				|							|								|						|	MISO(2)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	18 	|						|					PE_0				|			A3			|								|						|					|				RX(7)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	19 	|						|					PB_2				|							|			PB_2			|		SCL(0)	|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	20 	|	GND				|											|							|								|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	21 	|	VBUS			|											|							|								|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	22 	|	GND				|											|							|								|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	23 	|						|					PD_0				|			A7			|								|		SCL(3)	|	SCK(3)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	24 	|						|					PD_1				|			A6			|								|		SDA(3)	|	CS(3)		|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	25 	|						|					PD_2				|			A5			|								|						|	MISO(3)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	26 	|						|					PD_3				|			A4			|								|						|	MOSI(3)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	27 	|						|					PE_1				|			A2			|			PE_1			|						|					|				TX(7)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	28 	|						|					PE_2				|			A1			|			PE_2			|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	29 	|						|					PE_3				|			A0			|			PE_3			|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	30 	|	RED_LED		|					PF_1				|							|								|						|	MOSI(1)	|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	31 	|	PUSH1			|					PF_4				|							|			PF_4			|						|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	32 	|						|					PD_7				|							|			PD_7			|						|					|				TX(2)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	33 	|						|					PD_6				|							|			PD_6			|						|					|				RX(2)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	34 	|						|					PC_7				|							|			PC_7			|						|					|				TX(3)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	35 	|						|					PC_6				|							|			PC_6			|						|					|				RX(3)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	36 	|						|					PC_5				|							|			PC_5			|						|					|				TX(1)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	37 	|						|					PC_4				|							|			PC_4			|						|					|				RX(1)			|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	38 	|						|					PB_3				|							|			PB_3			|		SDA(0)	|					|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	39 	|	GREEN_LED	|					PF_3				|							|			PF_3			|						|	CS(1)		|									|
+|-----|-----------|---------------------|-------------|---------------|-----------|---------|-----------------|
+|	40 	|	BLUE_LED	|					PF_2				|							|			PF_2			|						|	SCK(1)	|									|
+---------------------------------------------------------------------------------------------------------------
+===============================================================================================================
+
diff --git a/Firmware/pidmata3.ino b/Firmware/pidmata3.ino
new file mode 100644
index 0000000..01a4710
--- /dev/null
+++ b/Firmware/pidmata3.ino
@@ -0,0 +1,904 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please click on the following link
+  to open the list of Firmata client libraries in your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated August 17th, 2017
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+#include <AutoPID.h>
+
+//Comment the line below, "#define PID", if you are not using PID else uncomment it and follow further instructions.
+#define PID
+
+//------------PID values----------------
+// Pin 6  - setPoint  (input value)
+// Pin 5  - feedback (input value)
+// Pin 15 - control (output value)
+
+//If you use real analog pin for input for PID just uncomment next line with #define REAL_INPUT, if use virtual sources comment this line
+//If you use both real will be prefered
+//#define REAL_INPUT
+// This line define analog pin for read values from real sources
+#define AREAD_PIN A1
+
+
+#define PID_OUTPUT_MIN -512.0
+#define PID_OUTPUT_MAX 512.0
+#define PID_KP 15
+#define PID_KI 10
+#define PID_KD 10
+#define PID_BANG_BANG 40
+#define PID_INTERVAL  100
+
+
+
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+
+double currentPoint=0.0, setPoint=0.0, control_value=0.0;
+
+#ifdef PID
+  AutoPID myPID(&currentPoint, &setPoint, &control_value, PID_OUTPUT_MIN, PID_OUTPUT_MAX, PID_KP, PID_KI, PID_KD);
+#endif
+
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+// Forward declare a few functions to avoid compiler errors with older versions
+// of the Arduino IDE.
+void setPinModeCallback(byte, int);
+void reportAnalogCallback(byte analogPin, int value);
+void sysexCallback(byte, byte, byte*);
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+ /*==============================================================================
+ * PID
+ *==============================================================================*/
+
+void update_setPoint(int value)
+{
+  
+   #ifdef REAL_INPUT
+     setPoint=(double)analogRead(AREAD_PIN);
+   #else
+     setPoint=(double)(value-512);
+    #endif
+ }
+
+void update_currentPoint(int value)
+{
+  currentPoint=(double)(value-512); 
+}
+  
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
+          // Disable PWM if pin mode was previously set to PWM.
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW);
+        }
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          //analogWrite(PIN_TO_PWM(pin), value);
+          switch(pin)
+          {
+            case 5:
+            update_currentPoint(value);
+            break;
+            case 6:
+            update_setPoint(value);
+            }
+          
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (argc > 1 && delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(DEFAULT_PWM_RESOLUTION);
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+   
+  #ifdef PID
+   //if temperature is more than 4  below or above setpoint, OUTPUT will be set to min or max respectively
+    myPID.setBangBang(PID_BANG_BANG);
+    //set PID update interval to 
+    myPID.setTimeStep(PID_INTERVAL);
+  #endif
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+    #ifdef PID
+       #ifdef REAL_INPUT
+         update_setPoint(1);
+       #endif    
+       myPID.run(); //call every loop, updates automatically at certain time interval
+    #endif
+  
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          #ifdef PID
+            Firmata.sendAnalog(analogPin, (int)(control_value+512.0));
+          #else
+            Firmata.sendAnalog(analogPin, analogRead(analogPin));
+          #endif
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
diff --git a/Firmware/pidmata3/pidmata3.ino b/Firmware/pidmata3/pidmata3.ino
new file mode 100644
index 0000000..01a4710
--- /dev/null
+++ b/Firmware/pidmata3/pidmata3.ino
@@ -0,0 +1,904 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please click on the following link
+  to open the list of Firmata client libraries in your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated August 17th, 2017
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+#include <AutoPID.h>
+
+//Comment the line below, "#define PID", if you are not using PID else uncomment it and follow further instructions.
+#define PID
+
+//------------PID values----------------
+// Pin 6  - setPoint  (input value)
+// Pin 5  - feedback (input value)
+// Pin 15 - control (output value)
+
+//If you use real analog pin for input for PID just uncomment next line with #define REAL_INPUT, if use virtual sources comment this line
+//If you use both real will be prefered
+//#define REAL_INPUT
+// This line define analog pin for read values from real sources
+#define AREAD_PIN A1
+
+
+#define PID_OUTPUT_MIN -512.0
+#define PID_OUTPUT_MAX 512.0
+#define PID_KP 15
+#define PID_KI 10
+#define PID_KD 10
+#define PID_BANG_BANG 40
+#define PID_INTERVAL  100
+
+
+
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+
+double currentPoint=0.0, setPoint=0.0, control_value=0.0;
+
+#ifdef PID
+  AutoPID myPID(&currentPoint, &setPoint, &control_value, PID_OUTPUT_MIN, PID_OUTPUT_MAX, PID_KP, PID_KI, PID_KD);
+#endif
+
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+// Forward declare a few functions to avoid compiler errors with older versions
+// of the Arduino IDE.
+void setPinModeCallback(byte, int);
+void reportAnalogCallback(byte analogPin, int value);
+void sysexCallback(byte, byte, byte*);
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+ /*==============================================================================
+ * PID
+ *==============================================================================*/
+
+void update_setPoint(int value)
+{
+  
+   #ifdef REAL_INPUT
+     setPoint=(double)analogRead(AREAD_PIN);
+   #else
+     setPoint=(double)(value-512);
+    #endif
+ }
+
+void update_currentPoint(int value)
+{
+  currentPoint=(double)(value-512); 
+}
+  
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
+          // Disable PWM if pin mode was previously set to PWM.
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW);
+        }
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          //analogWrite(PIN_TO_PWM(pin), value);
+          switch(pin)
+          {
+            case 5:
+            update_currentPoint(value);
+            break;
+            case 6:
+            update_setPoint(value);
+            }
+          
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (argc > 1 && delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(DEFAULT_PWM_RESOLUTION);
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+   
+  #ifdef PID
+   //if temperature is more than 4  below or above setpoint, OUTPUT will be set to min or max respectively
+    myPID.setBangBang(PID_BANG_BANG);
+    //set PID update interval to 
+    myPID.setTimeStep(PID_INTERVAL);
+  #endif
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+    #ifdef PID
+       #ifdef REAL_INPUT
+         update_setPoint(1);
+       #endif    
+       myPID.run(); //call every loop, updates automatically at certain time interval
+    #endif
+  
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          #ifdef PID
+            Firmata.sendAnalog(analogPin, (int)(control_value+512.0));
+          #else
+            Firmata.sendAnalog(analogPin, analogRead(analogPin));
+          #endif
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}