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