initial commit

2018-08-08 08:33:26 +02:00
commit ada8fdbdf9
368 changed files with 65878 additions and 0 deletions
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogFirmata.cpp
@@ -0,0 +1,32 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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.
+*/
+
+#include <ConfigurableFirmata.h>
+#include "AnalogFirmata.h"
+
+boolean handleAnalogFirmataSysex(byte command, byte argc, byte* argv)
+{
+  if (command == 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);
+    return true;
+  }
+  return false;
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogFirmata.h
@@ -0,0 +1,24 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 AnalogFirmata_h
+#define AnalogFirmata_h
+
+#include <ConfigurableFirmata.h>
+
+boolean handleAnalogFirmataSysex(byte command, byte argc, byte* argv);
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogInputFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogInputFirmata.cpp
@@ -0,0 +1,109 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 by Jeff Hoefs: November 22nd, 2015
+*/
+
+#include <ConfigurableFirmata.h>
+#include "AnalogFirmata.h"
+#include "AnalogInputFirmata.h"
+
+AnalogInputFirmata *AnalogInputFirmataInstance;
+
+void reportAnalogInputCallback(byte analogPin, int value)
+{
+  AnalogInputFirmataInstance->reportAnalog(analogPin, value);
+}
+
+AnalogInputFirmata::AnalogInputFirmata()
+{
+  AnalogInputFirmataInstance = this;
+  analogInputsToReport = 0;
+  Firmata.attach(REPORT_ANALOG, reportAnalogInputCallback);
+}
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void AnalogInputFirmata::reportAnalog(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 (!Firmata.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
+}
+
+boolean AnalogInputFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_ANALOG(pin)) {
+    if (mode == PIN_MODE_ANALOG) {
+      reportAnalog(PIN_TO_ANALOG(pin), 1); // turn on reporting
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
+      }
+      return true;
+    } else {
+      reportAnalog(PIN_TO_ANALOG(pin), 0); // turn off reporting
+    }
+  }
+  return false;
+}
+
+void AnalogInputFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_ANALOG(pin)) {
+    Firmata.write(PIN_MODE_ANALOG);
+    Firmata.write(10); // 10 = 10-bit resolution
+  }
+}
+
+boolean AnalogInputFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  return handleAnalogFirmataSysex(command, argc, argv);
+}
+
+void AnalogInputFirmata::reset()
+{
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+}
+
+void AnalogInputFirmata::report()
+{
+  byte pin, analogPin;
+  /* 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)) {
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogInputFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogInputFirmata.h
@@ -0,0 +1,42 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 AnalogInputFirmata_h
+#define AnalogInputFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+#include "FirmataReporting.h"
+
+void reportAnalogInputCallback(byte analogPin, int value);
+
+class AnalogInputFirmata: public FirmataFeature
+{
+  public:
+    AnalogInputFirmata();
+    void reportAnalog(byte analogPin, int value);
+    void handleCapability(byte pin);
+    boolean handlePinMode(byte pin, int mode);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void reset();
+    void report();
+
+  private:
+    /* analog inputs */
+    int analogInputsToReport; // bitwise array to store pin reporting
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogOutputFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogOutputFirmata.cpp
@@ -0,0 +1,65 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 by Jeff Hoefs: November 22nd, 2015
+*/
+
+#include <ConfigurableFirmata.h>
+#include "AnalogFirmata.h"
+#include "AnalogOutputFirmata.h"
+
+AnalogOutputFirmata::AnalogOutputFirmata()
+{
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+}
+
+void AnalogOutputFirmata::reset()
+{
+
+}
+
+boolean AnalogOutputFirmata::handlePinMode(byte pin, int mode)
+{
+  if (mode == PIN_MODE_PWM && IS_PIN_PWM(pin)) {
+    pinMode(PIN_TO_PWM(pin), OUTPUT);
+    analogWrite(PIN_TO_PWM(pin), 0);
+    return true;
+  }
+  return false;
+}
+
+void AnalogOutputFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_PWM(pin)) {
+    Firmata.write(PIN_MODE_PWM);
+    Firmata.write(8); // 8 = 8-bit resolution
+  }
+}
+
+boolean AnalogOutputFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  if (command == 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);
+      return true;
+    }
+    return false;
+  } else {
+    return handleAnalogFirmataSysex(command, argc, argv);
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogOutputFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogOutputFirmata.h
@@ -0,0 +1,38 @@
+/*
+  AnalogFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 AnalogOutputFirmata_h
+#define AnalogOutputFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+
+// analogWriteCallback is defined in AnalogWrite.h but must also be declared here in order
+// for AnalogOutputFirmata to compile
+void analogWriteCallback(byte pin, int value);
+
+class AnalogOutputFirmata: public FirmataFeature
+{
+  public:
+    AnalogOutputFirmata();
+    void handleCapability(byte pin);
+    boolean handlePinMode(byte pin, int mode);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void reset();
+  private:
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/AnalogWrite.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/AnalogWrite.h
@@ -0,0 +1,51 @@
+/*
+  AnalogWrite.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 by Jeff Hoefs: November 15th, 2015
+*/
+
+#ifndef AnalogWrite_h
+#define AnalogWrite_h
+
+#include <ConfigurableFirmata.h>
+
+#if defined AnalogOutputFirmata_h || defined ServoFirmata_h
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+#ifdef ServoFirmata_h
+      case PIN_MODE_SERVO:
+        if (IS_PIN_SERVO(pin)) {
+          servoAnalogWrite(pin, value);
+          Firmata.setPinState(pin, value);
+        }
+        break;
+#endif
+#ifdef AnalogOutputFirmata_h
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin)) {
+          analogWrite(PIN_TO_PWM(pin), value);
+          Firmata.setPinState(pin, value);
+        }
+        break;
+#endif
+    }
+  }
+}
+
+#endif
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/DigitalInputFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/DigitalInputFirmata.cpp
@@ -0,0 +1,127 @@
+/*
+  DigitalInputFirmata.cpp - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 by Jeff Hoefs: November 15th, 2015
+*/
+
+#include <ConfigurableFirmata.h>
+#include "DigitalInputFirmata.h"
+
+DigitalInputFirmata *DigitalInputFirmataInstance;
+
+void reportDigitalInputCallback(byte port, int value)
+{
+  DigitalInputFirmataInstance->reportDigital(port, value);
+}
+
+DigitalInputFirmata::DigitalInputFirmata()
+{
+  DigitalInputFirmataInstance = this;
+  Firmata.attach(REPORT_DIGITAL, reportDigitalInputCallback);
+}
+
+boolean DigitalInputFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  return false;
+}
+
+void DigitalInputFirmata::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 DigitalInputFirmata::report(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);
+}
+
+void DigitalInputFirmata::reportDigital(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    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
+}
+
+boolean DigitalInputFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+      if (mode == INPUT) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);
+      } else {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      return true;
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  return false;
+}
+
+void DigitalInputFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_DIGITAL(pin)) {
+    Firmata.write((byte)INPUT);
+    Firmata.write(1);
+    Firmata.write((byte)PIN_MODE_PULLUP);
+    Firmata.write(1);
+  }
+}
+
+void DigitalInputFirmata::reset()
+{
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;      // by default, reporting off
+    portConfigInputs[i] = 0;    // until activated
+    previousPINs[i] = 0;
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/DigitalInputFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/DigitalInputFirmata.h
@@ -0,0 +1,46 @@
+/*
+  DigitalInputFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 DigitalInputFirmata_h
+#define DigitalInputFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+
+void reportDigitalInputCallback(byte port, int value);
+
+class DigitalInputFirmata: public FirmataFeature
+{
+  public:
+    DigitalInputFirmata();
+    void reportDigital(byte port, int value);
+    void report(void);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    boolean handlePinMode(byte pin, int mode);
+    void reset();
+
+  private:
+    /* 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
+    void outputPort(byte portNumber, byte portValue, byte forceSend);
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/DigitalOutputFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/DigitalOutputFirmata.cpp
@@ -0,0 +1,106 @@
+/*
+  DigitalOutputFirmata.cpp - Firmata library
+  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) 2013 Norbert Truchsess. 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 by Jeff Hoefs: February 16th, 2016
+*/
+
+#include <ConfigurableFirmata.h>
+#include "DigitalOutputFirmata.h"
+
+DigitalOutputFirmata *DigitalOutputFirmataInstance;
+
+void digitalOutputWriteCallback(byte port, int value)
+{
+  DigitalOutputFirmataInstance->digitalWritePort(port, value);
+}
+
+/*
+ * 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 handleSetPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      digitalWrite(pin, value);
+      Firmata.setPinState(pin, value);
+    }
+  }
+}
+
+DigitalOutputFirmata::DigitalOutputFirmata()
+{
+  DigitalOutputFirmataInstance = this;
+  Firmata.attach(DIGITAL_MESSAGE, digitalOutputWriteCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, handleSetPinValueCallback);
+}
+
+boolean DigitalOutputFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  return false;
+}
+
+void DigitalOutputFirmata::reset()
+{
+
+}
+
+void DigitalOutputFirmata::digitalWritePort(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) {
+            pinMode(pin, INPUT_PULLUP);
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+boolean DigitalOutputFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_DIGITAL(pin) && mode == OUTPUT && Firmata.getPinMode(pin) != PIN_MODE_IGNORE) {
+    digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+    pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+    return true;
+  }
+  return false;
+}
+
+void DigitalOutputFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_DIGITAL(pin)) {
+    Firmata.write((byte)OUTPUT);
+    Firmata.write(1);
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/DigitalOutputFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/DigitalOutputFirmata.h
@@ -0,0 +1,40 @@
+/*
+  DigitalOutputFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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 by Jeff Hoefs: November 15th, 2015
+*/
+
+#ifndef DigitalOutputFirmata_h
+#define DigitalOutputFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+
+void digitalOutputWriteCallback(byte port, int value);
+void handleSetPinValueCallback(byte pin, int value);
+
+class DigitalOutputFirmata: public FirmataFeature
+{
+  public:
+    DigitalOutputFirmata();
+    void digitalWritePort(byte port, int value);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    boolean handlePinMode(byte pin, int mode);
+    void reset();
+  private:
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/Encoder7Bit.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/Encoder7Bit.cpp
@@ -0,0 +1,64 @@
+/*
+  Encoder7Bit.cpp - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. 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.
+*/
+
+#include "Encoder7Bit.h"
+#include <ConfigurableFirmata.h>
+
+Encoder7BitClass::Encoder7BitClass()
+{
+  previous = 0;
+  shift = 0;
+}
+
+void Encoder7BitClass::startBinaryWrite()
+{
+  shift = 0;
+}
+
+void Encoder7BitClass::endBinaryWrite()
+{
+  if (shift > 0) {
+    Firmata.write(previous);
+  }
+}
+
+void Encoder7BitClass::writeBinary(byte data)
+{
+  if (shift == 0) {
+    Firmata.write(data & 0x7f);
+    shift++;
+    previous = data >> 7;
+  }
+  else {
+    Firmata.write(((data << shift) & 0x7f) | previous);
+    if (shift == 6) {
+      Firmata.write(data >> 1);
+      shift = 0;
+    }
+    else {
+      shift++;
+      previous = data >> (8 - shift);
+    }
+  }
+}
+
+void Encoder7BitClass::readBinary(int outBytes, byte *inData, byte *outData)
+{
+  for (int i = 0; i < outBytes; i++) {
+    int j = i << 3;
+    int pos = j / 7;
+    byte shift = j % 7;
+    outData[i] = (inData[pos] >> shift) | ((inData[pos + 1] << (7 - shift)) & 0xFF);
+  }
+}
+
+Encoder7BitClass Encoder7Bit;
--- a/libraries/FirmataWithDeviceFeature/src-features/Encoder7Bit.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/Encoder7Bit.h
@@ -0,0 +1,35 @@
+/*
+  Encoder7Bit.h - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. 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 Encoder7Bit_h
+#define Encoder7Bit_h
+#include <Arduino.h>
+
+#define num7BitOutbytes(a)(((a)*7)>>3)
+
+class Encoder7BitClass
+{
+  public:
+    Encoder7BitClass();
+    void startBinaryWrite();
+    void endBinaryWrite();
+    void writeBinary(byte data);
+    void readBinary(int outBytes, byte *inData, byte *outData);
+
+  private:
+    byte previous;
+    int shift;
+};
+
+extern Encoder7BitClass Encoder7Bit;
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/EthernetClientStream.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/EthernetClientStream.cpp
@@ -0,0 +1,103 @@
+/*
+  EthernetClientStream.cpp
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. 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.
+ */
+
+#include "EthernetClientStream.h"
+#include <Arduino.h>
+
+#define MILLIS_RECONNECT 5000
+
+EthernetClientStream::EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port)
+: client(client),
+  localip(localip),
+  ip(ip),
+  host(host),
+  port(port),
+  connected(false)
+{
+}
+
+int
+EthernetClientStream::available()
+{
+  return maintain() ? client.available() : 0;
+}
+
+int
+EthernetClientStream::read()
+{
+  return maintain() ? client.read() : -1;
+}
+
+int
+EthernetClientStream::peek()
+{
+  return maintain() ? client.peek() : -1;
+}
+
+void EthernetClientStream::flush()
+{
+  if (maintain())
+    client.flush();
+}
+
+size_t
+EthernetClientStream::write(uint8_t c)
+{
+  return maintain() ? client.write(c) : 0;
+}
+
+void
+EthernetClientStream::maintain(IPAddress localip)
+{
+// temporary hack to Firmata to compile for Intel Galileo
+// the issue is documented here: https://github.com/firmata/arduino/issues/218
+#if !defined(ARDUINO_LINUX)
+  if (this->localip!=localip)
+    {
+      this->localip = localip;
+      if (connected)
+        stop();
+    }
+#endif
+}
+
+void
+EthernetClientStream::stop()
+{
+  client.stop();
+  connected = false;
+  time_connect = millis();
+}
+
+bool
+EthernetClientStream::maintain()
+{
+  if (client && client.connected())
+    return true;
+
+  if (connected)
+  {
+    stop();
+  }
+  else if (millis() - time_connect >= MILLIS_RECONNECT)
+  {
+    connected = host ? client.connect(host, port) : client.connect(ip, port);
+    if (!connected)
+      time_connect = millis();
+  }
+  return connected;
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/EthernetClientStream.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/EthernetClientStream.h
@@ -0,0 +1,50 @@
+/*
+  EthernetClientStream.h
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. 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 ETHERNETCLIENTSTREAM_H
+#define ETHERNETCLIENTSTREAM_H
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <Stream.h>
+#include <Client.h>
+#include <IPAddress.h>
+
+class EthernetClientStream : public Stream
+{
+public:
+  EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port);
+  int available();
+  int read();
+  int peek();
+  void flush();
+  size_t write(uint8_t);
+  void maintain(IPAddress localip);
+
+private:
+  Client &client;
+  IPAddress localip;
+  IPAddress ip;
+  const char* host;
+  uint16_t port;
+  bool connected;
+  uint32_t time_connect;
+  bool maintain();
+  void stop();
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/FirmataReporting.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/FirmataReporting.cpp
@@ -0,0 +1,66 @@
+/*
+  FirmataReporting.cpp - Firmata library
+  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) 2013 Norbert Truchsess. 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.
+*/
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+#include "FirmataReporting.h"
+
+void FirmataReporting::setSamplingInterval(int interval)
+{
+  samplingInterval = interval;
+}
+
+void FirmataReporting::handleCapability(byte pin)
+{
+
+}
+
+boolean FirmataReporting::handlePinMode(byte pin, int mode)
+{
+  return false;
+}
+
+boolean FirmataReporting::handleSysex(byte command, byte argc, byte* argv)
+{
+  if (command == SAMPLING_INTERVAL) {
+    if (argc > 1) {
+      samplingInterval = argv[0] + (argv[1] << 7);
+      if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+        samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+      }
+      return true;
+    }
+  }
+  return false;
+}
+
+boolean FirmataReporting::elapsed()
+{
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    if (currentMillis - previousMillis > samplingInterval)
+      previousMillis = currentMillis - samplingInterval;
+    return true;
+  }
+  return false;
+}
+
+void FirmataReporting::reset()
+{
+  previousMillis = millis();
+  samplingInterval = 19;
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/FirmataReporting.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/FirmataReporting.h
@@ -0,0 +1,43 @@
+/*
+  FirmataReporting.h - Firmata library
+  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) 2013 Norbert Truchsess. 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 by Jeff Hoefs: January 23rd, 2016
+*/
+
+#ifndef FirmataReporting_h
+#define FirmataReporting_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+
+#define MINIMUM_SAMPLING_INTERVAL 1
+
+class FirmataReporting: public FirmataFeature
+{
+  public:
+    void setSamplingInterval(int interval);
+    void handleCapability(byte pin); //empty method
+    boolean handlePinMode(byte pin, int mode); //empty method
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    boolean elapsed();
+    void reset();
+  private:
+    /* timer variables */
+    unsigned long currentMillis;        // store the current value from millis()
+    unsigned long previousMillis;       // for comparison with currentMillis
+    unsigned int samplingInterval;          // how often to run the main loop (in ms)
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/FirmataScheduler.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/FirmataScheduler.cpp
@@ -0,0 +1,300 @@
+/*
+  FirmataScheduler.cpp - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. 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.
+*/
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+#include "Encoder7Bit.h"
+#include "FirmataScheduler.h"
+#include "FirmataExt.h"
+
+FirmataScheduler *FirmataSchedulerInstance;
+
+void delayTaskCallback(long delay)
+{
+  FirmataSchedulerInstance->delayTask(delay);
+}
+
+FirmataScheduler::FirmataScheduler()
+{
+  FirmataSchedulerInstance = this;
+  tasks = NULL;
+  running = NULL;
+  Firmata.attachDelayTask(delayTaskCallback);
+}
+
+void FirmataScheduler::handleCapability(byte pin)
+{
+
+}
+
+boolean FirmataScheduler::handlePinMode(byte pin, int mode)
+{
+  return false;
+}
+
+boolean FirmataScheduler::handleSysex(byte command, byte argc, byte* argv)
+{
+  if (command == SCHEDULER_DATA) {
+    if (argc > 0) {
+      switch (argv[0]) {
+        case CREATE_FIRMATA_TASK:
+          {
+            if (argc == 4) {
+              createTask(argv[1], argv[2] | argv[3] << 7);
+            }
+            break;
+          }
+        case DELETE_FIRMATA_TASK:
+          {
+            if (argc == 2) {
+              deleteTask(argv[1]);
+            }
+            break;
+          }
+        case ADD_TO_FIRMATA_TASK:
+          {
+            if (argc > 2) {
+              int len = num7BitOutbytes(argc - 2);
+              Encoder7Bit.readBinary(len, argv + 2, argv + 2); //decode inplace
+              addToTask(argv[1], len, argv + 2); //addToTask copies data...
+            }
+            break;
+          }
+        case DELAY_FIRMATA_TASK:
+          {
+            if (argc == 6) {
+              argv++;
+              Encoder7Bit.readBinary(4, argv, argv); //decode inplace
+              delayTask(*(long*)((byte*)argv));
+            }
+            break;
+          }
+        case SCHEDULE_FIRMATA_TASK:
+          {
+            if (argc == 7) { //one byte taskid, 5 bytes to encode 4 bytes of long
+              Encoder7Bit.readBinary(4, argv + 2, argv + 2); //decode inplace
+              schedule(argv[1], *(long*)((byte*)argv + 2)); //argv[1] | argv[2]<<8 | argv[3]<<16 | argv[4]<<24
+            }
+            break;
+          }
+        case QUERY_ALL_FIRMATA_TASKS:
+          {
+            queryAllTasks();
+            break;
+          }
+        case QUERY_FIRMATA_TASK:
+          {
+            if (argc == 2) {
+              queryTask(argv[1]);
+            }
+            break;
+          }
+        case RESET_FIRMATA_TASKS:
+          {
+            reset();
+          }
+      }
+    }
+    return true;
+  }
+  return false;
+};
+
+void FirmataScheduler::createTask(byte id, int len)
+{
+  firmata_task *existing = findTask(id);
+  if (existing) {
+    reportTask(id, existing, true);
+  }
+  else {
+    firmata_task *newTask = (firmata_task*)malloc(sizeof(firmata_task) + len);
+    newTask->id = id;
+    newTask->time_ms = 0;
+    newTask->len = len;
+    newTask->nextTask = tasks;
+    newTask->pos = 0;
+    tasks = newTask;
+  }
+};
+
+void FirmataScheduler::deleteTask(byte id)
+{
+  firmata_task *current = tasks;
+  firmata_task *previous = NULL;
+  while (current) {
+    if (current->id == id) {
+      if (previous) {
+        previous->nextTask = current->nextTask;
+      }
+      else {
+        tasks = current->nextTask;
+      }
+      free (current);
+      return;
+    }
+    else {
+      previous = current;
+      current = current->nextTask;
+    }
+  }
+};
+
+void FirmataScheduler::addToTask(byte id, int additionalBytes, byte *message)
+{
+  firmata_task *existing = findTask(id);
+  if (existing) { //task exists and has not been fully loaded yet
+    if (existing->pos + additionalBytes <= existing->len) {
+      for (int i = 0; i < additionalBytes; i++) {
+        existing->messages[existing->pos++] = message[i];
+      }
+    }
+  }
+  else {
+    reportTask(id, NULL, true);
+  }
+};
+
+void FirmataScheduler::schedule(byte id, long delay_ms)
+{
+  firmata_task *existing = findTask(id);
+  if (existing) {
+    existing->pos = 0;
+    existing->time_ms = millis() + delay_ms;
+  }
+  else {
+    reportTask(id, NULL, true);
+  }
+};
+
+void FirmataScheduler::delayTask(long delay_ms)
+{
+  if (running) {
+    long now = millis();
+    running->time_ms += delay_ms;
+    if (running->time_ms < now) { //if delay time allready passed by schedule to 'now'.
+      running->time_ms = now;
+    }
+  }
+}
+
+void FirmataScheduler::queryAllTasks()
+{
+  Firmata.write(START_SYSEX);
+  Firmata.write(SCHEDULER_DATA);
+  Firmata.write(QUERY_ALL_TASKS_REPLY);
+  firmata_task *task = tasks;
+  while (task) {
+    Firmata.write(task->id);
+    task = task->nextTask;
+  }
+  Firmata.write(END_SYSEX);
+};
+
+void FirmataScheduler::queryTask(byte id)
+{
+  firmata_task *task = findTask(id);
+  reportTask(id, task, false);
+}
+
+void FirmataScheduler::reportTask(byte id, firmata_task *task, boolean error)
+{
+  Firmata.write(START_SYSEX);
+  Firmata.write(SCHEDULER_DATA);
+  if (error) {
+    Firmata.write(ERROR_TASK_REPLY);
+  } else {
+    Firmata.write(QUERY_TASK_REPLY);
+  }
+  Firmata.write(id);
+  if (task) {
+    Encoder7Bit.startBinaryWrite();
+    for (int i = 3; i < firmata_task_len(task); i++) {
+      Encoder7Bit.writeBinary(((byte *)task)[i]); //don't write first 3 bytes (firmata_task*, byte); makes use of AVR byteorder (LSB first)
+    }
+    Encoder7Bit.endBinaryWrite();
+  }
+  Firmata.write(END_SYSEX);
+};
+
+void FirmataScheduler::runTasks()
+{
+  if (tasks) {
+    long now = millis();
+    firmata_task *current = tasks;
+    firmata_task *previous = NULL;
+    while (current) {
+      if (current->time_ms > 0 && current->time_ms < now) { // TODO handle overflow
+        if (execute(current)) {
+          previous = current;
+          current = current->nextTask;
+        }
+        else {
+          if (previous) {
+            previous->nextTask = current->nextTask;
+            free(current);
+            current = previous->nextTask;
+          }
+          else {
+            tasks = current->nextTask;
+            free(current);
+            current = tasks;
+          }
+        }
+      }
+      else {
+        current = current->nextTask;
+      }
+    }
+  }
+};
+
+void FirmataScheduler::reset()
+{
+  while (tasks) {
+    firmata_task *nextTask = tasks->nextTask;
+    free(tasks);
+    tasks = nextTask;
+  }
+};
+
+//private
+boolean FirmataScheduler::execute(firmata_task *task)
+{
+  long start = task->time_ms;
+  int pos = task->pos;
+  int len = task->len;
+  byte *messages = task->messages;
+  running = task;
+  while (pos < len) {
+    Firmata.parse(messages[pos++]);
+    if (start != task->time_ms) { // return true if task got rescheduled during run.
+      task->pos = ( pos == len ? 0 : pos ); // last message executed? -> start over next time
+      running = NULL;
+      return true;
+    }
+  };
+  running = NULL;
+  return false;
+}
+
+firmata_task *FirmataScheduler::findTask(byte id)
+{
+  firmata_task *currentTask = tasks;
+  while (currentTask) {
+    if (id == currentTask->id) {
+      return currentTask;
+    } else {
+      currentTask = currentTask->nextTask;
+    }
+  };
+  return NULL;
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/FirmataScheduler.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/FirmataScheduler.h
@@ -0,0 +1,73 @@
+/*
+  FirmataScheduler.h - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. 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 FirmataScheduler_h
+#define FirmataScheduler_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+#include "Encoder7Bit.h"
+
+//subcommands
+#define CREATE_FIRMATA_TASK     0
+#define DELETE_FIRMATA_TASK     1
+#define ADD_TO_FIRMATA_TASK     2
+#define DELAY_FIRMATA_TASK      3
+#define SCHEDULE_FIRMATA_TASK   4
+#define QUERY_ALL_FIRMATA_TASKS 5
+#define QUERY_FIRMATA_TASK      6
+#define RESET_FIRMATA_TASKS     7
+#define ERROR_TASK_REPLY        8
+#define QUERY_ALL_TASKS_REPLY   9
+#define QUERY_TASK_REPLY        10
+
+#define firmata_task_len(a)(sizeof(firmata_task)+(a)->len)
+
+void delayTaskCallback(long delay);
+
+struct firmata_task
+{
+  firmata_task *nextTask;
+  byte id; //only 7bits used -> supports 127 tasks
+  long time_ms;
+  int len;
+  int pos;
+  byte messages[];
+};
+
+class FirmataScheduler: public FirmataFeature
+{
+  public:
+    FirmataScheduler();
+    void handleCapability(byte pin); //empty method
+    boolean handlePinMode(byte pin, int mode); //empty method
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void runTasks();
+    void reset();
+    void createTask(byte id, int len);
+    void deleteTask(byte id);
+    void addToTask(byte id, int len, byte *message);
+    void schedule(byte id, long time_ms);
+    void delayTask(long time_ms);
+    void queryAllTasks();
+    void queryTask(byte id);
+
+  private:
+    firmata_task *tasks;
+    firmata_task *running;
+
+    boolean execute(firmata_task *task);
+    firmata_task *findTask(byte id);
+    void reportTask(byte id, firmata_task *task, boolean error);
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/I2CFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/I2CFirmata.cpp
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in I2CFirmata.h to avoid having to include Wire.h in all
+ * sketch files that include ConfigurableFirmata.h
+ */
--- a/libraries/FirmataWithDeviceFeature/src-features/I2CFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/I2CFirmata.h
@@ -0,0 +1,333 @@
+/*
+  I2CFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. 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.
+
+  I2CFirmata.cpp has been merged into this header file as a hack to avoid having to
+  include Wire.h for every arduino sketch that includes ConfigurableFirmata.
+
+  Last updated by Jeff Hoefs: January 23rd, 2015
+*/
+
+#ifndef I2CFirmata_h
+#define I2CFirmata_h
+
+#include <Wire.h>
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+#include "FirmataReporting.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
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+class I2CFirmata: public FirmataFeature
+{
+  public:
+    I2CFirmata();
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void reset();
+    void report();
+
+  private:
+    /* for i2c read continuous more */
+    i2c_device_info query[I2C_MAX_QUERIES];
+
+    byte i2cRxData[32];
+    boolean isI2CEnabled;
+    signed char queryIndex;
+    unsigned int i2cReadDelayTime;  // default delay time between i2c read request and Wire.requestFrom()
+
+    void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX);
+    void handleI2CRequest(byte argc, byte *argv);
+    boolean handleI2CConfig(byte argc, byte *argv);
+    boolean enableI2CPins();
+    void disableI2CPins();
+};
+
+
+/*
+ * I2CFirmata.cpp
+ * Copied here as a hack to avoid having to include Wire.h in all sketch files that
+ * include ConfigurableFirmata.h
+ */
+
+I2CFirmata::I2CFirmata()
+{
+  isI2CEnabled = false;
+  queryIndex = -1;
+  i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
+}
+
+void I2CFirmata::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);
+    Wire.write((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] = Wire.read();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+boolean I2CFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_I2C(pin)) {
+    if (mode == PIN_MODE_I2C) {
+      // the user must call I2C_CONFIG to enable I2C for a device
+      return true;
+    } else if (isI2CEnabled) {
+      // disable i2c so pins can be used for other functions
+      // the following if statements should reconfigure the pins properly
+      if (Firmata.getPinMode(pin) == PIN_MODE_I2C) {
+        disableI2CPins();
+      }
+    }
+  }
+  return false;
+}
+
+void I2CFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_I2C(pin)) {
+    Firmata.write(PIN_MODE_I2C);
+    Firmata.write(1); // TODO: could assign a number to map to SCL or SDA
+  }
+}
+
+boolean I2CFirmata::handleSysex(byte command, byte argc, byte *argv)
+{
+  switch (command) {
+    case I2C_REQUEST:
+      if (isI2CEnabled) {
+        handleI2CRequest(argc, argv);
+        return true;
+      }
+    case I2C_CONFIG:
+      return handleI2CConfig(argc, argv);
+  }
+  return false;
+}
+
+void I2CFirmata::handleI2CRequest(byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  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);
+        Wire.write(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;
+  }
+}
+
+boolean I2CFirmata::handleI2CConfig(byte argc, byte *argv)
+{
+  unsigned int delayTime = (argv[0] + (argv[1] << 7));
+
+  if (delayTime > 0) {
+    i2cReadDelayTime = delayTime;
+  }
+
+  if (!isI2CEnabled) {
+    enableI2CPins();
+  }
+  return isI2CEnabled;
+}
+
+boolean I2CFirmata::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)) {
+      if (Firmata.getPinMode(i) == PIN_MODE_IGNORE) {
+        return false;
+      }
+      // mark pins as i2c so they are ignore in non i2c data requests
+      Firmata.setPinMode(i, PIN_MODE_I2C);
+      pinMode(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+  return true;
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void I2CFirmata::disableI2CPins()
+{
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+  // uncomment the following if or when the end() method is added to Wire library
+  // Wire.end();
+}
+
+void I2CFirmata::reset()
+{
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+}
+
+void I2CFirmata::report()
+{
+  // 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);
+    }
+  }
+}
+
+#endif /* I2CFirmata_h */
--- a/libraries/FirmataWithDeviceFeature/src-features/OneWireFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/OneWireFirmata.cpp
@@ -0,0 +1,178 @@
+/*
+  OneWireFirmata.cpp - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 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 by Jeff Hoefs: January 23rd, 2016
+*/
+
+#include <ConfigurableFirmata.h>
+#include "OneWireFirmata.h"
+#include "Encoder7Bit.h"
+
+boolean OneWireFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_DIGITAL(pin) && mode == PIN_MODE_ONEWIRE) {
+    oneWireConfig(pin, ONEWIRE_POWER);
+    return true;
+  }
+  return false;
+}
+
+void OneWireFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_DIGITAL(pin)) {
+    Firmata.write(PIN_MODE_ONEWIRE);
+    Firmata.write(1);
+  }
+}
+
+void OneWireFirmata::oneWireConfig(byte pin, boolean power)
+{
+  ow_device_info *info = &pinOneWire[pin];
+  if (info->device == NULL) {
+    info->device = new OneWire(pin);
+  }
+  for (int i = 0; i < 8; i++) {
+    info->addr[i] = 0x0;
+  }
+  info->power = power;
+}
+
+boolean OneWireFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  if (command == ONEWIRE_DATA) {
+    if (argc > 1) {
+      byte subcommand = argv[0];
+      byte pin = argv[1];
+      ow_device_info *info = &pinOneWire[pin];
+      OneWire *device = info->device;
+      if (device || subcommand == ONEWIRE_CONFIG_REQUEST) {
+        switch (subcommand) {
+          case ONEWIRE_SEARCH_REQUEST:
+          case ONEWIRE_SEARCH_ALARMS_REQUEST:
+            {
+              device->reset_search();
+              Firmata.write(START_SYSEX);
+              Firmata.write(ONEWIRE_DATA);
+              boolean isAlarmSearch = (subcommand == ONEWIRE_SEARCH_ALARMS_REQUEST);
+              Firmata.write(isAlarmSearch ? (byte)ONEWIRE_SEARCH_ALARMS_REPLY : (byte)ONEWIRE_SEARCH_REPLY);
+              Firmata.write(pin);
+              Encoder7Bit.startBinaryWrite();
+              byte addrArray[8];
+              while (isAlarmSearch ? device->search(addrArray, false) : device->search(addrArray)) {
+                for (int i = 0; i < 8; i++) {
+                  Encoder7Bit.writeBinary(addrArray[i]);
+                }
+              }
+              Encoder7Bit.endBinaryWrite();
+              Firmata.write(END_SYSEX);
+              break;
+            }
+          case ONEWIRE_CONFIG_REQUEST:
+            {
+              if (argc == 3 && Firmata.getPinMode(pin) != PIN_MODE_IGNORE) {
+                Firmata.setPinMode(pin, PIN_MODE_ONEWIRE);
+                oneWireConfig(pin, argv[2]); // this calls oneWireConfig again, this time setting the correct config (which doesn't cause harm though)
+              } else {
+                return false;
+              }
+              break;
+            }
+          default:
+            {
+              if (subcommand & ONEWIRE_RESET_REQUEST_BIT) {
+                device->reset();
+                for (int i = 0; i < 8; i++) {
+                  info->addr[i] = 0x0;
+                }
+              }
+              if (subcommand & ONEWIRE_SKIP_REQUEST_BIT) {
+                device->skip();
+                for (byte i = 0; i < 8; i++) {
+                  info->addr[i] = 0x0;
+                }
+              }
+              if (subcommand & ONEWIRE_WITHDATA_REQUEST_BITS) {
+                int numBytes = num7BitOutbytes(argc - 2);
+                int numReadBytes = 0;
+                int correlationId;
+                argv += 2;
+                Encoder7Bit.readBinary(numBytes, argv, argv); //decode inplace
+
+                if (subcommand & ONEWIRE_SELECT_REQUEST_BIT) {
+                  if (numBytes < 8) break;
+                  device->select(argv);
+                  for (int i = 0; i < 8; i++) {
+                    info->addr[i] = argv[i];
+                  }
+                  argv += 8;
+                  numBytes -= 8;
+                }
+
+                if (subcommand & ONEWIRE_READ_REQUEST_BIT) {
+                  if (numBytes < 4) break;
+                  numReadBytes = *((int*)argv);
+                  argv += 2;
+                  correlationId = *((int*)argv);
+                  argv += 2;
+                  numBytes -= 4;
+                }
+
+                if (subcommand & ONEWIRE_DELAY_REQUEST_BIT) {
+                  if (numBytes < 4) break;
+                  Firmata.delayTask(*((long*)argv));
+                  argv += 4;
+                  numBytes -= 4;
+                }
+
+                if (subcommand & ONEWIRE_WRITE_REQUEST_BIT) {
+                  for (int i = 0; i < numBytes; i++) {
+                    info->device->write(argv[i], info->power);
+                  }
+                }
+
+                if (numReadBytes > 0) {
+                  Firmata.write(START_SYSEX);
+                  Firmata.write(ONEWIRE_DATA);
+                  Firmata.write(ONEWIRE_READ_REPLY);
+                  Firmata.write(pin);
+                  Encoder7Bit.startBinaryWrite();
+                  Encoder7Bit.writeBinary(correlationId & 0xFF);
+                  Encoder7Bit.writeBinary((correlationId >> 8) & 0xFF);
+                  for (int i = 0; i < numReadBytes; i++) {
+                    Encoder7Bit.writeBinary(device->read());
+                  }
+                  Encoder7Bit.endBinaryWrite();
+                  Firmata.write(END_SYSEX);
+                }
+              }
+            }
+        }
+      }
+      return true;
+    }
+  }
+  return false;
+}
+
+void OneWireFirmata::reset()
+{
+  for (int i = 0; i < TOTAL_PINS; i++) {
+    if (pinOneWire[i].device) {
+      free(pinOneWire[i].device);
+      pinOneWire[i].device = NULL;
+    }
+    for (int j = 0; j < 8; j++) {
+      pinOneWire[i].addr[j] = 0;
+    }
+    pinOneWire[i].power = false;
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/OneWireFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/OneWireFirmata.h
@@ -0,0 +1,63 @@
+/*
+  OneWireFirmata.h - Firmata library
+  Copyright (C) 2012-2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 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 OneWireFirmata_h
+#define OneWireFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "utility/OneWire.h"
+#include "FirmataFeature.h"
+
+//subcommands:
+#define ONEWIRE_SEARCH_REQUEST 0x40
+#define ONEWIRE_CONFIG_REQUEST 0x41
+#define ONEWIRE_SEARCH_REPLY 0x42
+#define ONEWIRE_READ_REPLY 0x43
+#define ONEWIRE_SEARCH_ALARMS_REQUEST 0x44
+#define ONEWIRE_SEARCH_ALARMS_REPLY 0x45
+
+#define ONEWIRE_RESET_REQUEST_BIT 0x01
+#define ONEWIRE_SKIP_REQUEST_BIT 0x02
+#define ONEWIRE_SELECT_REQUEST_BIT 0x04
+#define ONEWIRE_READ_REQUEST_BIT 0x08
+#define ONEWIRE_DELAY_REQUEST_BIT 0x10
+#define ONEWIRE_WRITE_REQUEST_BIT 0x20
+
+#define ONEWIRE_WITHDATA_REQUEST_BITS 0x3C
+
+#define ONEWIRE_CRC 0 //for OneWire.h: crc-functions are not used by Firmata
+
+//default value for power:
+#define ONEWIRE_POWER 1
+
+struct ow_device_info
+{
+  OneWire* device;
+  byte addr[8];
+  boolean power;
+};
+
+class OneWireFirmata: public FirmataFeature
+{
+  public:
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void reset();
+
+  private:
+    ow_device_info pinOneWire[TOTAL_PINS];
+    void oneWireConfig(byte pin, boolean power);
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/SerialFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/SerialFirmata.cpp
@@ -0,0 +1,358 @@
+/*
+  SerialFirmata.cpp - Firmata library
+  Copyright (C) 2015-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 March 6th, 2016
+*/
+
+#include "SerialFirmata.h"
+
+SerialFirmata::SerialFirmata()
+{
+  swSerial0 = NULL;
+  swSerial1 = NULL;
+  swSerial2 = NULL;
+  swSerial3 = NULL;
+
+  serialIndex = -1;
+}
+
+boolean SerialFirmata::handlePinMode(byte pin, int mode)
+{
+  if (mode == PIN_MODE_SERIAL) {
+    // nothing else to do here since the mode is set in SERIAL_CONFIG
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_SERIAL(pin)) {
+    Firmata.write(PIN_MODE_SERIAL);
+    Firmata.write(getSerialPinType(pin));
+  }
+}
+
+boolean SerialFirmata::handleSysex(byte command, byte argc, byte *argv)
+{
+  if (command == SERIAL_MESSAGE) {
+
+    Stream *serialPort;
+    byte mode = argv[0] & SERIAL_MODE_MASK;
+    byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+    if (portId >= SERIAL_READ_ARR_LEN) return false;
+
+    switch (mode) {
+      case SERIAL_CONFIG:
+        {
+          long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+          serial_pins pins;
+          lastAvailableBytes[portId] = 0;
+          lastReceive[portId] = 0;
+// this ifdef will be removed once a command to enable RX buffering has been added to the protocol
+#if defined(FIRMATA_SERIAL_PORT_RX_BUFFERING)
+          // 8N1 = 10 bits per char, max. 50 bits -> 50000 = 50bits * 1000ms/s
+          // char delay value (ms) to detect the end of a message, defaults to 50 bits * 1000 / baud rate
+          // a value of 0 will disable RX buffering, resulting in single byte transfers to the host with
+          // baud rates below approximately 56k (varies with CPU speed)
+          maxCharDelay[portId] = 50000 / baud;
+#else
+          maxCharDelay[portId] = 0;
+#endif
+          if (portId < 8) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              pins = getSerialPinNumbers(portId);
+              if (pins.rx != 0 && pins.tx != 0) {
+                Firmata.setPinMode(pins.rx, PIN_MODE_SERIAL);
+                Firmata.setPinMode(pins.tx, PIN_MODE_SERIAL);
+                // Fixes an issue where some serial devices would not work properly with Arduino Due
+                // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                pinMode(pins.rx, INPUT);
+              }
+              ((HardwareSerial*)serialPort)->begin(baud);
+            }
+          } else {
+#if defined(SoftwareSerial_h)
+            byte swTxPin, swRxPin;
+            if (argc > 4) {
+              swRxPin = argv[4];
+              swTxPin = argv[5];
+            } else {
+              // RX and TX pins must be specified when using software serial
+              Firmata.sendString("Specify serial RX and TX pins");
+              return false;
+            }
+            switch (portId) {
+              case SW_SERIAL0:
+                if (swSerial0 == NULL) {
+                  swSerial0 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL1:
+                if (swSerial1 == NULL) {
+                  swSerial1 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL2:
+                if (swSerial2 == NULL) {
+                  swSerial2 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL3:
+                if (swSerial3 == NULL) {
+                  swSerial3 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+            }
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              Firmata.setPinMode(swRxPin, PIN_MODE_SERIAL);
+              Firmata.setPinMode(swTxPin, PIN_MODE_SERIAL);
+              ((SoftwareSerial*)serialPort)->begin(baud);
+            }
+#endif
+          }
+          break; // SERIAL_CONFIG
+        }
+      case SERIAL_WRITE:
+        {
+          byte data;
+          serialPort = getPortFromId(portId);
+          if (serialPort == NULL) {
+            break;
+          }
+          for (byte i = 1; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            serialPort->write(data);
+          }
+          break; // SERIAL_WRITE
+        }
+      case SERIAL_READ:
+        if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+          if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+            break;
+          }
+
+          if (argc > 2) {
+            // maximum number of bytes to read from buffer per iteration of loop()
+            serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+          } else {
+            // read all available bytes per iteration of loop()
+            serialBytesToRead[portId] = 0;
+          }
+          serialIndex++;
+          reportSerial[serialIndex] = portId;
+        } else if (argv[1] == SERIAL_STOP_READING) {
+          byte serialIndexToSkip = 0;
+          if (serialIndex <= 0) {
+            serialIndex = -1;
+          } else {
+            for (byte i = 0; i < serialIndex + 1; i++) {
+              if (reportSerial[i] == portId) {
+                serialIndexToSkip = i;
+                break;
+              }
+            }
+            // shift elements over to fill space left by removed element
+            for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+              if (i < MAX_SERIAL_PORTS) {
+                reportSerial[i] = reportSerial[i + 1];
+              }
+            }
+            serialIndex--;
+          }
+        }
+        break; // SERIAL_READ
+      case SERIAL_CLOSE:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          if (portId < 8) {
+            ((HardwareSerial*)serialPort)->end();
+          } else {
+#if defined(SoftwareSerial_h)
+            ((SoftwareSerial*)serialPort)->end();
+            if (serialPort != NULL) {
+              free(serialPort);
+              serialPort = NULL;
+            }
+#endif
+          }
+        }
+        break; // SERIAL_CLOSE
+      case SERIAL_FLUSH:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          getPortFromId(portId)->flush();
+        }
+        break; // SERIAL_FLUSH
+#if defined(SoftwareSerial_h)
+      case SERIAL_LISTEN:
+        // can only call listen() on software serial ports
+        if (portId > 7) {
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            ((SoftwareSerial*)serialPort)->listen();
+          }
+        }
+        break; // SERIAL_LISTEN
+#endif
+    }
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::update()
+{
+  checkSerial();
+}
+
+void SerialFirmata::reset()
+{
+#if defined(SoftwareSerial_h)
+  Stream *serialPort;
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+#endif
+
+  serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
+}
+
+// get a pointer to the serial port associated with the specified port id
+Stream* SerialFirmata::getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(SoftwareSerial_h)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void SerialFirmata::checkSerial()
+{
+  byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    unsigned long currentMillis = millis();
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+#if defined(SoftwareSerial_h)
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+#endif
+      int availableBytes = serialPort->available();
+      if (availableBytes > 0) {
+        bool read = true;
+
+        // check if reading should be delayed to collect some bytes before
+        // forwarding (for baud rates significantly below 57600 baud)
+        if (maxCharDelay[portId]) {
+          // inter character delay exceeded or more than 48 bytes available or more bytes available than required
+          read = (lastAvailableBytes[portId] > 0 && (currentMillis - lastReceive[portId]) >= maxCharDelay[portId])
+                 || (bytesToRead == 0 && availableBytes >= 48) || (bytesToRead > 0 && availableBytes >= bytesToRead);
+          if (availableBytes > lastAvailableBytes[portId]) {
+            lastReceive[portId] = currentMillis;
+            lastAvailableBytes[portId] = availableBytes;
+          }
+        }
+
+        if (read) {
+          Firmata.write(START_SYSEX);
+          Firmata.write(SERIAL_MESSAGE);
+          Firmata.write(SERIAL_REPLY | portId);
+
+          if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+            numBytesToRead = serialPort->available();
+          } else {
+            numBytesToRead = bytesToRead;
+          }
+
+          if (lastAvailableBytes[portId] - numBytesToRead >= 0) {
+            lastAvailableBytes[portId] -= numBytesToRead;
+          } else {
+            lastAvailableBytes[portId] = 0;
+          }
+
+          // relay serial data to the serial device
+          while (numBytesToRead > 0) {
+            serialData = serialPort->read();
+            Firmata.write(serialData & 0x7F);
+            Firmata.write((serialData >> 7) & 0x7F);
+            numBytesToRead--;
+          }
+          Firmata.write(END_SYSEX);
+        }
+      }
+    }
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/SerialFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/SerialFirmata.h
@@ -0,0 +1,163 @@
+/*
+  SerialFirmata.h - Firmata library
+  Copyright (C) 2015-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 March 6th, 2016
+*/
+
+#ifndef SerialFirmata_h
+#define SerialFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+// SoftwareSerial is currently only supported for AVR-based boards and the Arduino 101
+// The third condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+#if defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_ARCH_ARC32) || (ARDUINO >= 100 && ARDUINO < 10500)
+#include <SoftwareSerial.h>
+#endif
+
+// uncomment FIRMATA_SERIAL_PORT_RX_BUFFERING to collect bytes received by serial port until the
+// receive buffer gets filled or a data gap is detected to avoid forwarding single bytes at baud
+// rates below 50000
+//#define FIRMATA_SERIAL_PORT_RX_BUFFERING
+
+// Serial port Ids
+#define HW_SERIAL0                  0x00
+#define HW_SERIAL1                  0x01
+#define HW_SERIAL2                  0x02
+#define HW_SERIAL3                  0x03
+// extensible up to 0x07
+
+#define SW_SERIAL0                  0x08
+#define SW_SERIAL1                  0x09
+#define SW_SERIAL2                  0x0A
+#define SW_SERIAL3                  0x0B
+// extensible up to 0x0F
+
+#define SERIAL_PORT_ID_MASK         0x0F
+#define MAX_SERIAL_PORTS            8
+#define SERIAL_READ_ARR_LEN         12
+
+// map configuration query response resolution value to serial pin type
+#define RES_RX1                     0x02
+#define RES_TX1                     0x03
+#define RES_RX2                     0x04
+#define RES_TX2                     0x05
+#define RES_RX3                     0x06
+#define RES_TX3                     0x07
+
+// Serial command bytes
+#define SERIAL_CONFIG               0x10
+#define SERIAL_WRITE                0x20
+#define SERIAL_READ                 0x30
+#define SERIAL_REPLY                0x40
+#define SERIAL_CLOSE                0x50
+#define SERIAL_FLUSH                0x60
+#define SERIAL_LISTEN               0x70
+
+// Serial read modes
+#define SERIAL_READ_CONTINUOUSLY    0x00
+#define SERIAL_STOP_READING         0x01
+#define SERIAL_MODE_MASK            0xF0
+
+struct serial_pins {
+  uint8_t rx;
+  uint8_t tx;
+};
+
+/*
+ * Get the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin.
+ */
+inline uint8_t getSerialPinType(uint8_t pin) {
+#if defined(PIN_SERIAL_RX)
+  // TODO when use of HW_SERIAL0 is enabled
+#endif
+#if defined(PIN_SERIAL1_RX)
+  if (pin == PIN_SERIAL1_RX) return RES_RX1;
+  if (pin == PIN_SERIAL1_TX) return RES_TX1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+  if (pin == PIN_SERIAL2_RX) return RES_RX2;
+  if (pin == PIN_SERIAL2_TX) return RES_TX2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+  if (pin == PIN_SERIAL3_RX) return RES_RX3;
+  if (pin == PIN_SERIAL3_TX) return RES_TX3;
+#endif
+  return 0;
+}
+
+/*
+ * Get the RX and TX pins numbers for the specified HW serial port.
+ */
+inline serial_pins getSerialPinNumbers(uint8_t portId) {
+  serial_pins pins;
+  switch (portId) {
+#if defined(PIN_SERIAL_RX)
+    // case HW_SERIAL0:
+    //   // TODO when use of HW_SERIAL0 is enabled
+    //   break;
+#endif
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      pins.rx = PIN_SERIAL1_RX;
+      pins.tx = PIN_SERIAL1_TX;
+      break;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      pins.rx = PIN_SERIAL2_RX;
+      pins.tx = PIN_SERIAL2_TX;
+      break;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      pins.rx = PIN_SERIAL3_RX;
+      pins.tx = PIN_SERIAL3_TX;
+      break;
+#endif
+    default:
+      pins.rx = 0;
+      pins.tx = 0;
+  }
+  return pins;
+}
+
+
+class SerialFirmata: public FirmataFeature
+{
+  public:
+    SerialFirmata();
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void update();
+    void reset();
+    void checkSerial();
+
+  private:
+    byte reportSerial[MAX_SERIAL_PORTS];
+    int serialBytesToRead[SERIAL_READ_ARR_LEN];
+    signed char serialIndex;
+
+    unsigned long lastReceive[SERIAL_READ_ARR_LEN];
+    unsigned char maxCharDelay[SERIAL_READ_ARR_LEN];
+    int lastAvailableBytes[SERIAL_READ_ARR_LEN];
+
+    Stream *swSerial0;
+    Stream *swSerial1;
+    Stream *swSerial2;
+    Stream *swSerial3;
+
+    Stream* getPortFromId(byte portId);
+
+};
+
+#endif /* SerialFirmata_h */
--- a/libraries/FirmataWithDeviceFeature/src-features/ServoFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/ServoFirmata.cpp
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in ServoFirmata.h to avoid having to include Servo.h in all
+ * sketch files that include ConfigurableFirmata.h
+ */
--- a/libraries/FirmataWithDeviceFeature/src-features/ServoFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/ServoFirmata.h
@@ -0,0 +1,151 @@
+/*
+  ServoFirmata.h - Firmata library
+  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) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2015 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.
+
+  ServoFirmata.cpp has been merged into this header file as a hack to avoid having to
+  include Servo.h for every arduino sketch that includes ConfigurableFirmata.
+
+  Last updated by Jeff Hoefs: November 15th, 2015
+*/
+
+#ifndef ServoFirmata_h
+#define ServoFirmata_h
+
+#include <Servo.h>
+#include <ConfigurableFirmata.h>
+#include "FirmataFeature.h"
+
+void servoAnalogWrite(byte pin, int value);
+
+class ServoFirmata: public FirmataFeature
+{
+  public:
+    ServoFirmata();
+    boolean analogWrite(byte pin, int value);
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void reset();
+  private:
+    Servo *servos[MAX_SERVOS];
+    void attach(byte pin, int minPulse, int maxPulse);
+    void detach(byte pin);
+};
+
+
+/*
+ * ServoFirmata.cpp
+ * Copied here as a hack to avoid having to include Servo.h in all sketch files that
+ * include ConfigurableFirmata.h
+ */
+
+ServoFirmata *ServoInstance;
+
+void servoAnalogWrite(byte pin, int value)
+{
+  ServoInstance->analogWrite(pin, value);
+}
+
+ServoFirmata::ServoFirmata()
+{
+  ServoInstance = this;
+}
+
+boolean ServoFirmata::analogWrite(byte pin, int value)
+{
+  if (IS_PIN_SERVO(pin)) {
+    Servo *servo = servos[PIN_TO_SERVO(pin)];
+    if (servo) {
+      servo->write(value);
+      return true;
+    }
+  }
+  return false;
+}
+
+boolean ServoFirmata::handlePinMode(byte pin, int mode)
+{
+  if (IS_PIN_SERVO(pin)) {
+    if (mode == PIN_MODE_SERVO) {
+      attach(pin, -1, -1);
+      return true;
+    } else {
+      detach(pin);
+    }
+  }
+  return false;
+}
+
+void ServoFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_SERVO(pin)) {
+    Firmata.write(PIN_MODE_SERVO);
+    Firmata.write(14); //14 bit resolution (Servo takes int as argument)
+  }
+}
+
+boolean ServoFirmata::handleSysex(byte command, byte argc, byte* argv)
+{
+  if (command == SERVO_CONFIG) {
+    if (argc > 4) {
+      // these vars are here for clarity, they'll optimized away by the compiler
+      byte pin = argv[0];
+      if (IS_PIN_SERVO(pin) && Firmata.getPinMode(pin) != PIN_MODE_IGNORE) {
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        attach(pin, minPulse, maxPulse);
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+void ServoFirmata::attach(byte pin, int minPulse, int maxPulse)
+{
+  Servo *servo = servos[PIN_TO_SERVO(pin)];
+  if (!servo) {
+    servo = new Servo();
+    servos[PIN_TO_SERVO(pin)] = servo;
+  }
+  if (servo->attached())
+    servo->detach();
+  if (minPulse >= 0 || maxPulse >= 0)
+    servo->attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+  else
+    servo->attach(PIN_TO_DIGITAL(pin));
+}
+
+void ServoFirmata::detach(byte pin)
+{
+  Servo *servo = servos[PIN_TO_SERVO(pin)];
+  if (servo) {
+    if (servo->attached())
+      servo->detach();
+    free(servo);
+    servos[PIN_TO_SERVO(pin)] = NULL;
+  }
+}
+
+void ServoFirmata::reset()
+{
+  for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+    if (IS_PIN_SERVO(pin)) {
+      detach(pin);
+    }
+  }
+}
+
+#endif /* ServoFirmata_h */
--- a/libraries/FirmataWithDeviceFeature/src-features/StepperFirmata.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/StepperFirmata.cpp
@@ -0,0 +1,151 @@
+/*
+  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) 2013 Norbert Truchsess. 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 by Jeff Hoefs: January 23rd, 2016
+*/
+
+#include <ConfigurableFirmata.h>
+#include "StepperFirmata.h"
+#include "utility/FirmataStepper.h"
+
+boolean StepperFirmata::handlePinMode(byte pin, int mode)
+{
+  if (mode == PIN_MODE_STEPPER) {
+    if (IS_PIN_DIGITAL(pin)) {
+      digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+      pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+      return true;
+    }
+  }
+  return false;
+}
+
+void StepperFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_DIGITAL(pin)) {
+    Firmata.write(PIN_MODE_STEPPER);
+    Firmata.write(21); //21 bits used for number of steps
+  }
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+boolean StepperFirmata::handleSysex(byte command, byte argc, byte *argv)
+{
+  if (command == STEPPER_DATA) {
+    byte stepCommand, deviceNum, directionPin, stepPin, stepDirection;
+    byte interface, interfaceType;
+    byte motorPin3, motorPin4;
+    unsigned int stepsPerRev;
+    long numSteps;
+    int stepSpeed;
+    int accel;
+    int decel;
+
+    stepCommand = argv[0];
+    deviceNum = argv[1];
+
+    if (deviceNum < MAX_STEPPERS) {
+      if (stepCommand == STEPPER_CONFIG) {
+
+        interface = argv[2]; // upper 4 bits are the stepDelay, lower 4 bits are the interface type
+        interfaceType = interface & 0x0F; // the interface type is specified by the lower 4 bits
+        stepsPerRev = (argv[3] + (argv[4] << 7));
+
+        directionPin = argv[5]; // or motorPin1 for TWO_WIRE or FOUR_WIRE interface
+        stepPin = argv[6]; // // or motorPin2 for TWO_WIRE or FOUR_WIRE interface
+        if (Firmata.getPinMode(directionPin) == PIN_MODE_IGNORE || Firmata.getPinMode(stepPin) == PIN_MODE_IGNORE)
+          return false;
+        Firmata.setPinMode(directionPin, PIN_MODE_STEPPER);
+        Firmata.setPinMode(stepPin, PIN_MODE_STEPPER);
+
+        if (!stepper[deviceNum]) {
+          numSteppers++;
+        }
+        if (interfaceType == FirmataStepper::DRIVER || interfaceType == FirmataStepper::TWO_WIRE) {
+          stepper[deviceNum] = new FirmataStepper(interface, stepsPerRev, directionPin, stepPin);
+        } else if (interfaceType == FirmataStepper::FOUR_WIRE) {
+          motorPin3 = argv[7];
+          motorPin4 = argv[8];
+          if (Firmata.getPinMode(motorPin3) == PIN_MODE_IGNORE || Firmata.getPinMode(motorPin4) == PIN_MODE_IGNORE)
+            return false;
+          Firmata.setPinMode(motorPin3, PIN_MODE_STEPPER);
+          Firmata.setPinMode(motorPin4, PIN_MODE_STEPPER);
+          stepper[deviceNum] = new FirmataStepper(interface, stepsPerRev, directionPin, stepPin, motorPin3, motorPin4);
+        }
+      }
+      else if (stepCommand == STEPPER_STEP) {
+        stepDirection = argv[2];
+        numSteps = (long)argv[3] | ((long)argv[4] << 7) | ((long)argv[5] << 14);
+        stepSpeed = (argv[6] + (argv[7] << 7));
+
+        if (stepDirection == 0) {
+          numSteps *= -1;
+        }
+        if (stepper[deviceNum]) {
+          if (argc >= 8 && argc < 12) {
+            // num steps, speed (0.01*rad/sec)
+            stepper[deviceNum]->setStepsToMove(numSteps, stepSpeed);
+          } else if (argc == 12) {
+            accel = (argv[8] + (argv[9] << 7));
+            decel = (argv[10] + (argv[11] << 7));
+            // num steps, speed (0.01*rad/sec), accel (0.01*rad/sec^2), decel (0.01*rad/sec^2)
+            stepper[deviceNum]->setStepsToMove(numSteps, stepSpeed, accel, decel);
+          }
+        }
+      }
+      return true;
+    }
+  }
+  return false;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void StepperFirmata::reset()
+{
+  for (byte i = 0; i < MAX_STEPPERS; i++) {
+    if (stepper[i]) {
+      free(stepper[i]);
+      stepper[i] = 0;
+    }
+  }
+  numSteppers = 0;
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void StepperFirmata::update()
+{
+  if (numSteppers > 0) {
+    // if one or more stepper motors are used, update their position
+    for (byte i = 0; i < MAX_STEPPERS; i++) {
+      if (stepper[i]) {
+        bool done = stepper[i]->update();
+        // send command to client application when stepping is complete
+        if (done) {
+          Firmata.write(START_SYSEX);
+          Firmata.write(STEPPER_DATA);
+          Firmata.write(i);
+          Firmata.write(END_SYSEX);
+        }
+      }
+    }
+  }
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/StepperFirmata.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/StepperFirmata.h
@@ -0,0 +1,40 @@
+/*
+  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) 2013 Norbert Truchsess. 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 StepperFirmata_h
+#define StepperFirmata_h
+
+#include <ConfigurableFirmata.h>
+#include "utility/FirmataStepper.h"
+#include "FirmataFeature.h"
+
+#define MAX_STEPPERS 6 // arbitrary value... may need to adjust
+#define STEPPER_CONFIG 0
+#define STEPPER_STEP 1
+
+class StepperFirmata: public FirmataFeature
+{
+  public:
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte *argv);
+    void update();
+    void reset();
+  private:
+    FirmataStepper *stepper[MAX_STEPPERS];
+    byte numSteppers;
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/FirmataStepper.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/FirmataStepper.cpp
@@ -0,0 +1,448 @@
+/**
+  FirmataStepper is a simple non-blocking stepper motor library
+  for 2 and 4 wire bipolar and unipolar stepper motor drive circuits
+  as well as EasyDriver (http://schmalzhaus.com/EasyDriver/) and
+  other step + direction drive circuits.
+
+  FirmataStepper (0.2) by Jeff Hoefs
+
+  EasyDriver support based on modifications by Chris Coleman
+
+  Acceleration / Deceleration algorithms and code based on:
+  app note: http://www.atmel.com/dyn/resources/prod_documents/doc8017.pdf
+  source code: http://www.atmel.com/dyn/resources/prod_documents/AVR446.zip
+
+  stepMotor function based on Stepper.cpp Stepper library for
+  Wiring/Arduino created by Tom Igoe, Sebastian Gassner
+  David Mellis and Noah Shibley.
+
+  Relevant notes from Stepper.cpp:
+
+  When wiring multiple stepper motors to a microcontroller,
+  you quickly run out of output pins, with each motor requiring 4 connections.
+
+  By making use of the fact that at any time two of the four motor
+  coils are the inverse  of the other two, the number of
+  control connections can be reduced from 4 to 2.
+
+  A slightly modified circuit around a Darlington transistor array or an L293 H-bridge
+  connects to only 2 microcontroler pins, inverts the signals received,
+  and delivers the 4 (2 plus 2 inverted ones) output signals required
+  for driving a stepper motor.
+
+  The sequence of control signals for 4 control wires is as follows:
+
+  Step C0 C1 C2 C3
+     1  1  0  1  0
+     2  0  1  1  0
+     3  0  1  0  1
+     4  1  0  0  1
+
+  The sequence of controls signals for 2 control wires is as follows
+  (columns C1 and C2 from above):
+
+  Step C0 C1
+     1  0  1
+     2  1  1
+     3  1  0
+     4  0  0
+
+  The circuits can be found at
+  http://www.arduino.cc/en/Tutorial/Stepper
+*/
+
+#include "FirmataStepper.h"
+
+/**
+ * Constructor.
+ *
+ * Configure a stepper for an EasyDriver or other step + direction interface or
+ * configure a bipolar or unipolar stepper motor for 2 wire drive mode.
+ * Configure a bipolar or unipolar stepper for 4 wire drive mode.
+ * @param interface Lower 3 bits:
+ * The interface type: FirmataStepper::DRIVER,
+ * FirmataStepper::TWO_WIRE or FirmataStepper::FOUR_WIRE
+ * Upper 4 bits: Any bits set = use 2 microsecond delay
+ * @param steps_per_rev The number of steps to make 1 revolution.
+ * @param first_pin The direction pin (EasyDriver) or the pin attached to the
+ * 1st motor coil (2 wire drive mode)
+ * @param second_pin The step pin (EasyDriver) or the pin attached to the 2nd
+ * motor coil (2 wire drive mode)
+ * @param motor_pin_3 The pin attached to the 3rd motor coil
+ * @param motor_pin_4 The pin attached to the 4th motor coil
+ */
+FirmataStepper::FirmataStepper(byte interface,
+                               int steps_per_rev,
+                               byte pin1,
+                               byte pin2,
+                               byte pin3,
+                               byte pin4)
+{
+  this->step_number = 0;      // which step the motor is on
+  this->direction = 0;      // motor direction
+  this->last_step_time = 0;    // time stamp in ms of the last step taken
+  this->steps_per_rev = steps_per_rev;    // total number of steps for this motor
+  this->running = false;
+  this->interface = interface & 0x0F; // default to Easy Stepper (or other step + direction driver)
+
+  // could update this in future to support additional delays if necessary
+  if (((interface & 0xF0) >> 4) > 0)
+  {
+    // high current driver
+    this->stepDelay = 2; // microseconds
+  }
+  else
+  {
+    this->stepDelay = 1; // microseconds
+  }
+
+  this->motor_pin_1 = pin1;
+  this->motor_pin_2 = pin2;
+  this->dir_pin = pin1;
+  this->step_pin = pin2;
+
+  // setup the pins on the microcontroller:
+  pinMode(this->motor_pin_1, OUTPUT);
+  pinMode(this->motor_pin_2, OUTPUT);
+
+  if (this->interface == FirmataStepper::FOUR_WIRE)
+  {
+    this->motor_pin_3 = pin3;
+    this->motor_pin_4 = pin4;
+    pinMode(this->motor_pin_3, OUTPUT);
+    pinMode(this->motor_pin_4, OUTPUT);
+  }
+
+
+  this->alpha = PI_2 / this->steps_per_rev;
+  this->at_x100 = (long)(this->alpha * T1_FREQ * 100);
+  this->ax20000 = (long)(this->alpha * 20000);
+  this->alpha_x2 = this->alpha * 2;
+
+}
+
+/**
+ * Move the stepper a given number of steps at the specified
+ * speed (rad/sec), acceleration (rad/sec^2) and deceleration (rad/sec^2).
+ *
+ * @param steps_to_move The number ofsteps to move the motor
+ * @param speed Max speed in 0.01*rad/sec
+ * @param accel [optional] Acceleration in 0.01*rad/sec^2
+ * @param decel [optional] Deceleration in 0.01*rad/sec^2
+ */
+void FirmataStepper::setStepsToMove(long steps_to_move, int speed, int accel, int decel)
+{
+  unsigned long maxStepLimit;
+  unsigned long accelerationLimit;
+
+  this->step_number = 0;
+  this->lastAccelDelay = 0;
+  this->stepCount = 0;
+  this->rest = 0;
+
+  if (steps_to_move < 0)
+  {
+    this->direction = FirmataStepper::CCW;
+    steps_to_move = -steps_to_move;
+  }
+  else
+  {
+    this->direction = FirmataStepper::CW;
+  }
+
+  this->steps_to_move = steps_to_move;
+
+  // set max speed limit, by calc min_delay
+  // min_delay = (alpha / tt)/w
+  this->min_delay = this->at_x100 / speed;
+
+  // if acceleration or deceleration are not defined
+  // start in RUN state and do no decelerate
+  if (accel == 0 || decel == 0)
+  {
+    this->step_delay = this->min_delay;
+
+    this->decel_start = steps_to_move;
+    this->run_state = FirmataStepper::RUN;
+    this->accel_count = 0;
+    this->running = true;
+
+    return;
+  }
+
+  // if only moving 1 step
+  if (steps_to_move == 1)
+  {
+
+    // move one step
+    this->accel_count = -1;
+    this->run_state = FirmataStepper::DECEL;
+
+    this->step_delay = this->min_delay;
+    this->running = true;
+  }
+  else if (steps_to_move != 0)
+  {
+    // set initial step delay
+    // step_delay = 1/tt * sqrt(2*alpha/accel)
+    // step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000
+    this->step_delay = (long)((T1_FREQ_148 * sqrt(alpha_x2 / accel)) * 1000);
+
+    // find out after how many steps does the speed hit the max speed limit.
+    // maxSpeedLimit = speed^2 / (2*alpha*accel)
+    maxStepLimit = (long)speed * speed / (long)(((long)this->ax20000 * accel) / 100);
+
+    // if we hit max spped limit before 0.5 step it will round to 0.
+    // but in practice we need to move at least 1 step to get any speed at all.
+    if (maxStepLimit == 0)
+    {
+      maxStepLimit = 1;
+    }
+
+    // find out after how many steps we must start deceleration.
+    // n1 = (n1+n2)decel / (accel + decel)
+    accelerationLimit = (long)((steps_to_move * decel) / (accel + decel));
+
+    // we must accelerate at least 1 step before we can start deceleration
+    if (accelerationLimit == 0)
+    {
+      accelerationLimit = 1;
+    }
+
+    // use the limit we hit first to calc decel
+    if (accelerationLimit <= maxStepLimit)
+    {
+      this->decel_val = accelerationLimit - steps_to_move;
+    }
+    else
+    {
+      this->decel_val = -(long)(maxStepLimit * accel) / decel;
+    }
+
+    // we must decelerate at least 1 step to stop
+    if (this->decel_val == 0)
+    {
+      this->decel_val = -1;
+    }
+
+    // find step to start deceleration
+    this->decel_start = steps_to_move + this->decel_val;
+
+    // if the max speed is so low that we don't need to go via acceleration state.
+    if (this->step_delay <= this->min_delay)
+    {
+      this->step_delay = this->min_delay;
+      this->run_state = FirmataStepper::RUN;
+    }
+    else
+    {
+      this->run_state = FirmataStepper::ACCEL;
+    }
+
+    // reset counter
+    this->accel_count = 0;
+    this->running = true;
+  }
+}
+
+
+bool FirmataStepper::update()
+{
+  bool done = false;
+  unsigned long newStepDelay = this->min_delay;
+
+  unsigned long curTimeVal = micros();
+  unsigned long timeDiff = curTimeVal - this->last_step_time;
+
+  if (this->running == true && timeDiff >= this->step_delay)
+  {
+
+    this->last_step_time = curTimeVal;
+
+    switch (this->run_state)
+    {
+      case FirmataStepper::STOP:
+        this->stepCount = 0;
+        this->rest = 0;
+        if (this->running)
+        {
+          done = true;
+        }
+        this->running = false;
+        break;
+
+      case FirmataStepper::ACCEL:
+        updateStepPosition();
+        this->stepCount++;
+        this->accel_count++;
+        newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1));
+        this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1);
+
+        // check if we should start deceleration
+        if (this->stepCount >= this->decel_start)
+        {
+          this->accel_count = this->decel_val;
+          this->run_state = FirmataStepper::DECEL;
+          this->rest = 0;
+        }
+        // check if we hit max speed
+        else if (newStepDelay <= this->min_delay)
+        {
+          this->lastAccelDelay = newStepDelay;
+          newStepDelay = this->min_delay;
+          this->rest = 0;
+          this->run_state = FirmataStepper::RUN;
+        }
+        break;
+
+      case FirmataStepper::RUN:
+        updateStepPosition();
+        this->stepCount++;
+
+        // if no accel or decel was specified, go directly to STOP state
+        if (stepCount >= this->steps_to_move)
+        {
+          this->run_state = FirmataStepper::STOP;
+        }
+        // check if we should start deceleration
+        else if (this->stepCount >= this->decel_start)
+        {
+          this->accel_count = this->decel_val;
+          // start deceleration with same delay that accel ended with
+          newStepDelay = this->lastAccelDelay;
+          this->run_state = FirmataStepper::DECEL;
+        }
+        break;
+
+      case FirmataStepper::DECEL:
+        updateStepPosition();
+        this->stepCount++;
+        this->accel_count++;
+
+        newStepDelay = this->step_delay - (((2 * (long)this->step_delay) + this->rest) / (4 * this->accel_count + 1));
+        this->rest = ((2 * (long)this->step_delay) + this->rest) % (4 * this->accel_count + 1);
+
+        if (newStepDelay < 0) newStepDelay = -newStepDelay;
+        // check if we ar at the last step
+        if (this->accel_count >= 0)
+        {
+          this->run_state = FirmataStepper::STOP;
+        }
+
+        break;
+    }
+
+    this->step_delay = newStepDelay;
+
+  }
+
+  return done;
+
+}
+
+/**
+ * Update the step position.
+ * @private
+ */
+void FirmataStepper::updateStepPosition()
+{
+  // increment or decrement the step number,
+  // depending on direction:
+  if (this->direction == FirmataStepper::CW)
+  {
+    this->step_number++;
+    if (this->step_number >= this->steps_per_rev)
+    {
+      this->step_number = 0;
+    }
+  }
+  else
+  {
+    if (this->step_number <= 0)
+    {
+      this->step_number = this->steps_per_rev;
+    }
+    this->step_number--;
+  }
+
+  // step the motor to step number 0, 1, 2, or 3:
+  stepMotor(this->step_number % 4, this->direction);
+}
+
+/**
+ * Moves the motor forward or backwards.
+ * @param step_num For 2 or 4 wire configurations, this is the current step in
+ * the 2 or 4 step sequence.
+ * @param direction The direction of rotation
+ */
+void FirmataStepper::stepMotor(byte step_num, byte direction)
+{
+  if (this->interface == FirmataStepper::DRIVER)
+  {
+    digitalWrite(dir_pin, direction);
+    delayMicroseconds(this->stepDelay);
+    digitalWrite(step_pin, LOW);
+    delayMicroseconds(this->stepDelay);
+    digitalWrite(step_pin, HIGH);
+  }
+  else if (this->interface == FirmataStepper::TWO_WIRE)
+  {
+    switch (step_num)
+    {
+      case 0: /* 01 */
+        digitalWrite(motor_pin_1, LOW);
+        digitalWrite(motor_pin_2, HIGH);
+        break;
+      case 1: /* 11 */
+        digitalWrite(motor_pin_1, HIGH);
+        digitalWrite(motor_pin_2, HIGH);
+        break;
+      case 2: /* 10 */
+        digitalWrite(motor_pin_1, HIGH);
+        digitalWrite(motor_pin_2, LOW);
+        break;
+      case 3: /* 00 */
+        digitalWrite(motor_pin_1, LOW);
+        digitalWrite(motor_pin_2, LOW);
+        break;
+    }
+  }
+  else if (this->interface == FirmataStepper::FOUR_WIRE)
+  {
+    switch (step_num)
+    {
+      case 0:    // 1010
+        digitalWrite(motor_pin_1, HIGH);
+        digitalWrite(motor_pin_2, LOW);
+        digitalWrite(motor_pin_3, HIGH);
+        digitalWrite(motor_pin_4, LOW);
+        break;
+      case 1:    // 0110
+        digitalWrite(motor_pin_1, LOW);
+        digitalWrite(motor_pin_2, HIGH);
+        digitalWrite(motor_pin_3, HIGH);
+        digitalWrite(motor_pin_4, LOW);
+        break;
+      case 2:    //0101
+        digitalWrite(motor_pin_1, LOW);
+        digitalWrite(motor_pin_2, HIGH);
+        digitalWrite(motor_pin_3, LOW);
+        digitalWrite(motor_pin_4, HIGH);
+        break;
+      case 3:    //1001
+        digitalWrite(motor_pin_1, HIGH);
+        digitalWrite(motor_pin_2, LOW);
+        digitalWrite(motor_pin_3, LOW);
+        digitalWrite(motor_pin_4, HIGH);
+        break;
+    }
+  }
+}
+
+/**
+ * @return The version number of this library.
+ */
+byte FirmataStepper::version(void)
+{
+  return 2;
+}
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/FirmataStepper.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/FirmataStepper.h
@@ -0,0 +1,145 @@
+/*
+  FirmataStepper is a simple non-blocking stepper motor library
+  for 2 and 4 wire bipolar and unipolar stepper motor drive circuits
+  as well as EasyDriver (http://schmalzhaus.com/EasyDriver/) and
+  other step + direction drive circuits.
+
+  FirmataStepper (0.2) by Jeff Hoefs
+
+  EasyDriver support based on modifications by Chris Coleman
+
+  Acceleration / Deceleration algorithms and code based on:
+  app note: http://www.atmel.com/dyn/resources/prod_documents/doc8017.pdf
+  source code: http://www.atmel.com/dyn/resources/prod_documents/AVR446.zip
+
+  stepMotor function based on Stepper.cpp Stepper library for
+  Wiring/Arduino created by Tom Igoe, Sebastian Gassner
+  David Mellis and Noah Shibley.
+
+  Relevant notes from Stepper.cpp:
+
+  When wiring multiple stepper motors to a microcontroller,
+  you quickly run out of output pins, with each motor requiring 4 connections.
+
+  By making use of the fact that at any time two of the four motor
+  coils are the inverse  of the other two, the number of
+  control connections can be reduced from 4 to 2.
+
+  A slightly modified circuit around a Darlington transistor array or an L293 H-bridge
+  connects to only 2 microcontroler pins, inverts the signals received,
+  and delivers the 4 (2 plus 2 inverted ones) output signals required
+  for driving a stepper motor.
+
+  The sequence of control signals for 4 control wires is as follows:
+
+  Step C0 C1 C2 C3
+     1  1  0  1  0
+     2  0  1  1  0
+     3  0  1  0  1
+     4  1  0  0  1
+
+  The sequence of controls signals for 2 control wires is as follows
+  (columns C1 and C2 from above):
+
+  Step C0 C1
+     1  0  1
+     2  1  1
+     3  1  0
+     4  0  0
+
+  The circuits can be found at
+  http://www.arduino.cc/en/Tutorial/Stepper
+*/
+
+// ensure this library description is only included once
+#ifndef FirmataStepper_h
+#define FirmataStepper_h
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#define PI_2 2*3.14159
+#define T1_FREQ 1000000L // provides the most accurate step delay values
+#define T1_FREQ_148 ((long)((T1_FREQ*0.676)/100)) // divided by 100 and scaled by 0.676
+
+// library interface description
+class FirmataStepper
+{
+  public:
+    FirmataStepper(byte interface = FirmataStepper::DRIVER,
+                   int steps_per_rev = 200,
+                   byte pin1 = 2,
+                   byte pin2 = 3,
+                   byte pin3 = 4,
+                   byte pin4 = 5);
+
+    enum Interface
+    {
+      DRIVER = 1,
+      TWO_WIRE = 2,
+      FOUR_WIRE = 4
+    };
+
+    enum RunState
+    {
+      STOP = 0,
+      ACCEL = 1,
+      DECEL = 2,
+      RUN = 3
+    };
+
+    enum Direction
+    {
+      CCW = 0,
+      CW = 1
+    };
+
+    void setStepsToMove(long steps_to_move, int speed, int accel = 0, int decel = 0);
+
+    // update the stepper position
+    bool update();
+
+    byte version(void);
+
+  private:
+    void stepMotor(byte step_num, byte direction);
+    void updateStepPosition();
+    bool running;
+    byte interface;     // Type of interface: DRIVER, TWO_WIRE or FOUR_WIRE
+    byte direction;        // Direction of rotation
+    unsigned long step_delay;    // delay between steps, in microseconds
+    int steps_per_rev;      // number of steps to make one revolution
+    long step_number;        // which step the motor is on
+    long steps_to_move;   // total number of teps to move
+    byte stepDelay;       // delay between steps (default = 1, increase for high current drivers)
+
+    byte run_state;
+    int accel_count;
+    unsigned long min_delay;
+    long decel_start;
+    int decel_val;
+
+    long lastAccelDelay;
+    long stepCount;
+    unsigned int rest;
+
+    float alpha;  // PI * 2 / steps_per_rev
+    long at_x100;  // alpha * T1_FREQ * 100
+    long ax20000;  // alph a* 20000
+    float alpha_x2;  // alpha * 2
+
+    // motor pin numbers:
+    byte dir_pin;
+    byte step_pin;
+    byte motor_pin_1;
+    byte motor_pin_2;
+    byte motor_pin_3;
+    byte motor_pin_4;
+
+    unsigned long last_step_time; // time stamp in microseconds of when the last step was taken
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/OneWire.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/OneWire.cpp
@@ -0,0 +1,567 @@
+/*
+Copyright (c) 2007, Jim Studt  (original old version - many contributors since)
+
+The latest version of this library may be found at:
+  http://www.pjrc.com/teensy/td_libs_OneWire.html
+
+OneWire has been maintained by Paul Stoffregen (paul@pjrc.com) since
+January 2010.  At the time, it was in need of many bug fixes, but had
+been abandoned the original author (Jim Studt).  None of the known
+contributors were interested in maintaining OneWire.  Paul typically
+works on OneWire every 6 to 12 months.  Patches usually wait that
+long.  If anyone is interested in more actively maintaining OneWire,
+please contact Paul.
+
+Version 2.3:
+  Unknonw chip fallback mode, Roger Clark
+  Teensy-LC compatibility, Paul Stoffregen
+  Search bug fix, Love Nystrom
+
+Version 2.2:
+  Teensy 3.0 compatibility, Paul Stoffregen, paul@pjrc.com
+  Arduino Due compatibility, http://arduino.cc/forum/index.php?topic=141030
+  Fix DS18B20 example negative temperature
+  Fix DS18B20 example's low res modes, Ken Butcher
+  Improve reset timing, Mark Tillotson
+  Add const qualifiers, Bertrik Sikken
+  Add initial value input to crc16, Bertrik Sikken
+  Add target_search() function, Scott Roberts
+
+Version 2.1:
+  Arduino 1.0 compatibility, Paul Stoffregen
+  Improve temperature example, Paul Stoffregen
+  DS250x_PROM example, Guillermo Lovato
+  PIC32 (chipKit) compatibility, Jason Dangel, dangel.jason AT gmail.com
+  Improvements from Glenn Trewitt:
+  - crc16() now works
+  - check_crc16() does all of calculation/checking work.
+  - Added read_bytes() and write_bytes(), to reduce tedious loops.
+  - Added ds2408 example.
+  Delete very old, out-of-date readme file (info is here)
+
+Version 2.0: Modifications by Paul Stoffregen, January 2010:
+http://www.pjrc.com/teensy/td_libs_OneWire.html
+  Search fix from Robin James
+    http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1238032295/27#27
+  Use direct optimized I/O in all cases
+  Disable interrupts during timing critical sections
+    (this solves many random communication errors)
+  Disable interrupts during read-modify-write I/O
+  Reduce RAM consumption by eliminating unnecessary
+    variables and trimming many to 8 bits
+  Optimize both crc8 - table version moved to flash
+
+Modified to work with larger numbers of devices - avoids loop.
+Tested in Arduino 11 alpha with 12 sensors.
+26 Sept 2008 -- Robin James
+http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1238032295/27#27
+
+Updated to work with arduino-0008 and to include skip() as of
+2007/07/06. --RJL20
+
+Modified to calculate the 8-bit CRC directly, avoiding the need for
+the 256-byte lookup table to be loaded in RAM.  Tested in arduino-0010
+-- Tom Pollard, Jan 23, 2008
+
+Jim Studt's original library was modified by Josh Larios.
+
+Tom Pollard, pollard@alum.mit.edu, contributed around May 20, 2008
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including
+without limitation the rights to use, copy, modify, merge, publish,
+distribute, sublicense, and/or sell copies of the Software, and to
+permit persons to whom the Software is furnished to do so, subject to
+the following conditions:
+
+The above copyright notice and this permission notice shall be
+included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+Much of the code was inspired by Derek Yerger's code, though I don't
+think much of that remains.  In any event that was..
+    (copyleft) 2006 by Derek Yerger - Free to distribute freely.
+
+The CRC code was excerpted and inspired by the Dallas Semiconductor
+sample code bearing this copyright.
+//---------------------------------------------------------------------------
+// Copyright (C) 2000 Dallas Semiconductor Corporation, All Rights Reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a
+// copy of this software and associated documentation files (the "Software"),
+// to deal in the Software without restriction, including without limitation
+// the rights to use, copy, modify, merge, publish, distribute, sublicense,
+// and/or sell copies of the Software, and to permit persons to whom the
+// Software is furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included
+// in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY,  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+// IN NO EVENT SHALL DALLAS SEMICONDUCTOR BE LIABLE FOR ANY CLAIM, DAMAGES
+// OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+// OTHER DEALINGS IN THE SOFTWARE.
+//
+// Except as contained in this notice, the name of Dallas Semiconductor
+// shall not be used except as stated in the Dallas Semiconductor
+// Branding Policy.
+//--------------------------------------------------------------------------
+*/
+
+#include "OneWire.h"
+
+
+OneWire::OneWire(uint8_t pin)
+{
+	pinMode(pin, INPUT);
+	bitmask = PIN_TO_BITMASK(pin);
+	baseReg = PIN_TO_BASEREG(pin);
+#if ONEWIRE_SEARCH
+	reset_search();
+#endif
+}
+
+
+// Perform the onewire reset function.  We will wait up to 250uS for
+// the bus to come high, if it doesn't then it is broken or shorted
+// and we return a 0;
+//
+// Returns 1 if a device asserted a presence pulse, 0 otherwise.
+//
+uint8_t OneWire::reset(void)
+{
+	IO_REG_TYPE mask = bitmask;
+	volatile IO_REG_TYPE *reg IO_REG_ASM = baseReg;
+	uint8_t r;
+	uint8_t retries = 125;
+
+	noInterrupts();
+	DIRECT_MODE_INPUT(reg, mask);
+	interrupts();
+	// wait until the wire is high... just in case
+	do {
+		if (--retries == 0) return 0;
+		delayMicroseconds(2);
+	} while ( !DIRECT_READ(reg, mask));
+
+	noInterrupts();
+	DIRECT_WRITE_LOW(reg, mask);
+	DIRECT_MODE_OUTPUT(reg, mask);	// drive output low
+	interrupts();
+	delayMicroseconds(480);
+	noInterrupts();
+	DIRECT_MODE_INPUT(reg, mask);	// allow it to float
+	delayMicroseconds(70);
+	r = !DIRECT_READ(reg, mask);
+	interrupts();
+	delayMicroseconds(410);
+	return r;
+}
+
+//
+// Write a bit. Port and bit is used to cut lookup time and provide
+// more certain timing.
+//
+void OneWire::write_bit(uint8_t v)
+{
+	IO_REG_TYPE mask=bitmask;
+	volatile IO_REG_TYPE *reg IO_REG_ASM = baseReg;
+
+	if (v & 1) {
+		noInterrupts();
+		DIRECT_WRITE_LOW(reg, mask);
+		DIRECT_MODE_OUTPUT(reg, mask);	// drive output low
+		delayMicroseconds(10);
+		DIRECT_WRITE_HIGH(reg, mask);	// drive output high
+		interrupts();
+		delayMicroseconds(55);
+	} else {
+		noInterrupts();
+		DIRECT_WRITE_LOW(reg, mask);
+		DIRECT_MODE_OUTPUT(reg, mask);	// drive output low
+		delayMicroseconds(65);
+		DIRECT_WRITE_HIGH(reg, mask);	// drive output high
+		interrupts();
+		delayMicroseconds(5);
+	}
+}
+
+//
+// Read a bit. Port and bit is used to cut lookup time and provide
+// more certain timing.
+//
+uint8_t OneWire::read_bit(void)
+{
+	IO_REG_TYPE mask=bitmask;
+	volatile IO_REG_TYPE *reg IO_REG_ASM = baseReg;
+	uint8_t r;
+
+	noInterrupts();
+	DIRECT_MODE_OUTPUT(reg, mask);
+	DIRECT_WRITE_LOW(reg, mask);
+	delayMicroseconds(3);
+	DIRECT_MODE_INPUT(reg, mask);	// let pin float, pull up will raise
+	delayMicroseconds(10);
+	r = DIRECT_READ(reg, mask);
+	interrupts();
+	delayMicroseconds(53);
+	return r;
+}
+
+//
+// Write a byte. The writing code uses the active drivers to raise the
+// pin high, if you need power after the write (e.g. DS18S20 in
+// parasite power mode) then set 'power' to 1, otherwise the pin will
+// go tri-state at the end of the write to avoid heating in a short or
+// other mishap.
+//
+void OneWire::write(uint8_t v, uint8_t power /* = 0 */) {
+    uint8_t bitMask;
+
+    for (bitMask = 0x01; bitMask; bitMask <<= 1) {
+	OneWire::write_bit( (bitMask & v)?1:0);
+    }
+    if ( !power) {
+	noInterrupts();
+	DIRECT_MODE_INPUT(baseReg, bitmask);
+	DIRECT_WRITE_LOW(baseReg, bitmask);
+	interrupts();
+    }
+}
+
+void OneWire::write_bytes(const uint8_t *buf, uint16_t count, bool power /* = 0 */) {
+  for (uint16_t i = 0 ; i < count ; i++)
+    write(buf[i]);
+  if (!power) {
+    noInterrupts();
+    DIRECT_MODE_INPUT(baseReg, bitmask);
+    DIRECT_WRITE_LOW(baseReg, bitmask);
+    interrupts();
+  }
+}
+
+//
+// Read a byte
+//
+uint8_t OneWire::read() {
+    uint8_t bitMask;
+    uint8_t r = 0;
+
+    for (bitMask = 0x01; bitMask; bitMask <<= 1) {
+	if ( OneWire::read_bit()) r |= bitMask;
+    }
+    return r;
+}
+
+void OneWire::read_bytes(uint8_t *buf, uint16_t count) {
+  for (uint16_t i = 0 ; i < count ; i++)
+    buf[i] = read();
+}
+
+//
+// Do a ROM select
+//
+void OneWire::select(const uint8_t rom[8])
+{
+    uint8_t i;
+
+    write(0x55);           // Choose ROM
+
+    for (i = 0; i < 8; i++) write(rom[i]);
+}
+
+//
+// Do a ROM skip
+//
+void OneWire::skip()
+{
+    write(0xCC);           // Skip ROM
+}
+
+void OneWire::depower()
+{
+	noInterrupts();
+	DIRECT_MODE_INPUT(baseReg, bitmask);
+	interrupts();
+}
+
+#if ONEWIRE_SEARCH
+
+//
+// You need to use this function to start a search again from the beginning.
+// You do not need to do it for the first search, though you could.
+//
+void OneWire::reset_search()
+{
+  // reset the search state
+  LastDiscrepancy = 0;
+  LastDeviceFlag = FALSE;
+  LastFamilyDiscrepancy = 0;
+  for(int i = 7; ; i--) {
+    ROM_NO[i] = 0;
+    if ( i == 0) break;
+  }
+}
+
+// Setup the search to find the device type 'family_code' on the next call
+// to search(*newAddr) if it is present.
+//
+void OneWire::target_search(uint8_t family_code)
+{
+   // set the search state to find SearchFamily type devices
+   ROM_NO[0] = family_code;
+   for (uint8_t i = 1; i < 8; i++)
+      ROM_NO[i] = 0;
+   LastDiscrepancy = 64;
+   LastFamilyDiscrepancy = 0;
+   LastDeviceFlag = FALSE;
+}
+
+//
+// Perform a search. If this function returns a '1' then it has
+// enumerated the next device and you may retrieve the ROM from the
+// OneWire::address variable. If there are no devices, no further
+// devices, or something horrible happens in the middle of the
+// enumeration then a 0 is returned.  If a new device is found then
+// its address is copied to newAddr.  Use OneWire::reset_search() to
+// start over.
+//
+// --- Replaced by the one from the Dallas Semiconductor web site ---
+//--------------------------------------------------------------------------
+// Perform the 1-Wire Search Algorithm on the 1-Wire bus using the existing
+// search state.
+// Return TRUE  : device found, ROM number in ROM_NO buffer
+//        FALSE : device not found, end of search
+//
+uint8_t OneWire::search(uint8_t *newAddr, bool search_mode /* = true */)
+{
+   uint8_t id_bit_number;
+   uint8_t last_zero, rom_byte_number, search_result;
+   uint8_t id_bit, cmp_id_bit;
+
+   unsigned char rom_byte_mask, search_direction;
+
+   // initialize for search
+   id_bit_number = 1;
+   last_zero = 0;
+   rom_byte_number = 0;
+   rom_byte_mask = 1;
+   search_result = 0;
+
+   // if the last call was not the last one
+   if (!LastDeviceFlag)
+   {
+      // 1-Wire reset
+      if (!reset())
+      {
+         // reset the search
+         LastDiscrepancy = 0;
+         LastDeviceFlag = FALSE;
+         LastFamilyDiscrepancy = 0;
+         return FALSE;
+      }
+
+      // issue the search command
+      if (search_mode == true) {
+        write(0xF0);   // NORMAL SEARCH
+      } else {
+        write(0xEC);   // CONDITIONAL SEARCH
+      }
+
+      // loop to do the search
+      do
+      {
+         // read a bit and its complement
+         id_bit = read_bit();
+         cmp_id_bit = read_bit();
+
+         // check for no devices on 1-wire
+         if ((id_bit == 1) && (cmp_id_bit == 1))
+            break;
+         else
+         {
+            // all devices coupled have 0 or 1
+            if (id_bit != cmp_id_bit)
+               search_direction = id_bit;  // bit write value for search
+            else
+            {
+               // if this discrepancy if before the Last Discrepancy
+               // on a previous next then pick the same as last time
+               if (id_bit_number < LastDiscrepancy)
+                  search_direction = ((ROM_NO[rom_byte_number] & rom_byte_mask) > 0);
+               else
+                  // if equal to last pick 1, if not then pick 0
+                  search_direction = (id_bit_number == LastDiscrepancy);
+
+               // if 0 was picked then record its position in LastZero
+               if (search_direction == 0)
+               {
+                  last_zero = id_bit_number;
+
+                  // check for Last discrepancy in family
+                  if (last_zero < 9)
+                     LastFamilyDiscrepancy = last_zero;
+               }
+            }
+
+            // set or clear the bit in the ROM byte rom_byte_number
+            // with mask rom_byte_mask
+            if (search_direction == 1)
+              ROM_NO[rom_byte_number] |= rom_byte_mask;
+            else
+              ROM_NO[rom_byte_number] &= ~rom_byte_mask;
+
+            // serial number search direction write bit
+            write_bit(search_direction);
+
+            // increment the byte counter id_bit_number
+            // and shift the mask rom_byte_mask
+            id_bit_number++;
+            rom_byte_mask <<= 1;
+
+            // if the mask is 0 then go to new SerialNum byte rom_byte_number and reset mask
+            if (rom_byte_mask == 0)
+            {
+                rom_byte_number++;
+                rom_byte_mask = 1;
+            }
+         }
+      }
+      while(rom_byte_number < 8);  // loop until through all ROM bytes 0-7
+
+      // if the search was successful then
+      if (!(id_bit_number < 65))
+      {
+         // search successful so set LastDiscrepancy,LastDeviceFlag,search_result
+         LastDiscrepancy = last_zero;
+
+         // check for last device
+         if (LastDiscrepancy == 0)
+            LastDeviceFlag = TRUE;
+
+         search_result = TRUE;
+      }
+   }
+
+   // if no device found then reset counters so next 'search' will be like a first
+   if (!search_result || !ROM_NO[0])
+   {
+      LastDiscrepancy = 0;
+      LastDeviceFlag = FALSE;
+      LastFamilyDiscrepancy = 0;
+      search_result = FALSE;
+   } else {
+      for (int i = 0; i < 8; i++) newAddr[i] = ROM_NO[i];
+   }
+   return search_result;
+  }
+
+#endif
+
+#if ONEWIRE_CRC
+// The 1-Wire CRC scheme is described in Maxim Application Note 27:
+// "Understanding and Using Cyclic Redundancy Checks with Maxim iButton Products"
+//
+
+#if ONEWIRE_CRC8_TABLE
+// This table comes from Dallas sample code where it is freely reusable,
+// though Copyright (C) 2000 Dallas Semiconductor Corporation
+static const uint8_t PROGMEM dscrc_table[] = {
+      0, 94,188,226, 97, 63,221,131,194,156,126, 32,163,253, 31, 65,
+    157,195, 33,127,252,162, 64, 30, 95,  1,227,189, 62, 96,130,220,
+     35,125,159,193, 66, 28,254,160,225,191, 93,  3,128,222, 60, 98,
+    190,224,  2, 92,223,129, 99, 61,124, 34,192,158, 29, 67,161,255,
+     70, 24,250,164, 39,121,155,197,132,218, 56,102,229,187, 89,  7,
+    219,133,103, 57,186,228,  6, 88, 25, 71,165,251,120, 38,196,154,
+    101, 59,217,135,  4, 90,184,230,167,249, 27, 69,198,152,122, 36,
+    248,166, 68, 26,153,199, 37,123, 58,100,134,216, 91,  5,231,185,
+    140,210, 48,110,237,179, 81, 15, 78, 16,242,172, 47,113,147,205,
+     17, 79,173,243,112, 46,204,146,211,141,111, 49,178,236, 14, 80,
+    175,241, 19, 77,206,144,114, 44,109, 51,209,143, 12, 82,176,238,
+     50,108,142,208, 83, 13,239,177,240,174, 76, 18,145,207, 45,115,
+    202,148,118, 40,171,245, 23, 73,  8, 86,180,234,105, 55,213,139,
+     87,  9,235,181, 54,104,138,212,149,203, 41,119,244,170, 72, 22,
+    233,183, 85, 11,136,214, 52,106, 43,117,151,201, 74, 20,246,168,
+    116, 42,200,150, 21, 75,169,247,182,232, 10, 84,215,137,107, 53};
+
+//
+// Compute a Dallas Semiconductor 8 bit CRC. These show up in the ROM
+// and the registers.  (note: this might better be done without to
+// table, it would probably be smaller and certainly fast enough
+// compared to all those delayMicrosecond() calls.  But I got
+// confused, so I use this table from the examples.)
+//
+uint8_t OneWire::crc8(const uint8_t *addr, uint8_t len)
+{
+	uint8_t crc = 0;
+
+	while (len--) {
+		crc = pgm_read_byte(dscrc_table + (crc ^ *addr++));
+	}
+	return crc;
+}
+#else
+//
+// Compute a Dallas Semiconductor 8 bit CRC directly.
+// this is much slower, but much smaller, than the lookup table.
+//
+uint8_t OneWire::crc8(const uint8_t *addr, uint8_t len)
+{
+	uint8_t crc = 0;
+	
+	while (len--) {
+		uint8_t inbyte = *addr++;
+		for (uint8_t i = 8; i; i--) {
+			uint8_t mix = (crc ^ inbyte) & 0x01;
+			crc >>= 1;
+			if (mix) crc ^= 0x8C;
+			inbyte >>= 1;
+		}
+	}
+	return crc;
+}
+#endif
+
+#if ONEWIRE_CRC16
+bool OneWire::check_crc16(const uint8_t* input, uint16_t len, const uint8_t* inverted_crc, uint16_t crc)
+{
+    crc = ~crc16(input, len, crc);
+    return (crc & 0xFF) == inverted_crc[0] && (crc >> 8) == inverted_crc[1];
+}
+
+uint16_t OneWire::crc16(const uint8_t* input, uint16_t len, uint16_t crc)
+{
+    static const uint8_t oddparity[16] =
+        { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 };
+
+    for (uint16_t i = 0 ; i < len ; i++) {
+      // Even though we're just copying a byte from the input,
+      // we'll be doing 16-bit computation with it.
+      uint16_t cdata = input[i];
+      cdata = (cdata ^ crc) & 0xff;
+      crc >>= 8;
+
+      if (oddparity[cdata & 0x0F] ^ oddparity[cdata >> 4])
+          crc ^= 0xC001;
+
+      cdata <<= 6;
+      crc ^= cdata;
+      cdata <<= 1;
+      crc ^= cdata;
+    }
+    return crc;
+}
+#endif
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/OneWire.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/OneWire.h
@@ -0,0 +1,367 @@
+#ifndef OneWire_h
+#define OneWire_h
+
+#include <inttypes.h>
+
+#if ARDUINO >= 100
+#include "Arduino.h"       // for delayMicroseconds, digitalPinToBitMask, etc
+#else
+#include "WProgram.h"      // for delayMicroseconds
+#include "pins_arduino.h"  // for digitalPinToBitMask, etc
+#endif
+
+// You can exclude certain features from OneWire.  In theory, this
+// might save some space.  In practice, the compiler automatically
+// removes unused code (technically, the linker, using -fdata-sections
+// and -ffunction-sections when compiling, and Wl,--gc-sections
+// when linking), so most of these will not result in any code size
+// reduction.  Well, unless you try to use the missing features
+// and redesign your program to not need them!  ONEWIRE_CRC8_TABLE
+// is the exception, because it selects a fast but large algorithm
+// or a small but slow algorithm.
+
+// you can exclude onewire_search by defining that to 0
+#ifndef ONEWIRE_SEARCH
+#define ONEWIRE_SEARCH 1
+#endif
+
+// You can exclude CRC checks altogether by defining this to 0
+#ifndef ONEWIRE_CRC
+#define ONEWIRE_CRC 1
+#endif
+
+// Select the table-lookup method of computing the 8-bit CRC
+// by setting this to 1.  The lookup table enlarges code size by
+// about 250 bytes.  It does NOT consume RAM (but did in very
+// old versions of OneWire).  If you disable this, a slower
+// but very compact algorithm is used.
+#ifndef ONEWIRE_CRC8_TABLE
+#define ONEWIRE_CRC8_TABLE 1
+#endif
+
+// You can allow 16-bit CRC checks by defining this to 1
+// (Note that ONEWIRE_CRC must also be 1.)
+#ifndef ONEWIRE_CRC16
+#define ONEWIRE_CRC16 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+#ifndef TRUE
+#define TRUE  1
+#endif
+
+// Platform specific I/O definitions
+
+#if defined(__AVR__)
+#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint8_t
+#define IO_REG_ASM asm("r30")
+#define DIRECT_READ(base, mask)         (((*(base)) & (mask)) ? 1 : 0)
+#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+1)) &= ~(mask))
+#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+1)) |= (mask))
+#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+2)) &= ~(mask))
+#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+2)) |= (mask))
+
+#elif defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK66FX1M0__) || defined(__MK64FX512__)
+#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
+#define PIN_TO_BITMASK(pin)             (1)
+#define IO_REG_TYPE uint8_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         (*((base)+512))
+#define DIRECT_MODE_INPUT(base, mask)   (*((base)+640) = 0)
+#define DIRECT_MODE_OUTPUT(base, mask)  (*((base)+640) = 1)
+#define DIRECT_WRITE_LOW(base, mask)    (*((base)+256) = 1)
+#define DIRECT_WRITE_HIGH(base, mask)   (*((base)+128) = 1)
+
+#elif defined(__MKL26Z64__)
+#define PIN_TO_BASEREG(pin)             (portOutputRegister(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint8_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         ((*((base)+16) & (mask)) ? 1 : 0)
+#define DIRECT_MODE_INPUT(base, mask)   (*((base)+20) &= ~(mask))
+#define DIRECT_MODE_OUTPUT(base, mask)  (*((base)+20) |= (mask))
+#define DIRECT_WRITE_LOW(base, mask)    (*((base)+8) = (mask))
+#define DIRECT_WRITE_HIGH(base, mask)   (*((base)+4) = (mask))
+
+#elif defined(__SAM3X8E__)
+// Arduino 1.5.1 may have a bug in delayMicroseconds() on Arduino Due.
+// http://arduino.cc/forum/index.php/topic,141030.msg1076268.html#msg1076268
+// If you have trouble with OneWire on Arduino Due, please check the
+// status of delayMicroseconds() before reporting a bug in OneWire!
+#define PIN_TO_BASEREG(pin)             (&(digitalPinToPort(pin)->PIO_PER))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint32_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         (((*((base)+15)) & (mask)) ? 1 : 0)
+#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+5)) = (mask))
+#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+4)) = (mask))
+#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+13)) = (mask))
+#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+12)) = (mask))
+#ifndef PROGMEM
+#define PROGMEM
+#endif
+#ifndef pgm_read_byte
+#define pgm_read_byte(addr) (*(const uint8_t *)(addr))
+#endif
+
+#elif defined(__PIC32MX__)
+#define PIN_TO_BASEREG(pin)             (portModeRegister(digitalPinToPort(pin)))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint32_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         (((*(base+4)) & (mask)) ? 1 : 0)  //PORTX + 0x10
+#define DIRECT_MODE_INPUT(base, mask)   ((*(base+2)) = (mask))            //TRISXSET + 0x08
+#define DIRECT_MODE_OUTPUT(base, mask)  ((*(base+1)) = (mask))            //TRISXCLR + 0x04
+#define DIRECT_WRITE_LOW(base, mask)    ((*(base+8+1)) = (mask))          //LATXCLR  + 0x24
+#define DIRECT_WRITE_HIGH(base, mask)   ((*(base+8+2)) = (mask))          //LATXSET + 0x28
+
+#elif defined(ARDUINO_ARCH_ESP8266)
+#define PIN_TO_BASEREG(pin)             ((volatile uint32_t*) GPO)
+#define PIN_TO_BITMASK(pin)             (1 << pin)
+#define IO_REG_TYPE uint32_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         ((GPI & (mask)) ? 1 : 0)    //GPIO_IN_ADDRESS
+#define DIRECT_MODE_INPUT(base, mask)   (GPE &= ~(mask))            //GPIO_ENABLE_W1TC_ADDRESS
+#define DIRECT_MODE_OUTPUT(base, mask)  (GPE |= (mask))             //GPIO_ENABLE_W1TS_ADDRESS
+#define DIRECT_WRITE_LOW(base, mask)    (GPOC = (mask))             //GPIO_OUT_W1TC_ADDRESS
+#define DIRECT_WRITE_HIGH(base, mask)   (GPOS = (mask))             //GPIO_OUT_W1TS_ADDRESS
+
+#elif defined(__SAMD21G18A__)
+#define PIN_TO_BASEREG(pin)             portModeRegister(digitalPinToPort(pin))
+#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
+#define IO_REG_TYPE uint32_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, mask)         (((*((base)+8)) & (mask)) ? 1 : 0)
+#define DIRECT_MODE_INPUT(base, mask)   ((*((base)+1)) = (mask))
+#define DIRECT_MODE_OUTPUT(base, mask)  ((*((base)+2)) = (mask))
+#define DIRECT_WRITE_LOW(base, mask)    ((*((base)+5)) = (mask))
+#define DIRECT_WRITE_HIGH(base, mask)   ((*((base)+6)) = (mask))
+
+#elif defined(RBL_NRF51822)
+#define PIN_TO_BASEREG(pin)             (0)
+#define PIN_TO_BITMASK(pin)             (pin)
+#define IO_REG_TYPE uint32_t
+#define IO_REG_ASM
+#define DIRECT_READ(base, pin)          nrf_gpio_pin_read(pin)
+#define DIRECT_WRITE_LOW(base, pin)     nrf_gpio_pin_clear(pin)
+#define DIRECT_WRITE_HIGH(base, pin)    nrf_gpio_pin_set(pin)
+#define DIRECT_MODE_INPUT(base, pin)    nrf_gpio_cfg_input(pin, NRF_GPIO_PIN_NOPULL)
+#define DIRECT_MODE_OUTPUT(base, pin)   nrf_gpio_cfg_output(pin)
+
+#elif defined(__arc__) /* Arduino101/Genuino101 specifics */
+
+#include "scss_registers.h"
+#include "portable.h"
+#include "avr/pgmspace.h"
+
+#define GPIO_ID(pin)			(g_APinDescription[pin].ulGPIOId)
+#define GPIO_TYPE(pin)			(g_APinDescription[pin].ulGPIOType)
+#define GPIO_BASE(pin)			(g_APinDescription[pin].ulGPIOBase)
+#define DIR_OFFSET_SS			0x01
+#define DIR_OFFSET_SOC			0x04
+#define EXT_PORT_OFFSET_SS		0x0A
+#define EXT_PORT_OFFSET_SOC		0x50
+
+/* GPIO registers base address */
+#define PIN_TO_BASEREG(pin)		((volatile uint32_t *)g_APinDescription[pin].ulGPIOBase)
+#define PIN_TO_BITMASK(pin)		pin
+#define IO_REG_TYPE			uint32_t
+#define IO_REG_ASM
+
+static inline __attribute__((always_inline))
+IO_REG_TYPE directRead(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    IO_REG_TYPE ret;
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        ret = READ_ARC_REG(((IO_REG_TYPE)base + EXT_PORT_OFFSET_SS));
+    } else {
+        ret = MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, EXT_PORT_OFFSET_SOC);
+    }
+    return ((ret >> GPIO_ID(pin)) & 0x01);
+}
+
+static inline __attribute__((always_inline))
+void directModeInput(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        WRITE_ARC_REG(READ_ARC_REG((((IO_REG_TYPE)base) + DIR_OFFSET_SS)) & ~(0x01 << GPIO_ID(pin)),
+			((IO_REG_TYPE)(base) + DIR_OFFSET_SS));
+    } else {
+        MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, DIR_OFFSET_SOC) &= ~(0x01 << GPIO_ID(pin));
+    }
+}
+
+static inline __attribute__((always_inline))
+void directModeOutput(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        WRITE_ARC_REG(READ_ARC_REG(((IO_REG_TYPE)(base) + DIR_OFFSET_SS)) | (0x01 << GPIO_ID(pin)),
+			((IO_REG_TYPE)(base) + DIR_OFFSET_SS));
+    } else {
+        MMIO_REG_VAL_FROM_BASE((IO_REG_TYPE)base, DIR_OFFSET_SOC) |= (0x01 << GPIO_ID(pin));
+    }
+}
+
+static inline __attribute__((always_inline))
+void directWriteLow(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        WRITE_ARC_REG(READ_ARC_REG(base) & ~(0x01 << GPIO_ID(pin)), base);
+    } else {
+        MMIO_REG_VAL(base) &= ~(0x01 << GPIO_ID(pin));
+    }
+}
+
+static inline __attribute__((always_inline))
+void directWriteHigh(volatile IO_REG_TYPE *base, IO_REG_TYPE pin)
+{
+    if (SS_GPIO == GPIO_TYPE(pin)) {
+        WRITE_ARC_REG(READ_ARC_REG(base) | (0x01 << GPIO_ID(pin)), base);
+    } else {
+        MMIO_REG_VAL(base) |= (0x01 << GPIO_ID(pin));
+    }
+}
+
+#define DIRECT_READ(base, pin)		directRead(base, pin)
+#define DIRECT_MODE_INPUT(base, pin)	directModeInput(base, pin)
+#define DIRECT_MODE_OUTPUT(base, pin)	directModeOutput(base, pin)
+#define DIRECT_WRITE_LOW(base, pin)	directWriteLow(base, pin)
+#define DIRECT_WRITE_HIGH(base, pin)	directWriteHigh(base, pin)
+
+#else
+#define PIN_TO_BASEREG(pin)             (0)
+#define PIN_TO_BITMASK(pin)             (pin)
+#define IO_REG_TYPE unsigned int
+#define IO_REG_ASM
+#define DIRECT_READ(base, pin)          digitalRead(pin)
+#define DIRECT_WRITE_LOW(base, pin)     digitalWrite(pin, LOW)
+#define DIRECT_WRITE_HIGH(base, pin)    digitalWrite(pin, HIGH)
+#define DIRECT_MODE_INPUT(base, pin)    pinMode(pin,INPUT)
+#define DIRECT_MODE_OUTPUT(base, pin)   pinMode(pin,OUTPUT)
+#warning "OneWire. Fallback mode. Using API calls for pinMode,digitalRead and digitalWrite. Operation of this library is not guaranteed on this architecture."
+
+#endif
+
+
+class OneWire
+{
+  private:
+    IO_REG_TYPE bitmask;
+    volatile IO_REG_TYPE *baseReg;
+
+#if ONEWIRE_SEARCH
+    // global search state
+    unsigned char ROM_NO[8];
+    uint8_t LastDiscrepancy;
+    uint8_t LastFamilyDiscrepancy;
+    uint8_t LastDeviceFlag;
+#endif
+
+  public:
+    OneWire( uint8_t pin);
+
+    // Perform a 1-Wire reset cycle. Returns 1 if a device responds
+    // with a presence pulse.  Returns 0 if there is no device or the
+    // bus is shorted or otherwise held low for more than 250uS
+    uint8_t reset(void);
+
+    // Issue a 1-Wire rom select command, you do the reset first.
+    void select(const uint8_t rom[8]);
+
+    // Issue a 1-Wire rom skip command, to address all on bus.
+    void skip(void);
+
+    // Write a byte. If 'power' is one then the wire is held high at
+    // the end for parasitically powered devices. You are responsible
+    // for eventually depowering it by calling depower() or doing
+    // another read or write.
+    void write(uint8_t v, uint8_t power = 0);
+
+    void write_bytes(const uint8_t *buf, uint16_t count, bool power = 0);
+
+    // Read a byte.
+    uint8_t read(void);
+
+    void read_bytes(uint8_t *buf, uint16_t count);
+
+    // Write a bit. The bus is always left powered at the end, see
+    // note in write() about that.
+    void write_bit(uint8_t v);
+
+    // Read a bit.
+    uint8_t read_bit(void);
+
+    // Stop forcing power onto the bus. You only need to do this if
+    // you used the 'power' flag to write() or used a write_bit() call
+    // and aren't about to do another read or write. You would rather
+    // not leave this powered if you don't have to, just in case
+    // someone shorts your bus.
+    void depower(void);
+
+#if ONEWIRE_SEARCH
+    // Clear the search state so that if will start from the beginning again.
+    void reset_search();
+
+    // Setup the search to find the device type 'family_code' on the next call
+    // to search(*newAddr) if it is present.
+    void target_search(uint8_t family_code);
+
+    // Look for the next device. Returns 1 if a new address has been
+    // returned. A zero might mean that the bus is shorted, there are
+    // no devices, or you have already retrieved all of them.  It
+    // might be a good idea to check the CRC to make sure you didn't
+    // get garbage.  The order is deterministic. You will always get
+    // the same devices in the same order.
+    uint8_t search(uint8_t *newAddr, bool search_mode = true);
+#endif
+
+#if ONEWIRE_CRC
+    // Compute a Dallas Semiconductor 8 bit CRC, these are used in the
+    // ROM and scratchpad registers.
+    static uint8_t crc8(const uint8_t *addr, uint8_t len);
+
+#if ONEWIRE_CRC16
+    // Compute the 1-Wire CRC16 and compare it against the received CRC.
+    // Example usage (reading a DS2408):
+    //    // Put everything in a buffer so we can compute the CRC easily.
+    //    uint8_t buf[13];
+    //    buf[0] = 0xF0;    // Read PIO Registers
+    //    buf[1] = 0x88;    // LSB address
+    //    buf[2] = 0x00;    // MSB address
+    //    WriteBytes(net, buf, 3);    // Write 3 cmd bytes
+    //    ReadBytes(net, buf+3, 10);  // Read 6 data bytes, 2 0xFF, 2 CRC16
+    //    if (!CheckCRC16(buf, 11, &buf[11])) {
+    //        // Handle error.
+    //    }     
+    //          
+    // @param input - Array of bytes to checksum.
+    // @param len - How many bytes to use.
+    // @param inverted_crc - The two CRC16 bytes in the received data.
+    //                       This should just point into the received data,
+    //                       *not* at a 16-bit integer.
+    // @param crc - The crc starting value (optional)
+    // @return True, iff the CRC matches.
+    static bool check_crc16(const uint8_t* input, uint16_t len, const uint8_t* inverted_crc, uint16_t crc = 0);
+
+    // Compute a Dallas Semiconductor 16 bit CRC.  This is required to check
+    // the integrity of data received from many 1-Wire devices.  Note that the
+    // CRC computed here is *not* what you'll get from the 1-Wire network,
+    // for two reasons:
+    //   1) The CRC is transmitted bitwise inverted.
+    //   2) Depending on the endian-ness of your processor, the binary
+    //      representation of the two-byte return value may have a different
+    //      byte order than the two bytes you get from 1-Wire.
+    // @param input - Array of bytes to checksum.
+    // @param len - How many bytes to use.
+    // @param crc - The crc starting value (optional)
+    // @return The CRC16, as defined by Dallas Semiconductor.
+    static uint16_t crc16(const uint8_t* input, uint16_t len, uint16_t crc = 0);
+#endif
+#endif
+};
+
+#endif
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/WiFi101Stream.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/WiFi101Stream.cpp
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFi101Stream.h to avoid linker issues. Legacy WiFi and modern WiFi101
+ * both define WiFiClass which will cause linker errors whenever Firmata.h is included.
+ */
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/WiFi101Stream.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/WiFi101Stream.h
@@ -0,0 +1,259 @@
+/*
+  WiFi101Stream.h
+  An Arduino Stream that wraps an instance of a WiFi101 server. For use
+  with Arduino WiFi 101 shield, Arduino MKR1000 and other boards and
+  shields that are compatible with the Arduino WiFi101 library.
+
+  Copyright (C) 2015-2016 Jesse Frush. 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 WIFI101_STREAM_H
+#define WIFI101_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi101.h>
+
+
+class WiFi101Stream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFi101Stream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI101_STREAM_H
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/WiFiStream.cpp
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/WiFiStream.cpp
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFiStream.h to avoid linker issues. Legacy WiFi and modern WiFi101 both
+ * define WiFiClass which will cause linker errors whenever Firmata.h is included.
+ */
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/WiFiStream.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/WiFiStream.h
@@ -0,0 +1,258 @@
+/*
+  WiFiStream.h
+  An Arduino Stream that wraps an instance of a WiFi server. For use
+  with legacy Arduino WiFi shield and other boards and sheilds that
+  are compatible with the Arduino WiFi library.
+
+  Copyright (C) 2015-2016 Jesse Frush. 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 WIFI_STREAM_H
+#define WIFI_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi.h>
+
+class WiFiStream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFiStream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI_STREAM_H
--- a/libraries/FirmataWithDeviceFeature/src-features/utility/firmataDebug.h
+++ b/libraries/FirmataWithDeviceFeature/src-features/utility/firmataDebug.h
@@ -0,0 +1,14 @@
+#ifndef FIRMATA_DEBUG_H
+#define FIRMATA_DEBUG_H
+
+#ifdef SERIAL_DEBUG
+  #define DEBUG_BEGIN(baud) Serial.begin(baud); while(!Serial) {;}
+  #define DEBUG_PRINTLN(x)  Serial.println (x)
+  #define DEBUG_PRINT(x)    Serial.print (x)
+#else
+  #define DEBUG_BEGIN(baud)
+  #define DEBUG_PRINTLN(x)
+  #define DEBUG_PRINT(x)
+#endif
+
+#endif /* FIRMATA_DEBUG_H */