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