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