Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

Untitled

By: a guest on Jan 18th, 2013  |  syntax: None  |  size: 21.29 KB  |  views: 51  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  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. }