Statistics
| Branch: | Revision:

root / arduino-1.0 / libraries / Firmata / examples / StandardFirmata / StandardFirmata.ino @ 58d82c77

History | View | Annotate | Download (20.7 KB)

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 clink on the following link
7
 * to open the download page in your default browser.
8
 *
9
 * http://firmata.org/wiki/Download
10
 */
11

    
12
/*
13
  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
14
  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
15
  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
16
  Copyright (C) 2009-2011 Jeff Hoefs.  All rights reserved.
17
  
18
  This library is free software; you can redistribute it and/or
19
  modify it under the terms of the GNU Lesser General Public
20
  License as published by the Free Software Foundation; either
21
  version 2.1 of the License, or (at your option) any later version.
22
 
23
  See file LICENSE.txt for further informations on licensing terms.
24

    
25
  formatted using the GNU C formatting and indenting
26
*/
27

    
28
/* 
29
 * TODO: use Program Control to load stored profiles from EEPROM
30
 */
31

    
32
#include <Servo.h>
33
#include <Wire.h>
34
#include <Firmata.h>
35

    
36
// move the following defines to Firmata.h?
37
#define I2C_WRITE B00000000
38
#define I2C_READ B00001000
39
#define I2C_READ_CONTINUOUSLY B00010000
40
#define I2C_STOP_READING B00011000
41
#define I2C_READ_WRITE_MODE_MASK B00011000
42
#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
43

    
44
#define MAX_QUERIES 8
45
#define MINIMUM_SAMPLING_INTERVAL 10
46

    
47
#define REGISTER_NOT_SPECIFIED -1
48

    
49
/*==============================================================================
50
 * GLOBAL VARIABLES
51
 *============================================================================*/
52

    
53
/* analog inputs */
54
int analogInputsToReport = 0; // bitwise array to store pin reporting
55

    
56
/* digital input ports */
57
byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
58
byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
59

    
60
/* pins configuration */
61
byte pinConfig[TOTAL_PINS];         // configuration of every pin
62
byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
63
int pinState[TOTAL_PINS];           // any value that has been written
64

    
65
/* timer variables */
66
unsigned long currentMillis;        // store the current value from millis()
67
unsigned long previousMillis;       // for comparison with currentMillis
68
int samplingInterval = 19;          // how often to run the main loop (in ms)
69

    
70
/* i2c data */
71
struct i2c_device_info {
72
  byte addr;
73
  byte reg;
74
  byte bytes;
75
};
76

    
77
/* for i2c read continuous more */
78
i2c_device_info query[MAX_QUERIES];
79

    
80
byte i2cRxData[32];
81
boolean isI2CEnabled = false;
82
signed char queryIndex = -1;
83
unsigned int i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
84

    
85
Servo servos[MAX_SERVOS];
86
/*==============================================================================
87
 * FUNCTIONS
88
 *============================================================================*/
89

    
90
void readAndReportData(byte address, int theRegister, byte numBytes) {
91
  // allow I2C requests that don't require a register read
92
  // for example, some devices using an interrupt pin to signify new data available
93
  // do not always require the register read so upon interrupt you call Wire.requestFrom()  
94
  if (theRegister != REGISTER_NOT_SPECIFIED) {
95
    Wire.beginTransmission(address);
96
    #if ARDUINO >= 100
97
    Wire.write((byte)theRegister);
98
    #else
99
    Wire.send((byte)theRegister);
100
    #endif
101
    Wire.endTransmission();
102
    delayMicroseconds(i2cReadDelayTime);  // delay is necessary for some devices such as WiiNunchuck
103
  } else {
104
    theRegister = 0;  // fill the register with a dummy value
105
  }
106

    
107
  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
108

    
109
  // check to be sure correct number of bytes were returned by slave
110
  if(numBytes == Wire.available()) {
111
    i2cRxData[0] = address;
112
    i2cRxData[1] = theRegister;
113
    for (int i = 0; i < numBytes; i++) {
114
      #if ARDUINO >= 100
115
      i2cRxData[2 + i] = Wire.read();
116
      #else
117
      i2cRxData[2 + i] = Wire.receive();
118
      #endif
119
    }
120
  }
121
  else {
122
    if(numBytes > Wire.available()) {
123
      Firmata.sendString("I2C Read Error: Too many bytes received");
124
    } else {
125
      Firmata.sendString("I2C Read Error: Too few bytes received"); 
126
    }
127
  }
128

    
129
  // send slave address, register and received bytes
130
  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
131
}
132

    
133
void outputPort(byte portNumber, byte portValue, byte forceSend)
134
{
135
  // pins not configured as INPUT are cleared to zeros
136
  portValue = portValue & portConfigInputs[portNumber];
137
  // only send if the value is different than previously sent
138
  if(forceSend || previousPINs[portNumber] != portValue) {
139
    Firmata.sendDigitalPort(portNumber, portValue);
140
    previousPINs[portNumber] = portValue;
141
  }
142
}
143

    
144
/* -----------------------------------------------------------------------------
145
 * check all the active digital inputs for change of state, then add any events
146
 * to the Serial output queue using Serial.print() */
147
void checkDigitalInputs(void)
148
{
149
  /* Using non-looping code allows constants to be given to readPort().
150
   * The compiler will apply substantial optimizations if the inputs
151
   * to readPort() are compile-time constants. */
152
  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
153
  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
154
  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
155
  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
156
  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
157
  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
158
  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
159
  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
160
  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
161
  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
162
  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
163
  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
164
  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
165
  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
166
  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
167
  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
168
}
169

    
170
// -----------------------------------------------------------------------------
171
/* sets the pin mode to the correct state and sets the relevant bits in the
172
 * two bit-arrays that track Digital I/O and PWM status
173
 */
174
void setPinModeCallback(byte pin, int mode)
175
{
176
  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
177
    // disable i2c so pins can be used for other functions
178
    // the following if statements should reconfigure the pins properly
179
    disableI2CPins();
180
  }
181
  if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached()) {
182
    servos[PIN_TO_SERVO(pin)].detach();
183
  }
184
  if (IS_PIN_ANALOG(pin)) {
185
    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
186
  }
187
  if (IS_PIN_DIGITAL(pin)) {
188
    if (mode == INPUT) {
189
      portConfigInputs[pin/8] |= (1 << (pin & 7));
190
    } else {
191
      portConfigInputs[pin/8] &= ~(1 << (pin & 7));
192
    }
193
  }
194
  pinState[pin] = 0;
195
  switch(mode) {
196
  case ANALOG:
197
    if (IS_PIN_ANALOG(pin)) {
198
      if (IS_PIN_DIGITAL(pin)) {
199
        pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
200
        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
201
      }
202
      pinConfig[pin] = ANALOG;
203
    }
204
    break;
205
  case INPUT:
206
    if (IS_PIN_DIGITAL(pin)) {
207
      pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
208
      digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
209
      pinConfig[pin] = INPUT;
210
    }
211
    break;
212
  case OUTPUT:
213
    if (IS_PIN_DIGITAL(pin)) {
214
      digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
215
      pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
216
      pinConfig[pin] = OUTPUT;
217
    }
218
    break;
219
  case PWM:
220
    if (IS_PIN_PWM(pin)) {
221
      pinMode(PIN_TO_PWM(pin), OUTPUT);
222
      analogWrite(PIN_TO_PWM(pin), 0);
223
      pinConfig[pin] = PWM;
224
    }
225
    break;
226
  case SERVO:
227
    if (IS_PIN_SERVO(pin)) {
228
      pinConfig[pin] = SERVO;
229
      if (!servos[PIN_TO_SERVO(pin)].attached()) {
230
          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
231
      }
232
    }
233
    break;
234
  case I2C:
235
    if (IS_PIN_I2C(pin)) {
236
      // mark the pin as i2c
237
      // the user must call I2C_CONFIG to enable I2C for a device
238
      pinConfig[pin] = I2C;
239
    }
240
    break;
241
  default:
242
    Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
243
  }
244
  // TODO: save status to EEPROM here, if changed
245
}
246

    
247
void analogWriteCallback(byte pin, int value)
248
{
249
  if (pin < TOTAL_PINS) {
250
    switch(pinConfig[pin]) {
251
    case SERVO:
252
      if (IS_PIN_SERVO(pin))
253
        servos[PIN_TO_SERVO(pin)].write(value);
254
        pinState[pin] = value;
255
      break;
256
    case PWM:
257
      if (IS_PIN_PWM(pin))
258
        analogWrite(PIN_TO_PWM(pin), value);
259
        pinState[pin] = value;
260
      break;
261
    }
262
  }
263
}
264

    
265
void digitalWriteCallback(byte port, int value)
266
{
267
  byte pin, lastPin, mask=1, pinWriteMask=0;
268

    
269
  if (port < TOTAL_PORTS) {
270
    // create a mask of the pins on this port that are writable.
271
    lastPin = port*8+8;
272
    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
273
    for (pin=port*8; pin < lastPin; pin++) {
274
      // do not disturb non-digital pins (eg, Rx & Tx)
275
      if (IS_PIN_DIGITAL(pin)) {
276
        // only write to OUTPUT and INPUT (enables pullup)
277
        // do not touch pins in PWM, ANALOG, SERVO or other modes
278
        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
279
          pinWriteMask |= mask;
280
          pinState[pin] = ((byte)value & mask) ? 1 : 0;
281
        }
282
      }
283
      mask = mask << 1;
284
    }
285
    writePort(port, (byte)value, pinWriteMask);
286
  }
287
}
288

    
289

    
290
// -----------------------------------------------------------------------------
291
/* sets bits in a bit array (int) to toggle the reporting of the analogIns
292
 */
293
//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
294
//}
295
void reportAnalogCallback(byte analogPin, int value)
296
{
297
  if (analogPin < TOTAL_ANALOG_PINS) {
298
    if(value == 0) {
299
      analogInputsToReport = analogInputsToReport &~ (1 << analogPin);
300
    } else {
301
      analogInputsToReport = analogInputsToReport | (1 << analogPin);
302
    }
303
  }
304
  // TODO: save status to EEPROM here, if changed
305
}
306

    
307
void reportDigitalCallback(byte port, int value)
308
{
309
  if (port < TOTAL_PORTS) {
310
    reportPINs[port] = (byte)value;
311
  }
312
  // do not disable analog reporting on these 8 pins, to allow some
313
  // pins used for digital, others analog.  Instead, allow both types
314
  // of reporting to be enabled, but check if the pin is configured
315
  // as analog when sampling the analog inputs.  Likewise, while
316
  // scanning digital pins, portConfigInputs will mask off values from any
317
  // pins configured as analog
318
}
319

    
320
/*==============================================================================
321
 * SYSEX-BASED commands
322
 *============================================================================*/
323

    
324
void sysexCallback(byte command, byte argc, byte *argv)
325
{
326
  byte mode;
327
  byte slaveAddress;
328
  byte slaveRegister;
329
  byte data;
330
  unsigned int delayTime; 
331
  
332
  switch(command) {
333
  case I2C_REQUEST:
334
    mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
335
    if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
336
      Firmata.sendString("10-bit addressing mode is not yet supported");
337
      return;
338
    }
339
    else {
340
      slaveAddress = argv[0];
341
    }
342

    
343
    switch(mode) {
344
    case I2C_WRITE:
345
      Wire.beginTransmission(slaveAddress);
346
      for (byte i = 2; i < argc; i += 2) {
347
        data = argv[i] + (argv[i + 1] << 7);
348
        #if ARDUINO >= 100
349
        Wire.write(data);
350
        #else
351
        Wire.send(data);
352
        #endif
353
      }
354
      Wire.endTransmission();
355
      delayMicroseconds(70);
356
      break;
357
    case I2C_READ:
358
      if (argc == 6) {
359
        // a slave register is specified
360
        slaveRegister = argv[2] + (argv[3] << 7);
361
        data = argv[4] + (argv[5] << 7);  // bytes to read
362
        readAndReportData(slaveAddress, (int)slaveRegister, data);
363
      }
364
      else {
365
        // a slave register is NOT specified
366
        data = argv[2] + (argv[3] << 7);  // bytes to read
367
        readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data);
368
      }
369
      break;
370
    case I2C_READ_CONTINUOUSLY:
371
      if ((queryIndex + 1) >= MAX_QUERIES) {
372
        // too many queries, just ignore
373
        Firmata.sendString("too many queries");
374
        break;
375
      }
376
      queryIndex++;
377
      query[queryIndex].addr = slaveAddress;
378
      query[queryIndex].reg = argv[2] + (argv[3] << 7);
379
      query[queryIndex].bytes = argv[4] + (argv[5] << 7);
380
      break;
381
    case I2C_STOP_READING:
382
	  byte queryIndexToSkip;      
383
      // if read continuous mode is enabled for only 1 i2c device, disable
384
      // read continuous reporting for that device
385
      if (queryIndex <= 0) {
386
        queryIndex = -1;        
387
      } else {
388
        // if read continuous mode is enabled for multiple devices,
389
        // determine which device to stop reading and remove it's data from
390
        // the array, shifiting other array data to fill the space
391
        for (byte i = 0; i < queryIndex + 1; i++) {
392
          if (query[i].addr = slaveAddress) {
393
            queryIndexToSkip = i;
394
            break;
395
          }
396
        }
397
        
398
        for (byte i = queryIndexToSkip; i<queryIndex + 1; i++) {
399
          if (i < MAX_QUERIES) {
400
            query[i].addr = query[i+1].addr;
401
            query[i].reg = query[i+1].addr;
402
            query[i].bytes = query[i+1].bytes; 
403
          }
404
        }
405
        queryIndex--;
406
      }
407
      break;
408
    default:
409
      break;
410
    }
411
    break;
412
  case I2C_CONFIG:
413
    delayTime = (argv[0] + (argv[1] << 7));
414

    
415
    if(delayTime > 0) {
416
      i2cReadDelayTime = delayTime;
417
    }
418

    
419
    if (!isI2CEnabled) {
420
      enableI2CPins();
421
    }
422
    
423
    break;
424
  case SERVO_CONFIG:
425
    if(argc > 4) {
426
      // these vars are here for clarity, they'll optimized away by the compiler
427
      byte pin = argv[0];
428
      int minPulse = argv[1] + (argv[2] << 7);
429
      int maxPulse = argv[3] + (argv[4] << 7);
430

    
431
      if (IS_PIN_SERVO(pin)) {
432
        if (servos[PIN_TO_SERVO(pin)].attached())
433
          servos[PIN_TO_SERVO(pin)].detach();
434
        servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
435
        setPinModeCallback(pin, SERVO);
436
      }
437
    }
438
    break;
439
  case SAMPLING_INTERVAL:
440
    if (argc > 1) {
441
      samplingInterval = argv[0] + (argv[1] << 7);
442
      if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
443
        samplingInterval = MINIMUM_SAMPLING_INTERVAL;
444
      }      
445
    } else {
446
      //Firmata.sendString("Not enough data");
447
    }
448
    break;
449
  case EXTENDED_ANALOG:
450
    if (argc > 1) {
451
      int val = argv[1];
452
      if (argc > 2) val |= (argv[2] << 7);
453
      if (argc > 3) val |= (argv[3] << 14);
454
      analogWriteCallback(argv[0], val);
455
    }
456
    break;
457
  case CAPABILITY_QUERY:
458
    Serial.write(START_SYSEX);
459
    Serial.write(CAPABILITY_RESPONSE);
460
    for (byte pin=0; pin < TOTAL_PINS; pin++) {
461
      if (IS_PIN_DIGITAL(pin)) {
462
        Serial.write((byte)INPUT);
463
        Serial.write(1);
464
        Serial.write((byte)OUTPUT);
465
        Serial.write(1);
466
      }
467
      if (IS_PIN_ANALOG(pin)) {
468
        Serial.write(ANALOG);
469
        Serial.write(10);
470
      }
471
      if (IS_PIN_PWM(pin)) {
472
        Serial.write(PWM);
473
        Serial.write(8);
474
      }
475
      if (IS_PIN_SERVO(pin)) {
476
        Serial.write(SERVO);
477
        Serial.write(14);
478
      }
479
      if (IS_PIN_I2C(pin)) {
480
        Serial.write(I2C);
481
        Serial.write(1);  // to do: determine appropriate value 
482
      }
483
      Serial.write(127);
484
    }
485
    Serial.write(END_SYSEX);
486
    break;
487
  case PIN_STATE_QUERY:
488
    if (argc > 0) {
489
      byte pin=argv[0];
490
      Serial.write(START_SYSEX);
491
      Serial.write(PIN_STATE_RESPONSE);
492
      Serial.write(pin);
493
      if (pin < TOTAL_PINS) {
494
        Serial.write((byte)pinConfig[pin]);
495
	Serial.write((byte)pinState[pin] & 0x7F);
496
	if (pinState[pin] & 0xFF80) Serial.write((byte)(pinState[pin] >> 7) & 0x7F);
497
	if (pinState[pin] & 0xC000) Serial.write((byte)(pinState[pin] >> 14) & 0x7F);
498
      }
499
      Serial.write(END_SYSEX);
500
    }
501
    break;
502
  case ANALOG_MAPPING_QUERY:
503
    Serial.write(START_SYSEX);
504
    Serial.write(ANALOG_MAPPING_RESPONSE);
505
    for (byte pin=0; pin < TOTAL_PINS; pin++) {
506
      Serial.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
507
    }
508
    Serial.write(END_SYSEX);
509
    break;
510
  }
511
}
512

    
513
void enableI2CPins()
514
{
515
  byte i;
516
  // is there a faster way to do this? would probaby require importing 
517
  // Arduino.h to get SCL and SDA pins
518
  for (i=0; i < TOTAL_PINS; i++) {
519
    if(IS_PIN_I2C(i)) {
520
      // mark pins as i2c so they are ignore in non i2c data requests
521
      setPinModeCallback(i, I2C);
522
    } 
523
  }
524
   
525
  isI2CEnabled = true; 
526
  
527
  // is there enough time before the first I2C request to call this here?
528
  Wire.begin();
529
}
530

    
531
/* disable the i2c pins so they can be used for other functions */
532
void disableI2CPins() {
533
    isI2CEnabled = false;
534
    // disable read continuous mode for all devices
535
    queryIndex = -1;
536
    // uncomment the following if or when the end() method is added to Wire library
537
    // Wire.end();
538
}
539

    
540
/*==============================================================================
541
 * SETUP()
542
 *============================================================================*/
543

    
544
void systemResetCallback()
545
{
546
  // initialize a defalt state
547
  // TODO: option to load config from EEPROM instead of default
548
  if (isI2CEnabled) {
549
  	disableI2CPins();
550
  }
551
  for (byte i=0; i < TOTAL_PORTS; i++) {
552
    reportPINs[i] = false;      // by default, reporting off
553
    portConfigInputs[i] = 0;	// until activated
554
    previousPINs[i] = 0;
555
  }
556
  // pins with analog capability default to analog input
557
  // otherwise, pins default to digital output
558
  for (byte i=0; i < TOTAL_PINS; i++) {
559
    if (IS_PIN_ANALOG(i)) {
560
      // turns off pullup, configures everything
561
      setPinModeCallback(i, ANALOG);
562
    } else {
563
      // sets the output to 0, configures portConfigInputs
564
      setPinModeCallback(i, OUTPUT);
565
    }
566
  }
567
  // by default, do not report any analog inputs
568
  analogInputsToReport = 0;
569

    
570
  /* send digital inputs to set the initial state on the host computer,
571
   * since once in the loop(), this firmware will only send on change */
572
  /*
573
  TODO: this can never execute, since no pins default to digital input
574
        but it will be needed when/if we support EEPROM stored config
575
  for (byte i=0; i < TOTAL_PORTS; i++) {
576
    outputPort(i, readPort(i, portConfigInputs[i]), true);
577
  }
578
  */
579
}
580

    
581
void setup() 
582
{
583
  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
584

    
585
  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
586
  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
587
  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
588
  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
589
  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
590
  Firmata.attach(START_SYSEX, sysexCallback);
591
  Firmata.attach(SYSTEM_RESET, systemResetCallback);
592

    
593
  Firmata.begin(57600);
594
  systemResetCallback();  // reset to default config
595
}
596

    
597
/*==============================================================================
598
 * LOOP()
599
 *============================================================================*/
600
void loop() 
601
{
602
  byte pin, analogPin;
603

    
604
  /* DIGITALREAD - as fast as possible, check for changes and output them to the
605
   * FTDI buffer using Serial.print()  */
606
  checkDigitalInputs();  
607

    
608
  /* SERIALREAD - processing incoming messagse as soon as possible, while still
609
   * checking digital inputs.  */
610
  while(Firmata.available())
611
    Firmata.processInput();
612

    
613
  /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
614
   * 60 bytes. use a timer to sending an event character every 4 ms to
615
   * trigger the buffer to dump. */
616

    
617
  currentMillis = millis();
618
  if (currentMillis - previousMillis > samplingInterval) {
619
    previousMillis += samplingInterval;
620
    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
621
    for(pin=0; pin<TOTAL_PINS; pin++) {
622
      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
623
        analogPin = PIN_TO_ANALOG(pin);
624
        if (analogInputsToReport & (1 << analogPin)) {
625
          Firmata.sendAnalog(analogPin, analogRead(analogPin));
626
        }
627
      }
628
    }
629
    // report i2c data for all device with read continuous mode enabled
630
    if (queryIndex > -1) {
631
      for (byte i = 0; i < queryIndex + 1; i++) {
632
        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
633
      }
634
    }
635
  }
636
}