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