1. #include <Servo.h>
  2. #include <Wire.h>
  3. #include <Firmata.h>
  4.  
  5. // move the following defines to Firmata.h?
  6. #define I2C_WRITE B00000000
  7. #define I2C_READ B00001000
  8. #define I2C_READ_CONTINUOUSLY B00010000
  9. #define I2C_STOP_READING B00011000
  10. #define I2C_READ_WRITE_MODE_MASK B00011000
  11. #define I2C_10BIT_ADDRESS_MODE_MASK B00100000
  12.  
  13. #define MAX_QUERIES 8
  14. #define MINIMUM_SAMPLING_INTERVAL 10
  15.  
  16. #define REGISTER_NOT_SPECIFIED -1
  17.  
  18. /*==============================================================================
  19. * GLOBAL VARIABLES
  20. *============================================================================*/
  21.  
  22. /* analog inputs */
  23. int analogInputsToReport = 0; // bitwise array to store pin reporting
  24.  
  25. /* digital input ports */
  26. byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
  27. byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
  28.  
  29. /* pins configuration */
  30. byte pinConfig[TOTAL_PINS];         // configuration of every pin
  31. byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
  32. int pinState[TOTAL_PINS];           // any value that has been written
  33.  
  34. /* timer variables */
  35. unsigned long currentMillis;        // store the current value from millis()
  36. unsigned long previousMillis;       // for comparison with currentMillis
  37. int samplingInterval = 19;          // how often to run the main loop (in ms)
  38.  
  39. /* i2c data */
  40. struct i2c_device_info {
  41.  byte addr;
  42.  byte reg;
  43.  byte bytes;
  44. };
  45.  
  46. /* for i2c read continuous more */
  47. i2c_device_info query[MAX_QUERIES];
  48.  
  49. byte i2cRxData[32];
  50. boolean isI2CEnabled = false;
  51. signed char queryIndex = -1;
  52. unsigned int i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
  53.  
  54. Servo servos[MAX_SERVOS];
  55. /*==============================================================================
  56. * FUNCTIONS
  57. *============================================================================*/
  58.  
  59. void readAndReportData(byte address, int theRegister, byte numBytes) {
  60.  // allow I2C requests that don't require a register read
  61.  // for example, some devices using an interrupt pin to signify new data available
  62.  // do not always require the register read so upon interrupt you call Wire.requestFrom()  
  63.  if (theRegister != REGISTER_NOT_SPECIFIED) {
  64.    Wire.beginTransmission(address);
  65.    #if ARDUINO >= 100
  66.    Wire.write((byte)theRegister);
  67.    #else
  68.    Wire.send((byte)theRegister);
  69.    #endif
  70.    Wire.endTransmission();
  71.    delayMicroseconds(i2cReadDelayTime);  // delay is necessary for some devices such as WiiNunchuck
  72.  } else {
  73.    theRegister = 0;  // fill the register with a dummy value
  74.  }
  75.  
  76.  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
  77.  
  78.  // check to be sure correct number of bytes were returned by slave
  79.  if(numBytes == Wire.available()) {
  80.    i2cRxData[0] = address;
  81.    i2cRxData[1] = theRegister;
  82.    for (int i = 0; i < numBytes; i++) {
  83.      #if ARDUINO >= 100
  84.      i2cRxData[2 + i] = Wire.read();
  85.      #else
  86.      i2cRxData[2 + i] = Wire.receive();
  87.      #endif
  88.    }
  89.  }
  90.  else {
  91.    if(numBytes > Wire.available()) {
  92.      Firmata.sendString("I2C Read Error: Too many bytes received");
  93.    } else {
  94.      Firmata.sendString("I2C Read Error: Too few bytes received");
  95.    }
  96.  }
  97.  
  98.  // send slave address, register and received bytes
  99.  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
  100. }
  101.  
  102. void outputPort(byte portNumber, byte portValue, byte forceSend)
  103. {
  104.  // pins not configured as INPUT are cleared to zeros
  105.  portValue = portValue & portConfigInputs[portNumber];
  106.  // only send if the value is different than previously sent
  107.  if(forceSend || previousPINs[portNumber] != portValue) {
  108.    Firmata.sendDigitalPort(portNumber, portValue);
  109.    previousPINs[portNumber] = portValue;
  110.  }
  111. }
  112.  
  113. /* -----------------------------------------------------------------------------
  114. * check all the active digital inputs for change of state, then add any events
  115. * to the Serial output queue using Serial.print() */
  116. void checkDigitalInputs(void)
  117. {
  118.  /* Using non-looping code allows constants to be given to readPort().
  119.   * The compiler will apply substantial optimizations if the inputs
  120.   * to readPort() are compile-time constants. */
  121.  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
  122.  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
  123.  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
  124.  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
  125.  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
  126.  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
  127.  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
  128.  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
  129.  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
  130.  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
  131.  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
  132.  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
  133.  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
  134.  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
  135.  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
  136.  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
  137. }
  138.  
  139. // -----------------------------------------------------------------------------
  140. /* sets the pin mode to the correct state and sets the relevant bits in the
  141. * two bit-arrays that track Digital I/O and PWM status
  142. */
  143. void setPinModeCallback(byte pin, int mode)
  144. {
  145.  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
  146.    // disable i2c so pins can be used for other functions
  147.    // the following if statements should reconfigure the pins properly
  148.    disableI2CPins();
  149.  }
  150.  if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached()) {
  151.    servos[PIN_TO_SERVO(pin)].detach();
  152.  }
  153.  if (IS_PIN_ANALOG(pin)) {
  154.    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
  155.  }
  156.  if (IS_PIN_DIGITAL(pin)) {
  157.    if (mode == INPUT) {
  158.      portConfigInputs[pin/8] |= (1 << (pin & 7));
  159.    } else {
  160.      portConfigInputs[pin/8] &= ~(1 << (pin & 7));
  161.    }
  162.  }
  163.  pinState[pin] = 0;
  164.  switch(mode) {
  165.  case ANALOG:
  166.    if (IS_PIN_ANALOG(pin)) {
  167.      if (IS_PIN_DIGITAL(pin)) {
  168.        pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
  169.        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
  170.      }
  171.      pinConfig[pin] = ANALOG;
  172.    }
  173.    break;
  174.  case INPUT:
  175.    if (IS_PIN_DIGITAL(pin)) {
  176.      pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
  177.      digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
  178.      pinConfig[pin] = INPUT;
  179.    }
  180.    break;
  181.  case OUTPUT:
  182.    if (IS_PIN_DIGITAL(pin)) {
  183.      digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
  184.      pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
  185.      pinConfig[pin] = OUTPUT;
  186.    }
  187.    break;
  188.  case PWM:
  189.    if (IS_PIN_PWM(pin)) {
  190.      pinMode(PIN_TO_PWM(pin), OUTPUT);
  191.      analogWrite(PIN_TO_PWM(pin), 0);
  192.      pinConfig[pin] = PWM;
  193.    }
  194.    break;
  195.  case SERVO:
  196.    if (IS_PIN_SERVO(pin)) {
  197.      pinConfig[pin] = SERVO;
  198.      if (!servos[PIN_TO_SERVO(pin)].attached()) {
  199.          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
  200.      }
  201.    }
  202.    break;
  203.  case I2C:
  204.    if (IS_PIN_I2C(pin)) {
  205.      // mark the pin as i2c
  206.      // the user must call I2C_CONFIG to enable I2C for a device
  207.      pinConfig[pin] = I2C;
  208.    }
  209.    break;
  210.  default:
  211.    Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
  212.  }
  213.  // TODO: save status to EEPROM here, if changed
  214. }
  215.  
  216. void analogWriteCallback(byte pin, int value)
  217. {
  218.  if (pin < TOTAL_PINS) {
  219.    switch(pinConfig[pin]) {
  220.    case SERVO:
  221.      if (IS_PIN_SERVO(pin))
  222.        servos[PIN_TO_SERVO(pin)].write(value);
  223.        pinState[pin] = value;
  224.      break;
  225.    case PWM:
  226.      if (IS_PIN_PWM(pin))
  227.        analogWrite(PIN_TO_PWM(pin), value);
  228.        pinState[pin] = value;
  229.      break;
  230.    }
  231.  }
  232. }
  233.  
  234. void digitalWriteCallback(byte port, int value)
  235. {
  236.  byte pin, lastPin, mask=1, pinWriteMask=0;
  237.  
  238.  if (port < TOTAL_PORTS) {
  239.    // create a mask of the pins on this port that are writable.
  240.    lastPin = port*8+8;
  241.    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
  242.    for (pin=port*8; pin < lastPin; pin++) {
  243.      // do not disturb non-digital pins (eg, Rx & Tx)
  244.      if (IS_PIN_DIGITAL(pin)) {
  245.        // only write to OUTPUT and INPUT (enables pullup)
  246.        // do not touch pins in PWM, ANALOG, SERVO or other modes
  247.        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
  248.          pinWriteMask |= mask;
  249.          pinState[pin] = ((byte)value & mask) ? 1 : 0;
  250.        }
  251.      }
  252.      mask = mask << 1;
  253.    }
  254.    writePort(port, (byte)value, pinWriteMask);
  255.  }
  256. }
  257.  
  258.  
  259. // -----------------------------------------------------------------------------
  260. /* sets bits in a bit array (int) to toggle the reporting of the analogIns
  261. */
  262. //void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
  263. //}
  264. void reportAnalogCallback(byte analogPin, int value)
  265. {
  266.  if (analogPin < TOTAL_ANALOG_PINS) {
  267.    if(value == 0) {
  268.      analogInputsToReport = analogInputsToReport &~ (1 << analogPin);
  269.    } else {
  270.      analogInputsToReport = analogInputsToReport | (1 << analogPin);
  271.    }
  272.  }
  273.  // TODO: save status to EEPROM here, if changed
  274. }
  275.  
  276. void reportDigitalCallback(byte port, int value)
  277. {
  278.  if (port < TOTAL_PORTS) {
  279.    reportPINs[port] = (byte)value;
  280.  }
  281.  // do not disable analog reporting on these 8 pins, to allow some
  282.  // pins used for digital, others analog.  Instead, allow both types
  283.  // of reporting to be enabled, but check if the pin is configured
  284.  // as analog when sampling the analog inputs.  Likewise, while
  285.  // scanning digital pins, portConfigInputs will mask off values from any
  286.  // pins configured as analog
  287. }
  288.  
  289. /*==============================================================================
  290. * SYSEX-BASED commands
  291. *============================================================================*/
  292.  
  293. void sysexCallback(byte command, byte argc, byte *argv)
  294. {
  295.  byte mode;
  296.  byte slaveAddress;
  297.  byte slaveRegister;
  298.  byte data;
  299.  unsigned int delayTime;
  300.  
  301.  switch(command) {
  302.  case I2C_REQUEST:
  303.    mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
  304.    if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
  305.      Firmata.sendString("10-bit addressing mode is not yet supported");
  306.      return;
  307.    }
  308.    else {
  309.      slaveAddress = argv[0];
  310.    }
  311.  
  312.    switch(mode) {
  313.    case I2C_WRITE:
  314.      Wire.beginTransmission(slaveAddress);
  315.      for (byte i = 2; i < argc; i += 2) {
  316.        data = argv[i] + (argv[i + 1] << 7);
  317.        #if ARDUINO >= 100
  318.        Wire.write(data);
  319.        #else
  320.        Wire.send(data);
  321.        #endif
  322.      }
  323.      Wire.endTransmission();
  324.      delayMicroseconds(70);
  325.      break;
  326.    case I2C_READ:
  327.      if (argc == 6) {
  328.        // a slave register is specified
  329.        slaveRegister = argv[2] + (argv[3] << 7);
  330.        data = argv[4] + (argv[5] << 7);  // bytes to read
  331.        readAndReportData(slaveAddress, (int)slaveRegister, data);
  332.      }
  333.      else {
  334.        // a slave register is NOT specified
  335.        data = argv[2] + (argv[3] << 7);  // bytes to read
  336.        readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data);
  337.      }
  338.      break;
  339.    case I2C_READ_CONTINUOUSLY:
  340.      if ((queryIndex + 1) >= MAX_QUERIES) {
  341.        // too many queries, just ignore
  342.        Firmata.sendString("too many queries");
  343.        break;
  344.      }
  345.      queryIndex++;
  346.      query[queryIndex].addr = slaveAddress;
  347.      query[queryIndex].reg = argv[2] + (argv[3] << 7);
  348.      query[queryIndex].bytes = argv[4] + (argv[5] << 7);
  349.      break;
  350.    case I2C_STOP_READING:
  351.  byte queryIndexToSkip;      
  352.      // if read continuous mode is enabled for only 1 i2c device, disable
  353.      // read continuous reporting for that device
  354.      if (queryIndex <= 0) {
  355.        queryIndex = -1;        
  356.      } else {
  357.        // if read continuous mode is enabled for multiple devices,
  358.        // determine which device to stop reading and remove it's data from
  359.        // the array, shifiting other array data to fill the space
  360.        for (byte i = 0; i < queryIndex + 1; i++) {
  361.          if (query[i].addr = slaveAddress) {
  362.            queryIndexToSkip = i;
  363.            break;
  364.          }
  365.        }
  366.        
  367.        for (byte i = queryIndexToSkip; i<queryIndex + 1; i++) {
  368.          if (i < MAX_QUERIES) {
  369.            query[i].addr = query[i+1].addr;
  370.            query[i].reg = query[i+1].addr;
  371.            query[i].bytes = query[i+1].bytes;
  372.          }
  373.        }
  374.        queryIndex--;
  375.      }
  376.      break;
  377.    default:
  378.      break;
  379.    }
  380.    break;
  381.  case I2C_CONFIG:
  382.    delayTime = (argv[0] + (argv[1] << 7));
  383.  
  384.    if(delayTime > 0) {
  385.      i2cReadDelayTime = delayTime;
  386.    }
  387.  
  388.    if (!isI2CEnabled) {
  389.      enableI2CPins();
  390.    }
  391.    
  392.    break;
  393.  case SERVO_CONFIG:
  394.    if(argc > 4) {
  395.      // these vars are here for clarity, they'll optimized away by the compiler
  396.      byte pin = argv[0];
  397.      int minPulse = argv[1] + (argv[2] << 7);
  398.      int maxPulse = argv[3] + (argv[4] << 7);
  399.  
  400.      if (IS_PIN_SERVO(pin)) {
  401.        if (servos[PIN_TO_SERVO(pin)].attached())
  402.          servos[PIN_TO_SERVO(pin)].detach();
  403.        servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
  404.        setPinModeCallback(pin, SERVO);
  405.      }
  406.    }
  407.    break;
  408.  case SAMPLING_INTERVAL:
  409.    if (argc > 1) {
  410.      samplingInterval = argv[0] + (argv[1] << 7);
  411.      if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
  412.        samplingInterval = MINIMUM_SAMPLING_INTERVAL;
  413.      }      
  414.    } else {
  415.      //Firmata.sendString("Not enough data");
  416.    }
  417.    break;
  418.  case EXTENDED_ANALOG:
  419.    if (argc > 1) {
  420.      int val = argv[1];
  421.      if (argc > 2) val |= (argv[2] << 7);
  422.      if (argc > 3) val |= (argv[3] << 14);
  423.      analogWriteCallback(argv[0], val);
  424.    }
  425.    break;
  426.  case CAPABILITY_QUERY:
  427.    Serial.write(START_SYSEX);
  428.    Serial.write(CAPABILITY_RESPONSE);
  429.    for (byte pin=0; pin < TOTAL_PINS; pin++) {
  430.      if (IS_PIN_DIGITAL(pin)) {
  431.        Serial.write((byte)INPUT);
  432.        Serial.write(1);
  433.        Serial.write((byte)OUTPUT);
  434.        Serial.write(1);
  435.      }
  436.      if (IS_PIN_ANALOG(pin)) {
  437.        Serial.write(ANALOG);
  438.        Serial.write(10);
  439.      }
  440.      if (IS_PIN_PWM(pin)) {
  441.        Serial.write(PWM);
  442.        Serial.write(8);
  443.      }
  444.      if (IS_PIN_SERVO(pin)) {
  445.        Serial.write(SERVO);
  446.        Serial.write(14);
  447.      }
  448.      if (IS_PIN_I2C(pin)) {
  449.        Serial.write(I2C);
  450.        Serial.write(1);  // to do: determine appropriate value
  451.      }
  452.      Serial.write(127);
  453.    }
  454.    Serial.write(END_SYSEX);
  455.    break;
  456.  case PIN_STATE_QUERY:
  457.    if (argc > 0) {
  458.      byte pin=argv[0];
  459.      Serial.write(START_SYSEX);
  460.      Serial.write(PIN_STATE_RESPONSE);
  461.      Serial.write(pin);
  462.      if (pin < TOTAL_PINS) {
  463.        Serial.write((byte)pinConfig[pin]);
  464. Serial.write((byte)pinState[pin] & 0x7F);
  465. if (pinState[pin] & 0xFF80) Serial.write((byte)(pinState[pin] >> 7) & 0x7F);
  466. if (pinState[pin] & 0xC000) Serial.write((byte)(pinState[pin] >> 14) & 0x7F);
  467.      }
  468.      Serial.write(END_SYSEX);
  469.    }
  470.    break;
  471.  case ANALOG_MAPPING_QUERY:
  472.    Serial.write(START_SYSEX);
  473.    Serial.write(ANALOG_MAPPING_RESPONSE);
  474.    for (byte pin=0; pin < TOTAL_PINS; pin++) {
  475.      Serial.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
  476.    }
  477.    Serial.write(END_SYSEX);
  478.    break;
  479.  }
  480. }
  481.  
  482. void enableI2CPins()
  483. {
  484.  byte i;
  485.  // is there a faster way to do this? would probaby require importing
  486.  // Arduino.h to get SCL and SDA pins
  487.  for (i=0; i < TOTAL_PINS; i++) {
  488.    if(IS_PIN_I2C(i)) {
  489.      // mark pins as i2c so they are ignore in non i2c data requests
  490.      setPinModeCallback(i, I2C);
  491.    }
  492.  }
  493.  
  494.  isI2CEnabled = true;
  495.  
  496.  // is there enough time before the first I2C request to call this here?
  497.  Wire.begin();
  498. }
  499.  
  500. /* disable the i2c pins so they can be used for other functions */
  501. void disableI2CPins() {
  502.    isI2CEnabled = false;
  503.    // disable read continuous mode for all devices
  504.    queryIndex = -1;
  505.    // uncomment the following if or when the end() method is added to Wire library
  506.    // Wire.end();
  507. }
  508.  
  509. /*==============================================================================
  510. * SETUP()
  511. *============================================================================*/
  512.  
  513. void systemResetCallback()
  514. {
  515.  // initialize a defalt state
  516.  // TODO: option to load config from EEPROM instead of default
  517.  if (isI2CEnabled) {
  518.   disableI2CPins();
  519.  }
  520.  for (byte i=0; i < TOTAL_PORTS; i++) {
  521.    reportPINs[i] = false;      // by default, reporting off
  522.    portConfigInputs[i] = 0; // until activated
  523.    previousPINs[i] = 0;
  524.  }
  525.  // pins with analog capability default to analog input
  526.  // otherwise, pins default to digital output
  527.  for (byte i=0; i < TOTAL_PINS; i++) {
  528.    if (IS_PIN_ANALOG(i)) {
  529.      // turns off pullup, configures everything
  530.      setPinModeCallback(i, ANALOG);
  531.    } else {
  532.      // sets the output to 0, configures portConfigInputs
  533.      setPinModeCallback(i, OUTPUT);
  534.    }
  535.  }
  536.  // by default, do not report any analog inputs
  537.  analogInputsToReport = 0;
  538.  
  539.  /* send digital inputs to set the initial state on the host computer,
  540.   * since once in the loop(), this firmware will only send on change */
  541.  /*
  542.  TODO: this can never execute, since no pins default to digital input
  543.        but it will be needed when/if we support EEPROM stored config
  544.  for (byte i=0; i < TOTAL_PORTS; i++) {
  545.    outputPort(i, readPort(i, portConfigInputs[i]), true);
  546.  }
  547.  */
  548. }
  549.  
  550. void setup()
  551. {
  552.  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
  553.  
  554.  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
  555.  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
  556.  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
  557.  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
  558.  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
  559.  Firmata.attach(START_SYSEX, sysexCallback);
  560.  Firmata.attach(SYSTEM_RESET, systemResetCallback);
  561.  
  562.  Firmata.begin(57600);
  563.  systemResetCallback();  // reset to default config
  564. }
  565.  
  566. /*==============================================================================
  567. * LOOP()
  568. *============================================================================*/
  569. void loop()
  570. {
  571.  byte pin, analogPin;
  572.  
  573.  /* DIGITALREAD - as fast as possible, check for changes and output them to the
  574.   * FTDI buffer using Serial.print()  */
  575.  checkDigitalInputs();  
  576.  
  577.  /* SERIALREAD - processing incoming messagse as soon as possible, while still
  578.   * checking digital inputs.  */
  579.  while(Firmata.available())
  580.    Firmata.processInput();
  581.  
  582.  /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
  583.   * 60 bytes. use a timer to sending an event character every 4 ms to
  584.   * trigger the buffer to dump. */
  585.  
  586.  currentMillis = millis();
  587.  if (currentMillis - previousMillis > samplingInterval) {
  588.    previousMillis += samplingInterval;
  589.    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
  590.    for(pin=0; pin<TOTAL_PINS; pin++) {
  591.      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
  592.        analogPin = PIN_TO_ANALOG(pin);
  593.        if (analogInputsToReport & (1 << analogPin)) {
  594.          Firmata.sendAnalog(analogPin, analogRead(analogPin));
  595.        }
  596.      }
  597.    }
  598.    // report i2c data for all device with read continuous mode enabled
  599.    if (queryIndex > -1) {
  600.      for (byte i = 0; i < queryIndex + 1; i++) {
  601.        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
  602.      }
  603.    }
  604.  }
  605. }