Arduino/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino

   1 /*
   2   Firmata is a generic protocol for communicating with microcontrollers
   3   from software on a host computer. It is intended to work with
   4   any host computer software package.
   5
   6   To download a host software package, please click on the following link
   7   to open the list of Firmata client libraries in your default browser.
   8
   9   https://github.com/firmata/arduino#firmata-client-libraries
  10
  11   Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
  12   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
  13   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
  14   Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
  15
  16   This library is free software; you can redistribute it and/or
  17   modify it under the terms of the GNU Lesser General Public
  18   License as published by the Free Software Foundation; either
  19   version 2.1 of the License, or (at your option) any later version.
  20
  21   See file LICENSE.txt for further informations on licensing terms.
  22
  23   Last updated August 17th, 2017
  24 */
  25
  26 #include <Servo.h>
  27 #include <Wire.h>
  28 #include <Firmata.h>
  29
  30 #define I2C_WRITE                   B00000000
  31 #define I2C_READ                    B00001000
  32 #define I2C_READ_CONTINUOUSLY       B00010000
  33 #define I2C_STOP_READING            B00011000
  34 #define I2C_READ_WRITE_MODE_MASK    B00011000
  35 #define I2C_10BIT_ADDRESS_MODE_MASK B00100000
  36 #define I2C_END_TX_MASK             B01000000
  37 #define I2C_STOP_TX                 1
  38 #define I2C_RESTART_TX              0
  39 #define I2C_MAX_QUERIES             8
  40 #define I2C_REGISTER_NOT_SPECIFIED  -1
  41
  42 // the minimum interval for sampling analog input
  43 #define MINIMUM_SAMPLING_INTERVAL   1
  44
  45
  46 /*==============================================================================
  47  * GLOBAL VARIABLES
  48  *============================================================================*/
  49
  50 #ifdef FIRMATA_SERIAL_FEATURE
  51 SerialFirmata serialFeature;
  52 #endif
  53
  54 /* analog inputs */
  55 int analogInputsToReport = 0; // bitwise array to store pin reporting
  56
  57 /* digital input ports */
  58 byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
  59 byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
  60
  61 /* pins configuration */
  62 byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
  63
  64 /* timer variables */
  65 unsigned long currentMillis;        // store the current value from millis()
  66 unsigned long previousMillis;       // for comparison with currentMillis
  67 unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
  68
  69 /* i2c data */
  70 struct i2c_device_info {
  71   byte addr;
  72   int reg;
  73   byte bytes;
  74   byte stopTX;
  75 };
  76
  77 /* for i2c read continuous more */
  78 i2c_device_info query[I2C_MAX_QUERIES];
  79
  80 byte i2cRxData[64];
  81 boolean isI2CEnabled = false;
  82 signed char queryIndex = -1;
  83 // default delay time between i2c read request and Wire.requestFrom()
  84 unsigned int i2cReadDelayTime = 0;
  85
  86 Servo servos[MAX_SERVOS];
  87 byte servoPinMap[TOTAL_PINS];
  88 byte detachedServos[MAX_SERVOS];
  89 byte detachedServoCount = 0;
  90 byte servoCount = 0;
  91
  92 boolean isResetting = false;
  93
  94 // Forward declare a few functions to avoid compiler errors with older versions
  95 // of the Arduino IDE.
  96 void setPinModeCallback(byte, int);
  97 void reportAnalogCallback(byte analogPin, int value);
  98 void sysexCallback(byte, byte, byte*);
  99
 100 /* utility functions */
 101 void wireWrite(byte data)
 102 {
 103 #if ARDUINO >= 100
 104   Wire.write((byte)data);
 105 #else
 106   Wire.send(data);
 107 #endif
 108 }
 109
 110 byte wireRead(void)
 111 {
 112 #if ARDUINO >= 100
 113   return Wire.read();
 114 #else
 115   return Wire.receive();
 116 #endif
 117 }
 118
 119 /*==============================================================================
 120  * FUNCTIONS
 121  *============================================================================*/
 122
 123 void attachServo(byte pin, int minPulse, int maxPulse)
 124 {
 125   if (servoCount < MAX_SERVOS) {
 126     // reuse indexes of detached servos until all have been reallocated
 127     if (detachedServoCount > 0) {
 128       servoPinMap[pin] = detachedServos[detachedServoCount - 1];
 129       if (detachedServoCount > 0) detachedServoCount--;
 130     } else {
 131       servoPinMap[pin] = servoCount;
 132       servoCount++;
 133     }
 134     if (minPulse > 0 && maxPulse > 0) {
 135       servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
 136     } else {
 137       servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
 138     }
 139   } else {
 140     Firmata.sendString("Max servos attached");
 141   }
 142 }
 143
 144 void detachServo(byte pin)
 145 {
 146   servos[servoPinMap[pin]].detach();
 147   // if we're detaching the last servo, decrement the count
 148   // otherwise store the index of the detached servo
 149   if (servoPinMap[pin] == servoCount && servoCount > 0) {
 150     servoCount--;
 151   } else if (servoCount > 0) {
 152     // keep track of detached servos because we want to reuse their indexes
 153     // before incrementing the count of attached servos
 154     detachedServoCount++;
 155     detachedServos[detachedServoCount - 1] = servoPinMap[pin];
 156   }
 157
 158   servoPinMap[pin] = 255;
 159 }
 160
 161 void enableI2CPins()
 162 {
 163   byte i;
 164   // is there a faster way to do this? would probaby require importing
 165   // Arduino.h to get SCL and SDA pins
 166   for (i = 0; i < TOTAL_PINS; i++) {
 167     if (IS_PIN_I2C(i)) {
 168       // mark pins as i2c so they are ignore in non i2c data requests
 169       setPinModeCallback(i, PIN_MODE_I2C);
 170     }
 171   }
 172
 173   isI2CEnabled = true;
 174
 175   Wire.begin();
 176 }
 177
 178 /* disable the i2c pins so they can be used for other functions */
 179 void disableI2CPins() {
 180   isI2CEnabled = false;
 181   // disable read continuous mode for all devices
 182   queryIndex = -1;
 183 }
 184
 185 void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
 186   // allow I2C requests that don't require a register read
 187   // for example, some devices using an interrupt pin to signify new data available
 188   // do not always require the register read so upon interrupt you call Wire.requestFrom()
 189   if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
 190     Wire.beginTransmission(address);
 191     wireWrite((byte)theRegister);
 192     Wire.endTransmission(stopTX); // default = true
 193     // do not set a value of 0
 194     if (i2cReadDelayTime > 0) {
 195       // delay is necessary for some devices such as WiiNunchuck
 196       delayMicroseconds(i2cReadDelayTime);
 197     }
 198   } else {
 199     theRegister = 0;  // fill the register with a dummy value
 200   }
 201
 202   Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
 203
 204   // check to be sure correct number of bytes were returned by slave
 205   if (numBytes < Wire.available()) {
 206     Firmata.sendString("I2C: Too many bytes received");
 207   } else if (numBytes > Wire.available()) {
 208     Firmata.sendString("I2C: Too few bytes received");
 209   }
 210
 211   i2cRxData[0] = address;
 212   i2cRxData[1] = theRegister;
 213
 214   for (int i = 0; i < numBytes && Wire.available(); i++) {
 215     i2cRxData[2 + i] = wireRead();
 216   }
 217
 218   // send slave address, register and received bytes
 219   Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
 220 }
 221
 222 void outputPort(byte portNumber, byte portValue, byte forceSend)
 223 {
 224   // pins not configured as INPUT are cleared to zeros
 225   portValue = portValue & portConfigInputs[portNumber];
 226   // only send if the value is different than previously sent
 227   if (forceSend || previousPINs[portNumber] != portValue) {
 228     Firmata.sendDigitalPort(portNumber, portValue);
 229     previousPINs[portNumber] = portValue;
 230   }
 231 }
 232
 233 /* -----------------------------------------------------------------------------
 234  * check all the active digital inputs for change of state, then add any events
 235  * to the Serial output queue using Serial.print() */
 236 void checkDigitalInputs(void)
 237 {
 238   /* Using non-looping code allows constants to be given to readPort().
 239    * The compiler will apply substantial optimizations if the inputs
 240    * to readPort() are compile-time constants. */
 241   if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
 242   if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
 243   if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
 244   if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
 245   if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
 246   if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
 247   if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
 248   if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
 249   if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
 250   if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
 251   if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
 252   if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
 253   if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
 254   if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
 255   if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
 256   if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
 257 }
 258
 259 // -----------------------------------------------------------------------------
 260 /* sets the pin mode to the correct state and sets the relevant bits in the
 261  * two bit-arrays that track Digital I/O and PWM status
 262  */
 263 void setPinModeCallback(byte pin, int mode)
 264 {
 265   if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
 266     return;
 267
 268   if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
 269     // disable i2c so pins can be used for other functions
 270     // the following if statements should reconfigure the pins properly
 271     disableI2CPins();
 272   }
 273   if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
 274     if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
 275       detachServo(pin);
 276     }
 277   }
 278   if (IS_PIN_ANALOG(pin)) {
 279     reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
 280   }
 281   if (IS_PIN_DIGITAL(pin)) {
 282     if (mode == INPUT || mode == PIN_MODE_PULLUP) {
 283       portConfigInputs[pin / 8] |= (1 << (pin & 7));
 284     } else {
 285       portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
 286     }
 287   }
 288   Firmata.setPinState(pin, 0);
 289   switch (mode) {
 290     case PIN_MODE_ANALOG:
 291       if (IS_PIN_ANALOG(pin)) {
 292         if (IS_PIN_DIGITAL(pin)) {
 293           pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 294 #if ARDUINO <= 100
 295           // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
 296           digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 297 #endif
 298         }
 299         Firmata.setPinMode(pin, PIN_MODE_ANALOG);
 300       }
 301       break;
 302     case INPUT:
 303       if (IS_PIN_DIGITAL(pin)) {
 304         pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
 305 #if ARDUINO <= 100
 306         // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
 307         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
 308 #endif
 309         Firmata.setPinMode(pin, INPUT);
 310       }
 311       break;
 312     case PIN_MODE_PULLUP:
 313       if (IS_PIN_DIGITAL(pin)) {
 314         pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
 315         Firmata.setPinMode(pin, PIN_MODE_PULLUP);
 316         Firmata.setPinState(pin, 1);
 317       }
 318       break;
 319     case OUTPUT:
 320       if (IS_PIN_DIGITAL(pin)) {
 321         if (Firmata.getPinMode(pin) == PIN_MODE_PWM) {
 322           // Disable PWM if pin mode was previously set to PWM.
 323           digitalWrite(PIN_TO_DIGITAL(pin), LOW);
 324         }
 325         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
 326         Firmata.setPinMode(pin, OUTPUT);
 327       }
 328       break;
 329     case PIN_MODE_PWM:
 330       if (IS_PIN_PWM(pin)) {
 331         pinMode(PIN_TO_PWM(pin), OUTPUT);
 332         analogWrite(PIN_TO_PWM(pin), 0);
 333         Firmata.setPinMode(pin, PIN_MODE_PWM);
 334       }
 335       break;
 336     case PIN_MODE_SERVO:
 337       if (IS_PIN_DIGITAL(pin)) {
 338         Firmata.setPinMode(pin, PIN_MODE_SERVO);
 339         if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
 340           // pass -1 for min and max pulse values to use default values set
 341           // by Servo library
 342           attachServo(pin, -1, -1);
 343         }
 344       }
 345       break;
 346     case PIN_MODE_I2C:
 347       if (IS_PIN_I2C(pin)) {
 348         // mark the pin as i2c
 349         // the user must call I2C_CONFIG to enable I2C for a device
 350         Firmata.setPinMode(pin, PIN_MODE_I2C);
 351       }
 352       break;
 353     case PIN_MODE_SERIAL:
 354 #ifdef FIRMATA_SERIAL_FEATURE
 355       serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
 356 #endif
 357       break;
 358     default:
 359       Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
 360   }
 361   // TODO: save status to EEPROM here, if changed
 362 }
 363
 364 /*
 365  * Sets the value of an individual pin. Useful if you want to set a pin value but
 366  * are not tracking the digital port state.
 367  * Can only be used on pins configured as OUTPUT.
 368  * Cannot be used to enable pull-ups on Digital INPUT pins.
 369  */
 370 void setPinValueCallback(byte pin, int value)
 371 {
 372   if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
 373     if (Firmata.getPinMode(pin) == OUTPUT) {
 374       Firmata.setPinState(pin, value);
 375       digitalWrite(PIN_TO_DIGITAL(pin), value);
 376     }
 377   }
 378 }
 379
 380 void analogWriteCallback(byte pin, int value)
 381 {
 382   if (pin < TOTAL_PINS) {
 383     switch (Firmata.getPinMode(pin)) {
 384       case PIN_MODE_SERVO:
 385         if (IS_PIN_DIGITAL(pin))
 386           servos[servoPinMap[pin]].write(value);
 387         Firmata.setPinState(pin, value);
 388         break;
 389       case PIN_MODE_PWM:
 390         if (IS_PIN_PWM(pin))
 391           analogWrite(PIN_TO_PWM(pin), value);
 392         Firmata.setPinState(pin, value);
 393         break;
 394     }
 395   }
 396 }
 397
 398 void digitalWriteCallback(byte port, int value)
 399 {
 400   byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
 401
 402   if (port < TOTAL_PORTS) {
 403     // create a mask of the pins on this port that are writable.
 404     lastPin = port * 8 + 8;
 405     if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
 406     for (pin = port * 8; pin < lastPin; pin++) {
 407       // do not disturb non-digital pins (eg, Rx & Tx)
 408       if (IS_PIN_DIGITAL(pin)) {
 409         // do not touch pins in PWM, ANALOG, SERVO or other modes
 410         if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
 411           pinValue = ((byte)value & mask) ? 1 : 0;
 412           if (Firmata.getPinMode(pin) == OUTPUT) {
 413             pinWriteMask |= mask;
 414           } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
 415             // only handle INPUT here for backwards compatibility
 416 #if ARDUINO > 100
 417             pinMode(pin, INPUT_PULLUP);
 418 #else
 419             // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
 420             pinWriteMask |= mask;
 421 #endif
 422           }
 423           Firmata.setPinState(pin, pinValue);
 424         }
 425       }
 426       mask = mask << 1;
 427     }
 428     writePort(port, (byte)value, pinWriteMask);
 429   }
 430 }
 431
 432
 433 // -----------------------------------------------------------------------------
 434 /* sets bits in a bit array (int) to toggle the reporting of the analogIns
 435  */
 436 //void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
 437 //}
 438 void reportAnalogCallback(byte analogPin, int value)
 439 {
 440   if (analogPin < TOTAL_ANALOG_PINS) {
 441     if (value == 0) {
 442       analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
 443     } else {
 444       analogInputsToReport = analogInputsToReport | (1 << analogPin);
 445       // prevent during system reset or all analog pin values will be reported
 446       // which may report noise for unconnected analog pins
 447       if (!isResetting) {
 448         // Send pin value immediately. This is helpful when connected via
 449         // ethernet, wi-fi or bluetooth so pin states can be known upon
 450         // reconnecting.
 451         Firmata.sendAnalog(analogPin, analogRead(analogPin));
 452       }
 453     }
 454   }
 455   // TODO: save status to EEPROM here, if changed
 456 }
 457
 458 void reportDigitalCallback(byte port, int value)
 459 {
 460   if (port < TOTAL_PORTS) {
 461     reportPINs[port] = (byte)value;
 462     // Send port value immediately. This is helpful when connected via
 463     // ethernet, wi-fi or bluetooth so pin states can be known upon
 464     // reconnecting.
 465     if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
 466   }
 467   // do not disable analog reporting on these 8 pins, to allow some
 468   // pins used for digital, others analog.  Instead, allow both types
 469   // of reporting to be enabled, but check if the pin is configured
 470   // as analog when sampling the analog inputs.  Likewise, while
 471   // scanning digital pins, portConfigInputs will mask off values from any
 472   // pins configured as analog
 473 }
 474
 475 /*==============================================================================
 476  * SYSEX-BASED commands
 477  *============================================================================*/
 478
 479 void sysexCallback(byte command, byte argc, byte *argv)
 480 {
 481   byte mode;
 482   byte stopTX;
 483   byte slaveAddress;
 484   byte data;
 485   int slaveRegister;
 486   unsigned int delayTime;
 487
 488   switch (command) {
 489     case I2C_REQUEST:
 490       mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
 491       if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
 492         Firmata.sendString("10-bit addressing not supported");
 493         return;
 494       }
 495       else {
 496         slaveAddress = argv[0];
 497       }
 498
 499       // need to invert the logic here since 0 will be default for client
 500       // libraries that have not updated to add support for restart tx
 501       if (argv[1] & I2C_END_TX_MASK) {
 502         stopTX = I2C_RESTART_TX;
 503       }
 504       else {
 505         stopTX = I2C_STOP_TX; // default
 506       }
 507
 508       switch (mode) {
 509         case I2C_WRITE:
 510           Wire.beginTransmission(slaveAddress);
 511           for (byte i = 2; i < argc; i += 2) {
 512             data = argv[i] + (argv[i + 1] << 7);
 513             wireWrite(data);
 514           }
 515           Wire.endTransmission();
 516           delayMicroseconds(70);
 517           break;
 518         case I2C_READ:
 519           if (argc == 6) {
 520             // a slave register is specified
 521             slaveRegister = argv[2] + (argv[3] << 7);
 522             data = argv[4] + (argv[5] << 7);  // bytes to read
 523           }
 524           else {
 525             // a slave register is NOT specified
 526             slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
 527             data = argv[2] + (argv[3] << 7);  // bytes to read
 528           }
 529           readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
 530           break;
 531         case I2C_READ_CONTINUOUSLY:
 532           if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
 533             // too many queries, just ignore
 534             Firmata.sendString("too many queries");
 535             break;
 536           }
 537           if (argc == 6) {
 538             // a slave register is specified
 539             slaveRegister = argv[2] + (argv[3] << 7);
 540             data = argv[4] + (argv[5] << 7);  // bytes to read
 541           }
 542           else {
 543             // a slave register is NOT specified
 544             slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
 545             data = argv[2] + (argv[3] << 7);  // bytes to read
 546           }
 547           queryIndex++;
 548           query[queryIndex].addr = slaveAddress;
 549           query[queryIndex].reg = slaveRegister;
 550           query[queryIndex].bytes = data;
 551           query[queryIndex].stopTX = stopTX;
 552           break;
 553         case I2C_STOP_READING:
 554           byte queryIndexToSkip;
 555           // if read continuous mode is enabled for only 1 i2c device, disable
 556           // read continuous reporting for that device
 557           if (queryIndex <= 0) {
 558             queryIndex = -1;
 559           } else {
 560             queryIndexToSkip = 0;
 561             // if read continuous mode is enabled for multiple devices,
 562             // determine which device to stop reading and remove it's data from
 563             // the array, shifiting other array data to fill the space
 564             for (byte i = 0; i < queryIndex + 1; i++) {
 565               if (query[i].addr == slaveAddress) {
 566                 queryIndexToSkip = i;
 567                 break;
 568               }
 569             }
 570
 571             for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
 572               if (i < I2C_MAX_QUERIES) {
 573                 query[i].addr = query[i + 1].addr;
 574                 query[i].reg = query[i + 1].reg;
 575                 query[i].bytes = query[i + 1].bytes;
 576                 query[i].stopTX = query[i + 1].stopTX;
 577               }
 578             }
 579             queryIndex--;
 580           }
 581           break;
 582         default:
 583           break;
 584       }
 585       break;
 586     case I2C_CONFIG:
 587       delayTime = (argv[0] + (argv[1] << 7));
 588
 589       if (argc > 1 && delayTime > 0) {
 590         i2cReadDelayTime = delayTime;
 591       }
 592
 593       if (!isI2CEnabled) {
 594         enableI2CPins();
 595       }
 596
 597       break;
 598     case SERVO_CONFIG:
 599       if (argc > 4) {
 600         // these vars are here for clarity, they'll optimized away by the compiler
 601         byte pin = argv[0];
 602         int minPulse = argv[1] + (argv[2] << 7);
 603         int maxPulse = argv[3] + (argv[4] << 7);
 604
 605         if (IS_PIN_DIGITAL(pin)) {
 606           if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
 607             detachServo(pin);
 608           }
 609           attachServo(pin, minPulse, maxPulse);
 610           setPinModeCallback(pin, PIN_MODE_SERVO);
 611         }
 612       }
 613       break;
 614     case SAMPLING_INTERVAL:
 615       if (argc > 1) {
 616         samplingInterval = argv[0] + (argv[1] << 7);
 617         if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
 618           samplingInterval = MINIMUM_SAMPLING_INTERVAL;
 619         }
 620       } else {
 621         //Firmata.sendString("Not enough data");
 622       }
 623       break;
 624     case EXTENDED_ANALOG:
 625       if (argc > 1) {
 626         int val = argv[1];
 627         if (argc > 2) val |= (argv[2] << 7);
 628         if (argc > 3) val |= (argv[3] << 14);
 629         analogWriteCallback(argv[0], val);
 630       }
 631       break;
 632     case CAPABILITY_QUERY:
 633       Firmata.write(START_SYSEX);
 634       Firmata.write(CAPABILITY_RESPONSE);
 635       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
 636         if (IS_PIN_DIGITAL(pin)) {
 637           Firmata.write((byte)INPUT);
 638           Firmata.write(1);
 639           Firmata.write((byte)PIN_MODE_PULLUP);
 640           Firmata.write(1);
 641           Firmata.write((byte)OUTPUT);
 642           Firmata.write(1);
 643         }
 644         if (IS_PIN_ANALOG(pin)) {
 645           Firmata.write(PIN_MODE_ANALOG);
 646           Firmata.write(10); // 10 = 10-bit resolution
 647         }
 648         if (IS_PIN_PWM(pin)) {
 649           Firmata.write(PIN_MODE_PWM);
 650           Firmata.write(DEFAULT_PWM_RESOLUTION);
 651         }
 652         if (IS_PIN_DIGITAL(pin)) {
 653           Firmata.write(PIN_MODE_SERVO);
 654           Firmata.write(14);
 655         }
 656         if (IS_PIN_I2C(pin)) {
 657           Firmata.write(PIN_MODE_I2C);
 658           Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
 659         }
 660 #ifdef FIRMATA_SERIAL_FEATURE
 661         serialFeature.handleCapability(pin);
 662 #endif
 663         Firmata.write(127);
 664       }
 665       Firmata.write(END_SYSEX);
 666       break;
 667     case PIN_STATE_QUERY:
 668       if (argc > 0) {
 669         byte pin = argv[0];
 670         Firmata.write(START_SYSEX);
 671         Firmata.write(PIN_STATE_RESPONSE);
 672         Firmata.write(pin);
 673         if (pin < TOTAL_PINS) {
 674           Firmata.write(Firmata.getPinMode(pin));
 675           Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
 676           if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
 677           if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
 678         }
 679         Firmata.write(END_SYSEX);
 680       }
 681       break;
 682     case ANALOG_MAPPING_QUERY:
 683       Firmata.write(START_SYSEX);
 684       Firmata.write(ANALOG_MAPPING_RESPONSE);
 685       for (byte pin = 0; pin < TOTAL_PINS; pin++) {
 686         Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
 687       }
 688       Firmata.write(END_SYSEX);
 689       break;
 690
 691     case SERIAL_MESSAGE:
 692 #ifdef FIRMATA_SERIAL_FEATURE
 693       serialFeature.handleSysex(command, argc, argv);
 694 #endif
 695       break;
 696   }
 697 }
 698
 699 /*==============================================================================
 700  * SETUP()
 701  *============================================================================*/
 702
 703 void systemResetCallback()
 704 {
 705   isResetting = true;
 706
 707   // initialize a defalt state
 708   // TODO: option to load config from EEPROM instead of default
 709
 710 #ifdef FIRMATA_SERIAL_FEATURE
 711   serialFeature.reset();
 712 #endif
 713
 714   if (isI2CEnabled) {
 715     disableI2CPins();
 716   }
 717
 718   for (byte i = 0; i < TOTAL_PORTS; i++) {
 719     reportPINs[i] = false;    // by default, reporting off
 720     portConfigInputs[i] = 0;  // until activated
 721     previousPINs[i] = 0;
 722   }
 723
 724   for (byte i = 0; i < TOTAL_PINS; i++) {
 725     // pins with analog capability default to analog input
 726     // otherwise, pins default to digital output
 727     if (IS_PIN_ANALOG(i)) {
 728       // turns off pullup, configures everything
 729       setPinModeCallback(i, PIN_MODE_ANALOG);
 730     } else if (IS_PIN_DIGITAL(i)) {
 731       // sets the output to 0, configures portConfigInputs
 732       setPinModeCallback(i, OUTPUT);
 733     }
 734
 735     servoPinMap[i] = 255;
 736   }
 737   // by default, do not report any analog inputs
 738   analogInputsToReport = 0;
 739
 740   detachedServoCount = 0;
 741   servoCount = 0;
 742
 743   /* send digital inputs to set the initial state on the host computer,
 744    * since once in the loop(), this firmware will only send on change */
 745   /*
 746   TODO: this can never execute, since no pins default to digital input
 747         but it will be needed when/if we support EEPROM stored config
 748   for (byte i=0; i < TOTAL_PORTS; i++) {
 749     outputPort(i, readPort(i, portConfigInputs[i]), true);
 750   }
 751   */
 752   isResetting = false;
 753 }
 754
 755 void setup()
 756 {
 757   Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
 758
 759   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
 760   Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
 761   Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
 762   Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
 763   Firmata.attach(SET_PIN_MODE, setPinModeCallback);
 764   Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
 765   Firmata.attach(START_SYSEX, sysexCallback);
 766   Firmata.attach(SYSTEM_RESET, systemResetCallback);
 767
 768   // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
 769   // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
 770   // Serial1.begin(57600);
 771   // Firmata.begin(Serial1);
 772   // However do not do this if you are using SERIAL_MESSAGE
 773
 774   Firmata.begin(57600);
 775   while (!Serial) {
 776     ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
 777   }
 778
 779   systemResetCallback();  // reset to default config
 780 }
 781
 782 /*==============================================================================
 783  * LOOP()
 784  *============================================================================*/
 785 void loop()
 786 {
 787   byte pin, analogPin;
 788
 789   /* DIGITALREAD - as fast as possible, check for changes and output them to the
 790    * FTDI buffer using Serial.print()  */
 791   checkDigitalInputs();
 792
 793   /* STREAMREAD - processing incoming messagse as soon as possible, while still
 794    * checking digital inputs.  */
 795   while (Firmata.available())
 796     Firmata.processInput();
 797
 798   // TODO - ensure that Stream buffer doesn't go over 60 bytes
 799
 800   currentMillis = millis();
 801   if (currentMillis - previousMillis > samplingInterval) {
 802     previousMillis += samplingInterval;
 803     /* ANALOGREAD - do all analogReads() at the configured sampling interval */
 804     for (pin = 0; pin < TOTAL_PINS; pin++) {
 805       if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
 806         analogPin = PIN_TO_ANALOG(pin);
 807         if (analogInputsToReport & (1 << analogPin)) {
 808           Firmata.sendAnalog(analogPin, analogRead(analogPin));
 809         }
 810       }
 811     }
 812     // report i2c data for all device with read continuous mode enabled
 813     if (queryIndex > -1) {
 814       for (byte i = 0; i < queryIndex + 1; i++) {
 815         readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
 816       }
 817     }
 818   }
 819
 820 #ifdef FIRMATA_SERIAL_FEATURE
 821   serialFeature.update();
 822 #endif
 823 }