Moving the processing-5503 branch (used for Arduino 0017) into the trunk.

33 files changed, 2136 insertions, 739 deletions

diff --git a/boards.txt b/boards.txt
index 24ddfc0..4ae4ac9 100644
--- a/boards.txt
+++ b/boards.txt
@@ -1,6 +1,6 @@
 ##############################################################
 
-atmega328.name=Arduino Duemilanove w/ ATmega328
+atmega328.name=Arduino Duemilanove or Nano w/ ATmega328
 
 atmega328.upload.protocol=stk500
 atmega328.upload.maximum_size=30720
@@ -20,7 +20,7 @@ atmega328.build.core=arduino
 
 ##############################################################
 
-diecimila.name=Arduino Diecimila or Duemilanove w/ ATmega168
+diecimila.name=Arduino Diecimila, Duemilanove, or Nano w/ ATmega168
 
 diecimila.upload.protocol=stk500
 diecimila.upload.maximum_size=14336
@@ -80,26 +80,6 @@ mini.build.core=arduino
 
 ##############################################################
 
-nano.name=Arduino Nano
-
-nano.upload.protocol=stk500
-nano.upload.maximum_size=14336
-nano.upload.speed=19200
-
-nano.bootloader.low_fuses=0xff
-nano.bootloader.high_fuses=0xdd
-nano.bootloader.extended_fuses=0x00
-nano.bootloader.path=atmega
-nano.bootloader.file=ATmegaBOOT_168_diecimila.hex
-nano.bootloader.unlock_bits=0x3F
-nano.bootloader.lock_bits=0x0F
-
-nano.build.mcu=atmega168
-nano.build.f_cpu=16000000L
-nano.build.core=arduino
-
-##############################################################
-
 bt.name=Arduino BT
 
 bt.upload.protocol=stk500
diff --git a/cores/arduino/HardwareSerial.cpp b/cores/arduino/HardwareSerial.cpp
index f8a959a..1af6a66 100755
--- a/cores/arduino/HardwareSerial.cpp
+++ b/cores/arduino/HardwareSerial.cpp
@@ -49,41 +49,41 @@ ring_buffer rx_buffer3 = { { 0 }, 0, 0 };
 
 inline void store_char(unsigned char c, ring_buffer *rx_buffer)
 {
-	int i = (rx_buffer->head + 1) % RX_BUFFER_SIZE;
-
-	// if we should be storing the received character into the location
-	// just before the tail (meaning that the head would advance to the
-	// current location of the tail), we're about to overflow the buffer
-	// and so we don't write the character or advance the head.
-	if (i != rx_buffer->tail) {
-		rx_buffer->buffer[rx_buffer->head] = c;
-		rx_buffer->head = i;
-	}
+  int i = (rx_buffer->head + 1) % RX_BUFFER_SIZE;
+
+  // if we should be storing the received character into the location
+  // just before the tail (meaning that the head would advance to the
+  // current location of the tail), we're about to overflow the buffer
+  // and so we don't write the character or advance the head.
+  if (i != rx_buffer->tail) {
+    rx_buffer->buffer[rx_buffer->head] = c;
+    rx_buffer->head = i;
+  }
 }
 
 #if defined(__AVR_ATmega1280__)
 
 SIGNAL(SIG_USART0_RECV)
 {
-	unsigned char c = UDR0;
+  unsigned char c = UDR0;
   store_char(c, &rx_buffer);
 }
 
 SIGNAL(SIG_USART1_RECV)
 {
-	unsigned char c = UDR1;
+  unsigned char c = UDR1;
   store_char(c, &rx_buffer1);
 }
 
 SIGNAL(SIG_USART2_RECV)
 {
-	unsigned char c = UDR2;
+  unsigned char c = UDR2;
   store_char(c, &rx_buffer2);
 }
 
 SIGNAL(SIG_USART3_RECV)
 {
-	unsigned char c = UDR3;
+  unsigned char c = UDR3;
   store_char(c, &rx_buffer3);
 }
 
@@ -96,9 +96,9 @@ SIGNAL(USART_RX_vect)
 #endif
 {
 #if defined(__AVR_ATmega8__)
-	unsigned char c = UDR;
+  unsigned char c = UDR;
 #else
-	unsigned char c = UDR0;
+  unsigned char c = UDR0;
 #endif
   store_char(c, &rx_buffer);
 }
@@ -111,7 +111,7 @@ HardwareSerial::HardwareSerial(ring_buffer *rx_buffer,
   volatile uint8_t *ubrrh, volatile uint8_t *ubrrl,
   volatile uint8_t *ucsra, volatile uint8_t *ucsrb,
   volatile uint8_t *udr,
-  uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udre)
+  uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udre, uint8_t u2x)
 {
   _rx_buffer = rx_buffer;
   _ubrrh = ubrrh;
@@ -123,14 +123,43 @@ HardwareSerial::HardwareSerial(ring_buffer *rx_buffer,
   _txen = txen;
   _rxcie = rxcie;
   _udre = udre;
+  _u2x = u2x;
 }
 
 // Public Methods //////////////////////////////////////////////////////////////
 
-void HardwareSerial::begin(long speed)
+void HardwareSerial::begin(long baud)
 {
-	*_ubrrh = ((F_CPU / 16 + speed / 2) / speed - 1) >> 8;
-	*_ubrrl = ((F_CPU / 16 + speed / 2) / speed - 1);
+  uint16_t baud_setting;
+  bool use_u2x;
+
+  // U2X mode is needed for baud rates higher than (CPU Hz / 16)
+  if (baud > F_CPU / 16) {
+    use_u2x = true;
+  } else {
+    // figure out if U2X mode would allow for a better connection
+    
+    // calculate the percent difference between the baud-rate specified and
+    // the real baud rate for both U2X and non-U2X mode (0-255 error percent)
+    uint8_t nonu2x_baud_error = abs((int)(255-((F_CPU/(16*(((F_CPU/8/baud-1)/2)+1))*255)/baud)));
+    uint8_t u2x_baud_error = abs((int)(255-((F_CPU/(8*(((F_CPU/4/baud-1)/2)+1))*255)/baud)));
+    
+    // prefer non-U2X mode because it handles clock skew better
+    use_u2x = (nonu2x_baud_error > u2x_baud_error);
+  }
+  
+  if (use_u2x) {
+    *_ucsra = 1 << _u2x;
+    baud_setting = (F_CPU / 4 / baud - 1) / 2;
+  } else {
+    *_ucsra = 0;
+    baud_setting = (F_CPU / 8 / baud - 1) / 2;
+  }
+
+  // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register)
+  *_ubrrh = baud_setting >> 8;
+  *_ubrrl = baud_setting;
+
   sbi(*_ucsrb, _rxen);
   sbi(*_ucsrb, _txen);
   sbi(*_ucsrb, _rxcie);
@@ -138,54 +167,53 @@ void HardwareSerial::begin(long speed)
 
 uint8_t HardwareSerial::available(void)
 {
-	return (RX_BUFFER_SIZE + _rx_buffer->head - _rx_buffer->tail) % RX_BUFFER_SIZE;
+  return (RX_BUFFER_SIZE + _rx_buffer->head - _rx_buffer->tail) % RX_BUFFER_SIZE;
 }
 
 int HardwareSerial::read(void)
 {
-	// if the head isn't ahead of the tail, we don't have any characters
-	if (_rx_buffer->head == _rx_buffer->tail) {
-		return -1;
-	} else {
-		unsigned char c = _rx_buffer->buffer[_rx_buffer->tail];
-		_rx_buffer->tail = (_rx_buffer->tail + 1) % RX_BUFFER_SIZE;
-		return c;
-	}
+  // if the head isn't ahead of the tail, we don't have any characters
+  if (_rx_buffer->head == _rx_buffer->tail) {
+    return -1;
+  } else {
+    unsigned char c = _rx_buffer->buffer[_rx_buffer->tail];
+    _rx_buffer->tail = (_rx_buffer->tail + 1) % RX_BUFFER_SIZE;
+    return c;
+  }
 }
 
 void HardwareSerial::flush()
 {
-	// don't reverse this or there may be problems if the RX interrupt
-	// occurs after reading the value of rx_buffer_head but before writing
-	// the value to rx_buffer_tail; the previous value of rx_buffer_head
-	// may be written to rx_buffer_tail, making it appear as if the buffer
-	// don't reverse this or there may be problems if the RX interrupt
-	// occurs after reading the value of rx_buffer_head but before writing
-	// the value to rx_buffer_tail; the previous value of rx_buffer_head
-	// may be written to rx_buffer_tail, making it appear as if the buffer
-	// were full, not empty.
-	_rx_buffer->head = _rx_buffer->tail;
+  // don't reverse this or there may be problems if the RX interrupt
+  // occurs after reading the value of rx_buffer_head but before writing
+  // the value to rx_buffer_tail; the previous value of rx_buffer_head
+  // may be written to rx_buffer_tail, making it appear as if the buffer
+  // don't reverse this or there may be problems if the RX interrupt
+  // occurs after reading the value of rx_buffer_head but before writing
+  // the value to rx_buffer_tail; the previous value of rx_buffer_head
+  // may be written to rx_buffer_tail, making it appear as if the buffer
+  // were full, not empty.
+  _rx_buffer->head = _rx_buffer->tail;
 }
 
 void HardwareSerial::write(uint8_t c)
 {
-	while (!((*_ucsra) & (1 << _udre)))
-		;
+  while (!((*_ucsra) & (1 << _udre)))
+    ;
 
-	*_udr = c;
+  *_udr = c;
 }
 
 // Preinstantiate Objects //////////////////////////////////////////////////////
 
 #if defined(__AVR_ATmega8__)
-HardwareSerial Serial(&rx_buffer, &UBRRH, &UBRRL, &UCSRA, &UCSRB, &UDR, RXEN, TXEN, RXCIE, UDRE);
+HardwareSerial Serial(&rx_buffer, &UBRRH, &UBRRL, &UCSRA, &UCSRB, &UDR, RXEN, TXEN, RXCIE, UDRE, U2X);
 #else
-HardwareSerial Serial(&rx_buffer, &UBRR0H, &UBRR0L, &UCSR0A, &UCSR0B, &UDR0, RXEN0, TXEN0, RXCIE0, UDRE0);
+HardwareSerial Serial(&rx_buffer, &UBRR0H, &UBRR0L, &UCSR0A, &UCSR0B, &UDR0, RXEN0, TXEN0, RXCIE0, UDRE0, U2X0);
 #endif
 
 #if defined(__AVR_ATmega1280__)
-HardwareSerial Serial1(&rx_buffer1, &UBRR1H, &UBRR1L, &UCSR1A, &UCSR1B, &UDR1, RXEN1, TXEN1, RXCIE1, UDRE1);
-HardwareSerial Serial2(&rx_buffer2, &UBRR2H, &UBRR2L, &UCSR2A, &UCSR2B, &UDR2, RXEN2, TXEN2, RXCIE2, UDRE2);
-HardwareSerial Serial3(&rx_buffer3, &UBRR3H, &UBRR3L, &UCSR3A, &UCSR3B, &UDR3, RXEN3, TXEN3, RXCIE3, UDRE3);
+HardwareSerial Serial1(&rx_buffer1, &UBRR1H, &UBRR1L, &UCSR1A, &UCSR1B, &UDR1, RXEN1, TXEN1, RXCIE1, UDRE1, U2X1);
+HardwareSerial Serial2(&rx_buffer2, &UBRR2H, &UBRR2L, &UCSR2A, &UCSR2B, &UDR2, RXEN2, TXEN2, RXCIE2, UDRE2, U2X2);
+HardwareSerial Serial3(&rx_buffer3, &UBRR3H, &UBRR3L, &UCSR3A, &UCSR3B, &UDR3, RXEN3, TXEN3, RXCIE3, UDRE3, U2X3);
 #endif
-
diff --git a/cores/arduino/HardwareSerial.h b/cores/arduino/HardwareSerial.h
index 8610f5a..f975ccd 100755
--- a/cores/arduino/HardwareSerial.h
+++ b/cores/arduino/HardwareSerial.h
@@ -39,12 +39,13 @@ class HardwareSerial : public Print
     uint8_t _txen;
     uint8_t _rxcie;
     uint8_t _udre;
+    uint8_t _u2x;
   public:
     HardwareSerial(ring_buffer *rx_buffer,
       volatile uint8_t *ubrrh, volatile uint8_t *ubrrl,
       volatile uint8_t *ucsra, volatile uint8_t *ucsrb,
       volatile uint8_t *udr,
-      uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udre);
+      uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udre, uint8_t u2x);
     void begin(long);
     uint8_t available(void);
     int read(void);
@@ -62,4 +63,3 @@ extern HardwareSerial Serial3;
 #endif
 
 #endif
-
diff --git a/cores/arduino/wiring.h b/cores/arduino/wiring.h
index 619e695..f0ba04c 100755
--- a/cores/arduino/wiring.h
+++ b/cores/arduino/wiring.h
@@ -86,8 +86,8 @@ extern "C"{
 #define clockCyclesToMicroseconds(a) ( (a) / clockCyclesPerMicrosecond() )
 #define microsecondsToClockCycles(a) ( (a) * clockCyclesPerMicrosecond() )
 
-#define lowByte(w) ((w) & 0xff)
-#define highByte(w) ((w) >> 8)
+#define lowByte(w) ((uint8_t) ((w) & 0xff))
+#define highByte(w) ((uint8_t) ((w) >> 8))
 
 #define bitRead(value, bit) (((value) >> (bit)) & 0x01)
 #define bitSet(value, bit) ((value) |= (1UL << (bit)))
diff --git a/cores/arduino/wiring_analog.c b/cores/arduino/wiring_analog.c
index 529ad52..782a0bd 100755
--- a/cores/arduino/wiring_analog.c
+++ b/cores/arduino/wiring_analog.c
@@ -42,7 +42,7 @@ int analogRead(uint8_t pin)
 	// set the analog reference (high two bits of ADMUX) and select the
 	// channel (low 4 bits).  this also sets ADLAR (left-adjust result)
 	// to 0 (the default).
-	ADMUX = (analog_reference << 6) | (pin & 0x07);
+	ADMUX = (analog_reference << 6) | (pin & 0x0f);
   
 #if defined(__AVR_ATmega1280__)
 	// the MUX5 bit of ADCSRB selects whether we're reading from channels
diff --git a/libraries/Ethernet/Client.cpp b/libraries/Ethernet/Client.cpp
index ebbb08d..0511c7b 100644
--- a/libraries/Ethernet/Client.cpp
+++ b/libraries/Ethernet/Client.cpp
@@ -113,13 +113,21 @@ void Client::stop() {
 }
 
 uint8_t Client::connected() {
-  uint8_t s = status();
-  return !(s == SOCK_LISTEN || s == SOCK_CLOSED || s == SOCK_FIN_WAIT ||
-    (s == SOCK_CLOSE_WAIT && !available()));
+  if (_sock == 255) {
+    return 0;
+  } else {
+    uint8_t s = status();
+    return !(s == SOCK_LISTEN || s == SOCK_CLOSED || s == SOCK_FIN_WAIT ||
+      (s == SOCK_CLOSE_WAIT && !available()));
+  }
 }
 
 uint8_t Client::status() {
-  return getSn_SR(_sock);
+  if (_sock == 255) {
+    return SOCK_CLOSED;
+  } else {
+    return getSn_SR(_sock);
+  }
 }
 
 // the next three functions are a hack so we can compare the client returned
diff --git a/libraries/Ethernet/keywords.txt b/libraries/Ethernet/keywords.txt
new file mode 100644
index 0000000..ebc5793
--- /dev/null
+++ b/libraries/Ethernet/keywords.txt
@@ -0,0 +1,30 @@
+#######################################
+# Syntax Coloring Map For Ethernet
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+Ethernet	KEYWORD1
+Client	KEYWORD1
+Server	KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+status	KEYWORD2
+connect	KEYWORD2
+write	KEYWORD2
+available	KEYWORD2
+read	KEYWORD2
+flush	KEYWORD2
+stop	KEYWORD2
+connected	KEYWORD2
+begin	KEYWORD2
+
+#######################################
+# Constants (LITERAL1)
+#######################################
+
diff --git a/libraries/Firmata/Firmata.cpp b/libraries/Firmata/Firmata.cpp
index 4e59d27..9a18615 100644
--- a/libraries/Firmata/Firmata.cpp
+++ b/libraries/Firmata/Firmata.cpp
@@ -29,18 +29,18 @@ extern "C" {
 
 void sendValueAsTwo7bitBytes(int value)
 {
-    Serial.print(value & B01111111, BYTE); // LSB
-    Serial.print(value >> 7 & B01111111, BYTE); // MSB
+  Serial.print(value & B01111111, BYTE); // LSB
+  Serial.print(value >> 7 & B01111111, BYTE); // MSB
 }
 
 void startSysex(void)
 {
-    Serial.print(START_SYSEX, BYTE);
+  Serial.print(START_SYSEX, BYTE);
 }
 
 void endSysex(void)
 {
-    Serial.print(END_SYSEX, BYTE);
+  Serial.print(END_SYSEX, BYTE);
 }
 
 //******************************************************************************
@@ -49,8 +49,8 @@ void endSysex(void)
 
 FirmataClass::FirmataClass(void)
 {
-    firmwareVersionCount = 0;
-    systemReset();
+  firmwareVersionCount = 0;
+  systemReset();
 }
 
 //******************************************************************************
@@ -60,83 +60,83 @@ FirmataClass::FirmataClass(void)
 /* begin method for overriding default serial bitrate */
 void FirmataClass::begin(void)
 {
-    Serial.begin(115200);
-    blinkVersion();
-    delay(300);
-    printVersion();
+  Serial.begin(57600);
+  blinkVersion();
+  delay(300);
+  printVersion();
 }
 
 /* begin method for overriding default serial bitrate */
 void FirmataClass::begin(long speed)
 {
-    blinkVersion();
+  blinkVersion();
 #if defined(__AVR_ATmega128__)  // Wiring
-    Serial.begin((uint32_t)speed);
+  Serial.begin((uint32_t)speed);
 #else
-    Serial.begin(speed);
+  Serial.begin(speed);
 #endif
-    delay(300);
-    printVersion();
-    printFirmwareVersion();
+  delay(300);
+  printVersion();
+  printFirmwareVersion();
 }
 
 // output the protocol version message to the serial port
 void FirmataClass::printVersion(void) {
-    Serial.print(REPORT_VERSION, BYTE);
-    Serial.print(FIRMATA_MAJOR_VERSION, BYTE);
-    Serial.print(FIRMATA_MINOR_VERSION, BYTE);
+  Serial.print(REPORT_VERSION, BYTE);
+  Serial.print(FIRMATA_MAJOR_VERSION, BYTE);
+  Serial.print(FIRMATA_MINOR_VERSION, BYTE);
 }
 
 void FirmataClass::blinkVersion(void)
 {
-    // flash the pin with the protocol version
-    pinMode(VERSION_BLINK_PIN,OUTPUT);
-    pin13strobe(FIRMATA_MAJOR_VERSION, 200, 400);
-    delay(300);
-    pin13strobe(2,1,4); // separator, a quick burst
-    delay(300);
-    pin13strobe(FIRMATA_MINOR_VERSION, 200, 400);
+  // flash the pin with the protocol version
+  pinMode(VERSION_BLINK_PIN,OUTPUT);
+  pin13strobe(FIRMATA_MAJOR_VERSION, 200, 400);
+  delay(300);
+  pin13strobe(2,1,4); // separator, a quick burst
+  delay(300);
+  pin13strobe(FIRMATA_MINOR_VERSION, 200, 400);
 }
 
 void FirmataClass::printFirmwareVersion(void)
 {
-    byte i;
-
-    if(firmwareVersionCount) { // make sure that the name has been set before reporting
-        startSysex();
-        Serial.print(REPORT_FIRMWARE, BYTE);
-        Serial.print(firmwareVersionVector[0]); // major version number
-        Serial.print(firmwareVersionVector[1]); // minor version number
-        for(i=2; i<firmwareVersionCount; ++i) {
-            sendValueAsTwo7bitBytes(firmwareVersionVector[i]);
-        }
-        endSysex();
+  byte i;
+
+  if(firmwareVersionCount) { // make sure that the name has been set before reporting
+    startSysex();
+    Serial.print(REPORT_FIRMWARE, BYTE);
+    Serial.print(firmwareVersionVector[0]); // major version number
+    Serial.print(firmwareVersionVector[1]); // minor version number
+    for(i=2; i<firmwareVersionCount; ++i) {
+      sendValueAsTwo7bitBytes(firmwareVersionVector[i]);
     }
+    endSysex();
+  }
 }
 
 void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte minor)
 {
-    const char *filename;
-    char *extension;
-
-// parse out ".cpp" and "applet/" that comes from using __FILE__
-    extension = strstr(name, ".cpp");
-    filename = strrchr(name, '/') + 1; //points to slash, +1 gets to start of filename
-    // add two bytes for version numbers
-    if(extension && filename) {
-        firmwareVersionCount = extension - filename + 2;
-    } else {
-        firmwareVersionCount = strlen(name) + 2;
-        filename = name;
-    }
-    firmwareVersionVector = (byte *) malloc(firmwareVersionCount);
-    firmwareVersionVector[firmwareVersionCount] = 0;
-    firmwareVersionVector[0] = major;
-    firmwareVersionVector[1] = minor;
-    strncpy((char*)firmwareVersionVector + 2, filename, firmwareVersionCount - 2);
-// alas, no snprintf on Arduino
-//    snprintf(firmwareVersionVector, MAX_DATA_BYTES, "%c%c%s", 
-//             (char)major, (char)minor, firmwareVersionVector);
+  const char *filename;
+  char *extension;
+
+  // parse out ".cpp" and "applet/" that comes from using __FILE__
+  extension = strstr(name, ".cpp");
+  filename = strrchr(name, '/') + 1; //points to slash, +1 gets to start of filename
+  // add two bytes for version numbers
+  if(extension && filename) {
+    firmwareVersionCount = extension - filename + 2;
+  } else {
+    firmwareVersionCount = strlen(name) + 2;
+    filename = name;
+  }
+  firmwareVersionVector = (byte *) malloc(firmwareVersionCount);
+  firmwareVersionVector[firmwareVersionCount] = 0;
+  firmwareVersionVector[0] = major;
+  firmwareVersionVector[1] = minor;
+  strncpy((char*)firmwareVersionVector + 2, filename, firmwareVersionCount - 2);
+  // alas, no snprintf on Arduino
+  //    snprintf(firmwareVersionVector, MAX_DATA_BYTES, "%c%c%s", 
+  //             (char)major, (char)minor, firmwareVersionVector);
 }
 
 //------------------------------------------------------------------------------
@@ -144,123 +144,123 @@ void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte
 
 int FirmataClass::available(void)
 {
-    return Serial.available();
+  return Serial.available();
 }
 
 
 void FirmataClass::processSysexMessage(void)
 {
-    switch(storedInputData[0]) { //first byte in buffer is command
-    case REPORT_FIRMWARE:
-        printFirmwareVersion();
-        break;
-    case FIRMATA_STRING:
-        if(currentStringCallback) {
-            byte bufferLength = (sysexBytesRead - 1) / 2;
-            char *buffer = (char*)malloc(bufferLength * sizeof(char));
-            byte i = 1;
-            byte j = 0;
-            while(j < bufferLength) {
-                buffer[j] = (char)storedInputData[i];
-                i++;
-                buffer[j] += (char)(storedInputData[i] << 7);
-                i++;
-                j++;
-            }
-            (*currentStringCallback)(buffer);
-        }
-        break;
-    default:
-        if(currentSysexCallback)
-        (*currentSysexCallback)(storedInputData[0], sysexBytesRead - 1, storedInputData + 1);
+  switch(storedInputData[0]) { //first byte in buffer is command
+  case REPORT_FIRMWARE:
+    printFirmwareVersion();
+    break;
+  case STRING_DATA:
+    if(currentStringCallback) {
+      byte bufferLength = (sysexBytesRead - 1) / 2;
+      char *buffer = (char*)malloc(bufferLength * sizeof(char));
+      byte i = 1;
+      byte j = 0;
+      while(j < bufferLength) {
+        buffer[j] = (char)storedInputData[i];
+        i++;
+        buffer[j] += (char)(storedInputData[i] << 7);
+        i++;
+        j++;
+      }
+      (*currentStringCallback)(buffer);
     }
+    break;
+  default:
+    if(currentSysexCallback)
+      (*currentSysexCallback)(storedInputData[0], sysexBytesRead - 1, storedInputData + 1);
+  }
 }
 
 void FirmataClass::processInput(void)
 {
-    int inputData = Serial.read(); // this is 'int' to handle -1 when no data
-    int command;
+  int inputData = Serial.read(); // this is 'int' to handle -1 when no data
+  int command;
     
-    // TODO make sure it handles -1 properly
-
-    if (parsingSysex) {
-        if(inputData == END_SYSEX) {
-            //stop sysex byte      
-            parsingSysex = false;
-            //fire off handler function
-            processSysexMessage();
-        } else {
-            //normal data byte - add to buffer
-            storedInputData[sysexBytesRead] = inputData;
-            sysexBytesRead++;
-        }
-    } else if( (waitForData > 0) && (inputData < 128) ) {  
-        waitForData--;
-        storedInputData[waitForData] = inputData;
-        if( (waitForData==0) && executeMultiByteCommand ) { // got the whole message
-            switch(executeMultiByteCommand) {
-            case ANALOG_MESSAGE:
-                if(currentAnalogCallback) {
-                    (*currentAnalogCallback)(multiByteChannel,
-                                             (storedInputData[0] << 7)
-                                             + storedInputData[1]);
-                }
-                break;
-            case DIGITAL_MESSAGE:
-                if(currentDigitalCallback) {
-                    (*currentDigitalCallback)(multiByteChannel,
-                                              (storedInputData[0] << 7)
-                                              + storedInputData[1]);
-                }
-                break;
-            case SET_PIN_MODE:
-                if(currentPinModeCallback)
-                    (*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
-                break;
-            case REPORT_ANALOG:
-                if(currentReportAnalogCallback)
-                    (*currentReportAnalogCallback)(multiByteChannel,storedInputData[0]);
-                break;
-            case REPORT_DIGITAL:
-                if(currentReportDigitalCallback)
-                    (*currentReportDigitalCallback)(multiByteChannel,storedInputData[0]);
-                break;
-            }
-            executeMultiByteCommand = 0;
-        }	
+  // TODO make sure it handles -1 properly
+
+  if (parsingSysex) {
+    if(inputData == END_SYSEX) {
+      //stop sysex byte      
+      parsingSysex = false;
+      //fire off handler function
+      processSysexMessage();
     } else {
-        // remove channel info from command byte if less than 0xF0
-        if(inputData < 0xF0) {
-            command = inputData & 0xF0;
-            multiByteChannel = inputData & 0x0F;
-        } else {
-            command = inputData;
-            // commands in the 0xF* range don't use channel data
+      //normal data byte - add to buffer
+      storedInputData[sysexBytesRead] = inputData;
+      sysexBytesRead++;
+    }
+  } else if( (waitForData > 0) && (inputData < 128) ) {  
+    waitForData--;
+    storedInputData[waitForData] = inputData;
+    if( (waitForData==0) && executeMultiByteCommand ) { // got the whole message
+      switch(executeMultiByteCommand) {
+      case ANALOG_MESSAGE:
+        if(currentAnalogCallback) {
+          (*currentAnalogCallback)(multiByteChannel,
+                                   (storedInputData[0] << 7)
+                                   + storedInputData[1]);
         }
-        switch (command) {
-        case ANALOG_MESSAGE:
-        case DIGITAL_MESSAGE:
-        case SET_PIN_MODE:
-            waitForData = 2; // two data bytes needed
-            executeMultiByteCommand = command;
-            break;
-        case REPORT_ANALOG:
-        case REPORT_DIGITAL:
-            waitForData = 1; // two data bytes needed
-            executeMultiByteCommand = command;
-            break;
-        case START_SYSEX:
-            parsingSysex = true;
-            sysexBytesRead = 0;
-            break;
-        case SYSTEM_RESET:
-            systemReset();
-            break;
-        case REPORT_VERSION:
-            Firmata.printVersion();
-            break;
+        break;
+      case DIGITAL_MESSAGE:
+        if(currentDigitalCallback) {
+          (*currentDigitalCallback)(multiByteChannel,
+                                    (storedInputData[0] << 7)
+                                    + storedInputData[1]);
         }
+        break;
+      case SET_PIN_MODE:
+        if(currentPinModeCallback)
+          (*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
+        break;
+      case REPORT_ANALOG:
+        if(currentReportAnalogCallback)
+          (*currentReportAnalogCallback)(multiByteChannel,storedInputData[0]);
+        break;
+      case REPORT_DIGITAL:
+        if(currentReportDigitalCallback)
+          (*currentReportDigitalCallback)(multiByteChannel,storedInputData[0]);
+        break;
+      }
+      executeMultiByteCommand = 0;
+    }	
+  } else {
+    // remove channel info from command byte if less than 0xF0
+    if(inputData < 0xF0) {
+      command = inputData & 0xF0;
+      multiByteChannel = inputData & 0x0F;
+    } else {
+      command = inputData;
+      // commands in the 0xF* range don't use channel data
     }
+    switch (command) {
+    case ANALOG_MESSAGE:
+    case DIGITAL_MESSAGE:
+    case SET_PIN_MODE:
+      waitForData = 2; // two data bytes needed
+      executeMultiByteCommand = command;
+      break;
+    case REPORT_ANALOG:
+    case REPORT_DIGITAL:
+      waitForData = 1; // two data bytes needed
+      executeMultiByteCommand = command;
+      break;
+    case START_SYSEX:
+      parsingSysex = true;
+      sysexBytesRead = 0;
+      break;
+    case SYSTEM_RESET:
+      systemReset();
+      break;
+    case REPORT_VERSION:
+      Firmata.printVersion();
+      break;
+    }
+  }
 }
 
 //------------------------------------------------------------------------------
@@ -269,31 +269,31 @@ void FirmataClass::processInput(void)
 // send an analog message
 void FirmataClass::sendAnalog(byte pin, int value) 
 {
-	// pin can only be 0-15, so chop higher bits
-	Serial.print(ANALOG_MESSAGE | (pin & 0xF), BYTE);
-    sendValueAsTwo7bitBytes(value);
+  // pin can only be 0-15, so chop higher bits
+  Serial.print(ANALOG_MESSAGE | (pin & 0xF), BYTE);
+  sendValueAsTwo7bitBytes(value);
 }
 
 // send a single digital pin in a digital message
 void FirmataClass::sendDigital(byte pin, int value) 
 {
-	/* TODO add single pin digital messages to the protocol, this needs to
-     * track the last digital data sent so that it can be sure to change just
-     * one bit in the packet.  This is complicated by the fact that the
-     * numbering of the pins will probably differ on Arduino, Wiring, and
-     * other boards.  The DIGITAL_MESSAGE sends 14 bits at a time, but it is
-     * probably easier to send 8 bit ports for any board with more than 14
-     * digital pins.
-     */
-
-	// TODO: the digital message should not be sent on the serial port every
-	// time sendDigital() is called.  Instead, it should add it to an int
-	// which will be sent on a schedule.  If a pin changes more than once
-	// before the digital message is sent on the serial port, it should send a
-	// digital message for each change.
-
-//    if(value == 0)
-//        sendDigitalPortPair();
+  /* TODO add single pin digital messages to the protocol, this needs to
+   * track the last digital data sent so that it can be sure to change just
+   * one bit in the packet.  This is complicated by the fact that the
+   * numbering of the pins will probably differ on Arduino, Wiring, and
+   * other boards.  The DIGITAL_MESSAGE sends 14 bits at a time, but it is
+   * probably easier to send 8 bit ports for any board with more than 14
+   * digital pins.
+   */
+
+  // TODO: the digital message should not be sent on the serial port every
+  // time sendDigital() is called.  Instead, it should add it to an int
+  // which will be sent on a schedule.  If a pin changes more than once
+  // before the digital message is sent on the serial port, it should send a
+  // digital message for each change.
+
+  //    if(value == 0)
+  //        sendDigitalPortPair();
 }
 
 
@@ -301,33 +301,33 @@ void FirmataClass::sendDigital(byte pin, int value)
 // send an 8-bit port in a single digital message (protocol v2)
 void FirmataClass::sendDigitalPort(byte portNumber, int portData)
 {
-	Serial.print(DIGITAL_MESSAGE | (portNumber & 0xF),BYTE);
-	Serial.print(portData % 128, BYTE); // Tx bits 0-6
-	Serial.print(portData >> 7, BYTE);  // Tx bits 7-13
+  Serial.print(DIGITAL_MESSAGE | (portNumber & 0xF),BYTE);
+  Serial.print(portData % 128, BYTE); // Tx bits 0-6
+  Serial.print(portData >> 7, BYTE);  // Tx bits 7-13
 }
 
 
 void FirmataClass::sendSysex(byte command, byte bytec, byte* bytev) 
 {
-    byte i;
-    startSysex();
-    Serial.print(command, BYTE);
-    for(i=0; i<bytec; i++) {
-        sendValueAsTwo7bitBytes(bytev[i]);        
-    }
-    endSysex();
+  byte i;
+  startSysex();
+  Serial.print(command, BYTE);
+  for(i=0; i<bytec; i++) {
+    sendValueAsTwo7bitBytes(bytev[i]);        
+  }
+  endSysex();
 }
 
 void FirmataClass::sendString(byte command, const char* string) 
 {
-    sendSysex(command, strlen(string), (byte *)string);
+  sendSysex(command, strlen(string), (byte *)string);
 }
 
 
 // send a string as the protocol string type
 void FirmataClass::sendString(const char* string) 
 {
-    sendString(FIRMATA_STRING, string);
+  sendString(STRING_DATA, string);
 }
 
 
@@ -336,43 +336,43 @@ void FirmataClass::sendString(const char* string)
 // generic callbacks
 void FirmataClass::attach(byte command, callbackFunction newFunction)
 {
-    switch(command) {
-    case ANALOG_MESSAGE: currentAnalogCallback = newFunction; break;
-    case DIGITAL_MESSAGE: currentDigitalCallback = newFunction; break;
-    case REPORT_ANALOG: currentReportAnalogCallback = newFunction; break;
-    case REPORT_DIGITAL: currentReportDigitalCallback = newFunction; break;
-    case SET_PIN_MODE: currentPinModeCallback = newFunction; break;
-    }
+  switch(command) {
+  case ANALOG_MESSAGE: currentAnalogCallback = newFunction; break;
+  case DIGITAL_MESSAGE: currentDigitalCallback = newFunction; break;
+  case REPORT_ANALOG: currentReportAnalogCallback = newFunction; break;
+  case REPORT_DIGITAL: currentReportDigitalCallback = newFunction; break;
+  case SET_PIN_MODE: currentPinModeCallback = newFunction; break;
+  }
 }
 
 void FirmataClass::attach(byte command, systemResetCallbackFunction newFunction)
 {
-    switch(command) {
-    case SYSTEM_RESET: currentSystemResetCallback = newFunction; break;
-    }
+  switch(command) {
+  case SYSTEM_RESET: currentSystemResetCallback = newFunction; break;
+  }
 }
 
 void FirmataClass::attach(byte command, stringCallbackFunction newFunction)
 {
-    switch(command) {
-    case FIRMATA_STRING: currentStringCallback = newFunction; break;
-    }
+  switch(command) {
+  case STRING_DATA: currentStringCallback = newFunction; break;
+  }
 }
 
 void FirmataClass::attach(byte command, sysexCallbackFunction newFunction)
 {
-    currentSysexCallback = newFunction;
+  currentSysexCallback = newFunction;
 }
 
 void FirmataClass::detach(byte command)
 {
-    switch(command) {
-    case SYSTEM_RESET: currentSystemResetCallback = NULL; break;
-    case FIRMATA_STRING: currentStringCallback = NULL; break;
-    case START_SYSEX: currentSysexCallback = NULL; break;
-    default:
-        attach(command, (callbackFunction)NULL);
-    }
+  switch(command) {
+  case SYSTEM_RESET: currentSystemResetCallback = NULL; break;
+  case STRING_DATA: currentStringCallback = NULL; break;
+  case START_SYSEX: currentSysexCallback = NULL; break;
+  default:
+    attach(command, (callbackFunction)NULL);
+  }
 }
 
 // sysex callbacks
@@ -402,24 +402,24 @@ void FirmataClass::detach(byte command)
 // resets the system state upon a SYSTEM_RESET message from the host software
 void FirmataClass::systemReset(void)
 {
-    byte i;
+  byte i;
 
-    waitForData = 0; // this flag says the next serial input will be data
-    executeMultiByteCommand = 0; // execute this after getting multi-byte data
-    multiByteChannel = 0; // channel data for multiByteCommands
+  waitForData = 0; // this flag says the next serial input will be data
+  executeMultiByteCommand = 0; // execute this after getting multi-byte data
+  multiByteChannel = 0; // channel data for multiByteCommands
 
 
-    for(i=0; i<MAX_DATA_BYTES; i++) {
-        storedInputData[i] = 0;
-    }
+  for(i=0; i<MAX_DATA_BYTES; i++) {
+    storedInputData[i] = 0;
+  }
 
-    parsingSysex = false;
-    sysexBytesRead = 0;
+  parsingSysex = false;
+  sysexBytesRead = 0;
 
-    if(currentSystemResetCallback)
-                    (*currentSystemResetCallback)();
+  if(currentSystemResetCallback)
+    (*currentSystemResetCallback)();
 
-    //flush(); //TODO uncomment when Firmata is a subclass of HardwareSerial
+  //flush(); //TODO uncomment when Firmata is a subclass of HardwareSerial
 }
 
 
@@ -428,14 +428,14 @@ void FirmataClass::systemReset(void)
 // used for flashing the pin for the version number
 void FirmataClass::pin13strobe(int count, int onInterval, int offInterval) 
 {
-    byte i;
-    pinMode(VERSION_BLINK_PIN, OUTPUT);
-    for(i=0; i<count; i++) {
-        delay(offInterval);
-        digitalWrite(VERSION_BLINK_PIN, HIGH);
-        delay(onInterval);
-        digitalWrite(VERSION_BLINK_PIN, LOW);
-    }
+  byte i;
+  pinMode(VERSION_BLINK_PIN, OUTPUT);
+  for(i=0; i<count; i++) {
+    delay(offInterval);
+    digitalWrite(VERSION_BLINK_PIN, HIGH);
+    delay(onInterval);
+    digitalWrite(VERSION_BLINK_PIN, LOW);
+  }
 }
 
 
diff --git a/libraries/Firmata/Firmata.h b/libraries/Firmata/Firmata.h
index a926462..2732fd6 100644
--- a/libraries/Firmata/Firmata.h
+++ b/libraries/Firmata/Firmata.h
@@ -22,8 +22,7 @@
  * software can test whether it will be compatible with the currently
  * installed firmware. */
 #define FIRMATA_MAJOR_VERSION   2 // for non-compatible changes
-#define FIRMATA_MINOR_VERSION   0 // for backwards compatible changes
-#define VERSION_BLINK_PIN       13 // digital pin to blink version on
+#define FIRMATA_MINOR_VERSION   1 // for backwards compatible changes
 
 #define MAX_DATA_BYTES 32 // max number of data bytes in non-Sysex messages
 
@@ -42,12 +41,22 @@
 #define END_SYSEX               0xF7 // end a MIDI Sysex message
 
 // extended command set using sysex (0-127/0x00-0x7F)
-/* 0x00-0x0F reserved for custom commands */
+/* 0x00-0x0F reserved for user-defined commands */
 #define SERVO_CONFIG            0x70 // set max angle, minPulse, maxPulse, freq
-#define FIRMATA_STRING          0x71 // a string message with 14-bits per char
+#define STRING_DATA             0x71 // a string message with 14-bits per char
+#define SHIFT_DATA              0x75 // a bitstream to/from a shift register
+#define I2C_REQUEST             0x76 // send an I2C read/write request
+#define I2C_REPLY               0x77 // a reply to an I2C read request
+#define I2C_CONFIG				0x78 // config I2C settings such as delay times and power pins
 #define REPORT_FIRMWARE         0x79 // report name and version of the firmware
+#define SAMPLING_INTERVAL       0x7A // set the poll rate of the main loop
 #define SYSEX_NON_REALTIME      0x7E // MIDI Reserved for non-realtime messages
 #define SYSEX_REALTIME          0x7F // MIDI Reserved for realtime messages
+// these are DEPRECATED to make the naming more consistent
+#define FIRMATA_STRING          0x71 // same as STRING_DATA
+#define SYSEX_I2C_REQUEST       0x76 // same as I2C_REQUEST
+#define SYSEX_I2C_REPLY         0x77 // same as I2C_REPLY
+#define SYSEX_SAMPLING_INTERVAL 0x7A // same as SAMPLING_INTERVAL
 
 // pin modes
 //#define INPUT                 0x00 // defined in wiring.h
@@ -55,7 +64,8 @@
 #define ANALOG                  0x02 // analog pin in analogInput mode
 #define PWM                     0x03 // digital pin in PWM output mode
 #define SERVO                   0x04 // digital pin in Servo output mode
-
+#define SHIFT                   0x05 // shiftIn/shiftOut mode
+#define I2C                     0x06 // pin included in I2C setup
 
 extern "C" {
 // callback function types
@@ -146,21 +156,71 @@ extern FirmataClass Firmata;
 #define TOTAL_DIGITAL_PINS      22 // 14 digital + 8 analog
 #define TOTAL_PORTS             3 // total number of ports for the board
 #define ANALOG_PORT             2 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        14 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
 #elif defined(__AVR_ATmega8__)  // old Arduinos
 #define TOTAL_ANALOG_PINS       6
 #define TOTAL_DIGITAL_PINS      20 // 14 digital + 6 analog
 #define TOTAL_PORTS             3  // total number of ports for the board
 #define ANALOG_PORT             2  // port# of analog used as digital
+#define FIRST_ANALOG_PIN        14 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
+#elif defined(__AVR_ATmega1280__)// Arduino Mega
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_DIGITAL_PINS      70 // 54 digital + 16 analog
+#define TOTAL_PORTS             9 // total number of ports for the board
+#define ANALOG_PORT             8 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        54 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
 #elif defined(__AVR_ATmega128__)// Wiring
 #define TOTAL_ANALOG_PINS       8
 #define TOTAL_DIGITAL_PINS      51
+#define TOTAL_PORTS             7 // total number of ports for the board
+#define ANALOG_PORT             5 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        40 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
+#elif defined(__AVR_AT90USB162__) // Teensy
+#define TOTAL_ANALOG_PINS       0
+#define TOTAL_DIGITAL_PINS      21 // 21 digital + no analog
+#define TOTAL_PORTS             4 // total number of ports for the board
+#define ANALOG_PORT             3 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        21 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       6 // digital pin to blink version on
+#elif defined(__AVR_ATmega32U4__) // Teensy
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_DIGITAL_PINS      25 // 11 digital + 12 analog
+#define TOTAL_PORTS             4 // total number of ports for the board
+#define ANALOG_PORT             3 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        11 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       11 // digital pin to blink version on
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) // Teensy++
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_DIGITAL_PINS      46 // 38 digital + 8 analog
 #define TOTAL_PORTS             6 // total number of ports for the board
-#define ANALOG_PORT             2 // port# of analog used as digital
+#define ANALOG_PORT             5 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        38 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       6 // digital pin to blink version on
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)  // Sanguino
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_DIGITAL_PINS      32 // 24 digital + 8 analog
+#define TOTAL_PORTS             4 // total number of ports for the board
+#define ANALOG_PORT             3 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        24 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       0 // digital pin to blink version on
+#elif defined(__AVR_ATmega645__)  // Illuminato
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_DIGITAL_PINS      42 // 36 digital + 6 analog
+#define TOTAL_PORTS             6 // total number of ports for the board
+#define ANALOG_PORT             4 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        36 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
 #else // anything else
 #define TOTAL_ANALOG_PINS       6
 #define TOTAL_DIGITAL_PINS      14
 #define TOTAL_PORTS             3 // total number of ports for the board
 #define ANALOG_PORT             2 // port# of analog used as digital
+#define FIRST_ANALOG_PIN        14 // pin# corresponding to analog 0
+#define VERSION_BLINK_PIN       13 // digital pin to blink version on
 #endif
 
 
diff --git a/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.pde b/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.pde
index fcd8e5e..ab83726 100644
--- a/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.pde
+++ b/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.pde
@@ -61,7 +61,7 @@ void setup()
 
     servo9.attach(9);
     servo10.attach(10);
-    Firmata.begin();
+    Firmata.begin(57600);
 }
 
 /*==============================================================================
diff --git a/libraries/Firmata/examples/EchoString/EchoString.pde b/libraries/Firmata/examples/EchoString/EchoString.pde
index ed13ad8..6559ae1 100644
--- a/libraries/Firmata/examples/EchoString/EchoString.pde
+++ b/libraries/Firmata/examples/EchoString/EchoString.pde
@@ -25,9 +25,9 @@ void sysexCallback(byte command, byte argc, byte*argv)
 void setup()
 {
     Firmata.setFirmwareVersion(0, 1);
-    Firmata.attach(FIRMATA_STRING, stringCallback);
+    Firmata.attach(STRING_DATA, stringCallback);
     Firmata.attach(START_SYSEX, sysexCallback);
-    Firmata.begin();
+    Firmata.begin(57600);
 }
 
 void loop()
diff --git a/libraries/Firmata/examples/I2CFirmata/I2CFirmata.pde b/libraries/Firmata/examples/I2CFirmata/I2CFirmata.pde
new file mode 100644
index 0000000..796a8d5
--- /dev/null
+++ b/libraries/Firmata/examples/I2CFirmata/I2CFirmata.pde
@@ -0,0 +1,217 @@
+/*
+ Copyright (C) 2009 Jeff Hoefs.  All rights reserved.
+ Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+ 
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+ 
+ See file LICENSE.txt for further informations on licensing terms.
+ */
+
+#include <Wire.h>
+#include <Firmata.h>
+
+
+#define I2C_WRITE B00000000
+#define I2C_READ B00001000
+#define I2C_READ_CONTINUOUSLY B00010000
+#define I2C_STOP_READING B00011000
+#define I2C_READ_WRITE_MODE_MASK B00011000
+
+#define MAX_QUERIES 8
+
+unsigned long currentMillis;     // store the current value from millis()
+unsigned long nextExecuteMillis; // for comparison with currentMillis
+unsigned int samplingInterval = 32;  // default sampling interval is 33ms
+unsigned int i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
+unsigned int powerPinsEnabled = 0;  // use as boolean to prevent enablePowerPins from being called more than once
+
+#define MINIMUM_SAMPLING_INTERVAL 10
+
+#define REGISTER_NOT_SPECIFIED -1
+
+struct i2c_device_info {
+  byte addr;
+  byte reg;
+  byte bytes;
+};
+
+i2c_device_info query[MAX_QUERIES];
+
+byte i2cRxData[32];
+boolean readingContinuously = false;
+byte queryIndex = 0;
+
+void readAndReportData(byte address, int theRegister, byte numBytes)
+{
+  if (theRegister != REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    Wire.send((byte)theRegister);
+    Wire.endTransmission();
+    delayMicroseconds(i2cReadDelayTime);  // delay is necessary for some devices such as WiiNunchuck
+  } 
+  else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);
+
+  // check to be sure correct number of bytes were returned by slave
+  if(numBytes == Wire.available()) {
+    i2cRxData[0] = address;
+    i2cRxData[1] = theRegister;
+    for (int i = 0; i < numBytes; i++) {
+      i2cRxData[2 + i] = Wire.receive();
+    }
+    // send slave address, register and received bytes
+    Firmata.sendSysex(I2C_REPLY, numBytes + 2, i2cRxData);
+  }
+  else {
+    if(numBytes > Wire.available()) {
+      Firmata.sendString("I2C Read Error: Too many bytes received");
+    } else {
+      Firmata.sendString("I2C Read Error: Too few bytes received"); 
+    }
+  }
+  
+}
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte slaveAddress;
+  byte slaveRegister;
+  byte data;
+  int  delayTime;
+
+  if (command == I2C_REQUEST) {
+    mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+    slaveAddress = argv[0];
+
+    switch(mode) {
+    case I2C_WRITE:
+      Wire.beginTransmission(slaveAddress);
+      for (byte i = 2; i < argc; i += 2) {
+        data = argv[i] + (argv[i + 1] << 7);
+        Wire.send(data);
+      }
+      Wire.endTransmission();
+      delayMicroseconds(70); // TODO is this needed?
+      break;
+    case I2C_READ:
+      if (argc == 6) {
+        // a slave register is specified
+        slaveRegister = argv[2] + (argv[3] << 7);
+        data = argv[4] + (argv[5] << 7);  // bytes to read
+        readAndReportData(slaveAddress, (int)slaveRegister, data);
+      } 
+      else {
+        // a slave register is NOT specified
+        data = argv[2] + (argv[3] << 7);  // bytes to read
+        readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data);
+      }
+      break;
+    case I2C_READ_CONTINUOUSLY:
+      if ((queryIndex + 1) >= MAX_QUERIES) {
+        // too many queries, just ignore
+        Firmata.sendString("too many queries");
+        break;
+      }
+      query[queryIndex].addr = slaveAddress;
+      query[queryIndex].reg = argv[2] + (argv[3] << 7);
+      query[queryIndex].bytes = argv[4] + (argv[5] << 7);
+      readingContinuously = true;
+      queryIndex++;
+      break;
+    case I2C_STOP_READING:
+      readingContinuously = false;
+      queryIndex = 0;
+      break;
+    default:
+      break;
+    }
+  }
+  else if (command == SAMPLING_INTERVAL) {
+    samplingInterval = argv[0] + (argv[1] << 7);
+
+    if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+      samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+    }
+
+    samplingInterval -= 1;
+    Firmata.sendString("sampling interval");
+  }
+
+  else if (command == I2C_CONFIG) {
+    delayTime = (argv[4] + (argv[5] << 7));                        // MSB
+    delayTime = (delayTime << 8) + (argv[2] + (argv[3] << 7));     // add LSB
+
+    if((argv[0] + (argv[1] << 7)) > 0) {
+      enablePowerPins(PORTC3, PORTC2);
+    }
+
+    if(delayTime > 0) {
+      i2cReadDelayTime = delayTime;
+    }
+
+    if(argc > 6) {
+      // If you extend I2C_Config, handle your data here
+    }
+
+  }
+}
+
+void systemResetCallback()
+{
+  readingContinuously = false;
+  queryIndex = 0;
+}
+
+/* reference: BlinkM_funcs.h by Tod E. Kurt, ThingM, http://thingm.com/ */
+// Enables Pins A2 and A3 to be used as GND and Power
+// so that I2C devices can be plugged directly
+// into Arduino header (pins A2 - A5)
+static void enablePowerPins(byte pwrpin, byte gndpin)
+{
+  if(powerPinsEnabled == 0) {
+    DDRC |= _BV(pwrpin) | _BV(gndpin);
+    PORTC &=~ _BV(gndpin);
+    PORTC |=  _BV(pwrpin);
+    powerPinsEnabled = 1;
+    Firmata.sendString("Power pins enabled");
+    delay(100);
+  }
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(2, 0);
+
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  for (int i = 0; i < TOTAL_DIGITAL_PINS; ++i) {
+    pinMode(i, OUTPUT);
+  }
+
+  Firmata.begin(57600);  
+  Wire.begin();
+}
+
+void loop()
+{
+  while (Firmata.available()) {
+    Firmata.processInput();
+  }
+
+  currentMillis = millis();
+  if (currentMillis > nextExecuteMillis) {
+    nextExecuteMillis = currentMillis + samplingInterval;
+
+    for (byte i = 0; i < queryIndex; i++) {
+      readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
+    }
+  }
+}
diff --git a/libraries/Firmata/examples/ServoFirmata/ServoFirmata.pde b/libraries/Firmata/examples/ServoFirmata/ServoFirmata.pde
index a3c609c..fa48e2b 100644
--- a/libraries/Firmata/examples/ServoFirmata/ServoFirmata.pde
+++ b/libraries/Firmata/examples/ServoFirmata/ServoFirmata.pde
@@ -28,7 +28,7 @@ void setup()
     servo9.attach(9);
     servo10.attach(10);
    
-    Firmata.begin();
+    Firmata.begin(57600);
 }
 
 void loop() 
diff --git a/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.pde b/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.pde
index 2950c01..430d0d0 100644
--- a/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.pde
+++ b/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.pde
@@ -16,7 +16,7 @@ void setup()
 {
     Firmata.setFirmwareVersion(0, 1);
     Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
-    Firmata.begin();
+    Firmata.begin(57600);
 }
 
 void loop()
diff --git a/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.pde b/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.pde
index 1104a92..9c4e05d 100644
--- a/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.pde
+++ b/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.pde
@@ -45,7 +45,7 @@ void setup()
     Firmata.setFirmwareVersion(0, 1);
     Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
     Firmata.attach(SET_PIN_MODE, setPinModeCallback);
-    Firmata.begin();
+    Firmata.begin(57600);
 }
 
 void loop()
diff --git a/libraries/Firmata/examples/StandardFirmata/Makefile b/libraries/Firmata/examples/StandardFirmata/Makefile
index 55ca8c2..835187a 100644
--- a/libraries/Firmata/examples/StandardFirmata/Makefile
+++ b/libraries/Firmata/examples/StandardFirmata/Makefile
@@ -50,14 +50,20 @@ TARGET := $(shell pwd | sed 's|.*/\(.*\)|\1|')
 ARDUINO = /Applications/arduino
 ARDUINO_SRC = $(ARDUINO)/hardware/cores/arduino
 ARDUINO_LIB_SRC = $(ARDUINO)/hardware/libraries
+ARDUINO_TOOLS = $(ARDUINO)/hardware/tools
 INCLUDE = -I$(ARDUINO_SRC) -I$(ARDUINO)/hardware/tools/avr/avr/include \
 	-I$(ARDUINO_LIB_SRC)/EEPROM \
 	-I$(ARDUINO_LIB_SRC)/Firmata \
+	-I$(ARDUINO_LIB_SRC)/Matrix \
+	-I$(ARDUINO_LIB_SRC)/Servo \
+	-I$(ARDUINO_LIB_SRC)/Wire \
 	-I$(ARDUINO_LIB_SRC)
 SRC = $(wildcard $(ARDUINO_SRC)/*.c)
 CXXSRC = applet/$(TARGET).cpp $(ARDUINO_SRC)/HardwareSerial.cpp \
 	$(ARDUINO_LIB_SRC)/EEPROM/EEPROM.cpp \
 	$(ARDUINO_LIB_SRC)/Firmata/Firmata.cpp \
+	$(ARDUINO_LIB_SRC)/Servo/Servo.cpp \
+	$(ARDUINO_SRC)/Print.cpp \
 	$(ARDUINO_SRC)/WMath.cpp
 HEADERS = $(wildcard $(ARDUINO_SRC)/*.h) $(wildcard $(ARDUINO_LIB_SRC)/*/*.h)
 
@@ -106,12 +112,14 @@ AVRDUDE_FLAGS = -F -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) \
   -b $(UPLOAD_RATE) -q -V
 
 # Program settings
-CC = avr-gcc
-CXX = avr-g++
-OBJCOPY = avr-objcopy
-OBJDUMP = avr-objdump
-SIZE = avr-size
-NM = avr-nm
+ARDUINO_AVR_BIN = $(ARDUINO_TOOLS)/avr/bin
+CC = $(ARDUINO_AVR_BIN)/avr-gcc 
+CXX = $(ARDUINO_AVR_BIN)/avr-g++
+OBJCOPY = $(ARDUINO_AVR_BIN)/avr-objcopy
+OBJDUMP = $(ARDUINO_AVR_BIN)/avr-objdump
+SIZE = $(ARDUINO_AVR_BIN)/avr-size
+NM = $(ARDUINO_AVR_BIN)/avr-nm
+#AVRDUDE = $(ARDUINO_AVR_BIN)/avrdude
 AVRDUDE = avrdude
 REMOVE = rm -f
 MV = mv -f
@@ -204,7 +212,8 @@ applet/$(TARGET).cpp: $(TARGET).pde
 
 # Link: create ELF output file from object files.
 applet/$(TARGET).elf: applet/$(TARGET).cpp $(OBJ)
-	$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
+	$(CC) $(ALL_CFLAGS) $(OBJ) -lm --output $@ $(LDFLAGS)
+#	$(CC) $(ALL_CFLAGS) $(OBJ) $(ARDUINO_TOOLS)/avr/avr/lib/avr5/crtm168.o --output $@ $(LDFLAGS)
 
 pd_close_serial:
 	echo 'close;' | /Applications/Pd-extended.app/Contents/Resources/bin/pdsend 34567 || true
@@ -258,4 +267,7 @@ etags_MINGW:
 #	etags -a /usr/include/*.h /usr/include/sys/*.h 
 
 
+path:
+	echo $(PATH)
+	echo $$PATH
 
diff --git a/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde b/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde
index 4cc8539..16c8b87 100644
--- a/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde
+++ b/libraries/Firmata/examples/StandardFirmata/StandardFirmata.pde
@@ -1,21 +1,24 @@
 /*
   Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
- 
+  
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
  
   See file LICENSE.txt for further informations on licensing terms.
- */
+
+  formatted using the GNU C formatting and indenting
+*/
+
 
 /* 
- * TODO: add Servo support using setPinMode(pin, SERVO);
+ * TODO: add Servo support using setPinModeCallback(pin, SERVO);
  * TODO: use Program Control to load stored profiles from EEPROM
  */
 
-#include <EEPROM.h>
 #include <Firmata.h>
+#include <Servo.h>
 
 /*==============================================================================
  * GLOBAL VARIABLES
@@ -34,37 +37,45 @@ byte portStatus[TOTAL_PORTS];
 /* timer variables */
 unsigned long currentMillis;     // store the current value from millis()
 unsigned long nextExecuteMillis; // for comparison with currentMillis
+int samplingInterval = 19;      // how often to run the main loop (in ms)
 
+Servo servos[2]; // the servo library can control servos on pins 9 and 10 only
 
 /*==============================================================================
- * FUNCTIONS                                                                
+ * FUNCTIONS
  *============================================================================*/
 
 void outputPort(byte portNumber, byte portValue)
 {
   portValue = portValue &~ portStatus[portNumber];
   if(previousPINs[portNumber] != portValue) {
-        Firmata.sendDigitalPort(portNumber, portValue); 
-        previousPINs[portNumber] = portValue;
-        Firmata.sendDigitalPort(portNumber, portValue); 
-    }
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+    Firmata.sendDigitalPort(portNumber, portValue);
+  }
 }
 
 /* -----------------------------------------------------------------------------
  * check all the active digital inputs for change of state, then add any events
  * to the Serial output queue using Serial.print() */
-void checkDigitalInputs(void) 
+void checkDigitalInputs(void)
 {
-    byte i, tmp;
-    for(i=0; i < TOTAL_PORTS; i++) {
-        if(reportPINs[i]) {
-            switch(i) {
-            case 0: outputPort(0, PIND &~ B00000011); break; // ignore Rx/Tx 0/1
-            case 1: outputPort(1, PINB); break;
-            case ANALOG_PORT: outputPort(ANALOG_PORT, PINC); break;
-            }
-        }
+  byte i, tmp;
+  for(i=0; i < TOTAL_PORTS; i++) {
+    if(reportPINs[i]) {
+      switch(i) {
+      case 0:
+        outputPort(0, PIND &~ B00000011); // ignore Rx/Tx 0/1
+        break;
+      case 1:
+        outputPort(1, PINB);
+        break;
+      case ANALOG_PORT:
+        outputPort(ANALOG_PORT, PINC);
+        break;
+      }
     }
+  }
 }
 
 // -----------------------------------------------------------------------------
@@ -72,61 +83,88 @@ void checkDigitalInputs(void)
  * two bit-arrays that track Digital I/O and PWM status
  */
 void setPinModeCallback(byte pin, int mode) {
-    byte port = 0;
-    byte offset = 0;
-
-    if (pin < 8) {
-      port = 0;
-      offset = 0;
-    } else if (pin < 14) {
-      port = 1;
-      offset = 8;     
-    } else if (pin < 22) {
-      port = 2;
-      offset = 14;
-    }
+  byte port = 0;
+  byte offset = 0;
 
-    if(pin > 1) { // ignore RxTx (pins 0 and 1)
+  // TODO: abstract for different boards
+  if (pin < 8) {
+    port = 0;
+    offset = 0;
+  } else if (pin < 14) {
+    port = 1;
+    offset = 8;
+  } else if (pin < 22) {
+    port = 2;
+    offset = 14;
+  }
+  
+  if(pin > 1) { // ignore RxTx (pins 0 and 1)
+    if(pin > 13) 
+      reportAnalogCallback(pin - 14, mode == ANALOG ? 1 : 0); // turn on/off reporting
+    switch(mode) {
+    case ANALOG:
+      digitalWrite(pin, LOW); // disable internal pull-ups and fall thru to 'case INPUT:'
+    case INPUT:
+      pinStatus[pin] = mode;
+      pinMode(pin, INPUT);
+      portStatus[port] = portStatus[port] &~ (1 << (pin - offset));
+      break;
+    case OUTPUT:
+      digitalWrite(pin, LOW); // disable PWM and fall thru to 'case PWM:' 
+    case PWM:
+      pinStatus[pin] = mode;
+      pinMode(pin, OUTPUT);
+      portStatus[port] = portStatus[port] | (1 << (pin - offset));
+      break;
+    case SERVO:
+      if((pin == 9 || pin == 10))
         pinStatus[pin] = mode;
-        switch(mode) {
-        case INPUT:
-            pinMode(pin, INPUT);
-            portStatus[port] = portStatus[port] &~ (1 << (pin - offset));
-            break;
-        case OUTPUT:
-            digitalWrite(pin, LOW); // disable PWM
-        case PWM:
-            pinMode(pin, OUTPUT);
-            portStatus[port] = portStatus[port] | (1 << (pin - offset));
-            break;
-        //case ANALOG: // TODO figure this out
-        default:
-            Firmata.sendString("");
-        }
-        // TODO: save status to EEPROM here, if changed
+      else
+        Firmata.sendString("Servo only on pins 9 and 10");
+      break;
+    case I2C:
+      pinStatus[pin] = mode;
+      Firmata.sendString("I2C mode not yet supported");
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
     }
+    // TODO: save status to EEPROM here, if changed
+  }
 }
 
 void analogWriteCallback(byte pin, int value)
 {
-    setPinModeCallback(pin,PWM);
+  switch(pinStatus[pin]) {
+  case SERVO:
+    if(pin == 9)  servos[0].write(value);
+    if(pin == 10) servos[1].write(value);
+    break;
+  case PWM:
     analogWrite(pin, value);
+    break;
+  }
 }
 
 void digitalWriteCallback(byte port, int value)
 {
-    switch(port) {
-    case 0: // pins 2-7 (don't change Rx/Tx, pins 0 and 1)
-        // 0xFF03 == B1111111100000011    0x03 == B00000011
-        PORTD = (value &~ 0xFF03) | (PORTD & 0x03);
-        break;
-    case 1: // pins 8-13 (14,15 are disabled for the crystal) 
-        PORTB = (byte)value;
-        break;
-    case 2: // analog pins used as digital
-        PORTC = (byte)value;
-        break;
-    }
+  switch(port) {
+  case 0: // pins 2-7 (don't change Rx/Tx, pins 0 and 1)
+    // 0xFF03 == B1111111100000011    0x03 == B00000011
+    PORTD = (value &~ 0xFF03) | (PORTD & 0x03);
+    break;
+  case 1: // pins 8-13 (14,15 are disabled for the crystal) 
+    PORTB = (byte)value;
+    break;
+  case 2: // analog pins used as digital
+    byte pin;
+    byte pinModeMask;
+    for(pin=0; pin<8; pin++)
+      if(pinStatus[pin] == OUTPUT)
+        pinModeMask += 1 << pin;
+    PORTC = (byte)value & pinModeMask;
+    break;
+  }
 }
 
 // -----------------------------------------------------------------------------
@@ -136,64 +174,93 @@ void digitalWriteCallback(byte port, int value)
 //}
 void reportAnalogCallback(byte pin, int value)
 {
-    if(value == 0) {
-        analogInputsToReport = analogInputsToReport &~ (1 << pin);
-    }
-    else { // everything but 0 enables reporting of that pin
-        analogInputsToReport = analogInputsToReport | (1 << pin);
-    }
-    // TODO: save status to EEPROM here, if changed
+  if(value == 0) {
+    analogInputsToReport = analogInputsToReport &~ (1 << pin);
+  }
+  else { // everything but 0 enables reporting of that pin
+    analogInputsToReport = analogInputsToReport | (1 << pin);
+    setPinModeCallback(pin, ANALOG);
+  }
+  // TODO: save status to EEPROM here, if changed
 }
 
 void reportDigitalCallback(byte port, int value)
 {
-    reportPINs[port] = (byte)value;
-    if(port == ANALOG_PORT) // turn off analog reporting when used as digital
-        analogInputsToReport = 0;
+  reportPINs[port] = (byte)value;
+  if(port == ANALOG_PORT) // turn off analog reporting when used as digital
+    analogInputsToReport = 0;
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  switch(command) {
+  case SERVO_CONFIG:
+    if(argc > 4) {
+      // these vars are here for clarity, they'll optimized away by the compiler
+      byte pin = argv[0] - 9; // servos are pins 9 and 10, so offset for array
+      int minPulse = argv[1] + (argv[2] << 7);
+      int maxPulse = argv[3] + (argv[4] << 7);
+      servos[pin].attach(argv[0], minPulse, maxPulse);
+      // TODO does the Servo have to be detach()ed before reconfiguring?
+      setPinModeCallback(pin, SERVO);
+    }
+    break;
+  case SAMPLING_INTERVAL:
+    if (argc > 1)
+      samplingInterval = argv[0] + (argv[1] << 7);
+    else
+      Firmata.sendString("Not enough data");
+    break;
+  }
 }
 
+
 /*==============================================================================
  * SETUP()
  *============================================================================*/
 void setup() 
 {
-    byte i;
+  byte i;
 
-    Firmata.setFirmwareVersion(2, 0);
+  Firmata.setFirmwareVersion(2, 1);
 
-    Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
-    Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
-    Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
-    Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
-    Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
 
-    portStatus[0] = B00000011;  // ignore Tx/RX pins
-    portStatus[1] = B11000000;  // ignore 14/15 pins 
-    portStatus[2] = B00000000;
+  portStatus[0] = B00000011;  // ignore Tx/RX pins
+  portStatus[1] = B11000000;  // ignore 14/15 pins 
+  portStatus[2] = B00000000;
 
-//    for(i=0; i<TOTAL_DIGITAL_PINS; ++i) { // TODO make this work with analogs
-    for(i=0; i<14; ++i) {
-        setPinModeCallback(i,OUTPUT);
-    }
-    // set all outputs to 0 to make sure internal pull-up resistors are off
-    PORTB = 0; // pins 8-15
-    PORTC = 0; // analog port
-    PORTD = 0; // pins 0-7
-
-    // TODO rethink the init, perhaps it should report analog on default
-    for(i=0; i<TOTAL_PORTS; ++i) {
-        reportPINs[i] = false;
-    }
-    // TODO: load state from EEPROM here
+  for(i=0; i<TOTAL_DIGITAL_PINS; ++i) { // TODO make this work with analogs
+    setPinModeCallback(i,OUTPUT);
+  }
+  // set all outputs to 0 to make sure internal pull-up resistors are off
+  PORTB = 0; // pins 8-15
+  PORTC = 0; // analog port
+  PORTD = 0; // pins 0-7
+
+  // TODO rethink the init, perhaps it should report analog on default
+  for(i=0; i<TOTAL_PORTS; ++i) {
+    reportPINs[i] = false;
+  }
+  // TODO: load state from EEPROM here
 
-    /* send digital inputs here, if enabled, to set the initial state on the
-     * host computer, since once in the loop(), this firmware will only send
-     * digital data on change. */
-    if(reportPINs[0]) outputPort(0, PIND &~ B00000011); // ignore Rx/Tx 0/1
-    if(reportPINs[1]) outputPort(1, PINB);
-    if(reportPINs[ANALOG_PORT]) outputPort(ANALOG_PORT, PINC);
+  /* send digital inputs here, if enabled, to set the initial state on the
+   * host computer, since once in the loop(), this firmware will only send
+   * digital data on change. */
+  if(reportPINs[0]) outputPort(0, PIND &~ B00000011); // ignore Rx/Tx 0/1
+  if(reportPINs[1]) outputPort(1, PINB);
+  if(reportPINs[ANALOG_PORT]) outputPort(ANALOG_PORT, PINC);
 
-    Firmata.begin();
+  Firmata.begin(57600);
 }
 
 /*==============================================================================
@@ -201,26 +268,26 @@ void setup()
  *============================================================================*/
 void loop() 
 {
-/* DIGITALREAD - as fast as possible, check for changes and output them to the
- * FTDI buffer using Serial.print()  */
-    checkDigitalInputs();  
-    currentMillis = millis();
-    if(currentMillis > nextExecuteMillis) {  
-        nextExecuteMillis = currentMillis + 19; // run this every 20ms
-        /* SERIALREAD - Serial.read() uses a 128 byte circular buffer, so handle
-         * all serialReads at once, i.e. empty the buffer */
-        while(Firmata.available())
-            Firmata.processInput();
-        /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
-         * 60 bytes. use a timer to sending an event character every 4 ms to
-         * trigger the buffer to dump. */
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();  
+  currentMillis = millis();
+  if(currentMillis > nextExecuteMillis) {  
+    nextExecuteMillis = currentMillis + samplingInterval;
+    /* SERIALREAD - Serial.read() uses a 128 byte circular buffer, so handle
+     * all serialReads at once, i.e. empty the buffer */
+    while(Firmata.available())
+      Firmata.processInput();
+    /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
+     * 60 bytes. use a timer to sending an event character every 4 ms to
+     * trigger the buffer to dump. */
 	
-        /* ANALOGREAD - right after the event character, do all of the
-         * analogReads().  These only need to be done every 4ms. */
-        for(analogPin=0;analogPin<TOTAL_ANALOG_PINS;analogPin++) {
-            if( analogInputsToReport & (1 << analogPin) ) {
-                Firmata.sendAnalog(analogPin, analogRead(analogPin));
-            }
-        }
+    /* ANALOGREAD - right after the event character, do all of the
+     * analogReads().  These only need to be done every 4ms. */
+    for(analogPin=0;analogPin<TOTAL_ANALOG_PINS;analogPin++) {
+      if( analogInputsToReport & (1 << analogPin) ) {
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
     }
+  }
 }
diff --git a/libraries/LiquidCrystal/LiquidCrystal.cpp b/libraries/LiquidCrystal/LiquidCrystal.cpp
index b5f2cd4..b66f107 100755
--- a/libraries/LiquidCrystal/LiquidCrystal.cpp
+++ b/libraries/LiquidCrystal/LiquidCrystal.cpp
@@ -25,10 +25,39 @@
 // LiquidCrystal constructor is called).
 
 LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
-  uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
-  uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7) :
-  _four_bit_mode(0), _rs_pin(rs), _rw_pin(rw), _enable_pin(enable)
+			     uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+			     uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)
 {
+  init(0, rs, rw, enable, d0, d1, d2, d3, d4, d5, d6, d7);
+}
+
+LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t enable,
+			     uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+			     uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)
+{
+  init(0, rs, -1, enable, d0, d1, d2, d3, d4, d5, d6, d7);
+}
+
+LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
+			     uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3)
+{
+  init(1, rs, rw, enable, d0, d1, d2, d3, 0, 0, 0, 0);
+}
+
+LiquidCrystal::LiquidCrystal(uint8_t rs,  uint8_t enable,
+			     uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3)
+{
+  init(1, rs, -1, enable, d0, d1, d2, d3, 0, 0, 0, 0);
+}
+
+void LiquidCrystal::init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable,
+			 uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+			 uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7)
+{
+  _rs_pin = rs;
+  _rw_pin = rw;
+  _enable_pin = enable;
+  
   _data_pins[0] = d0;
   _data_pins[1] = d1;
   _data_pins[2] = d2;
@@ -37,92 +66,244 @@ LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
   _data_pins[5] = d5;
   _data_pins[6] = d6;
   _data_pins[7] = d7; 
-  
+
   pinMode(_rs_pin, OUTPUT);
-  pinMode(_rw_pin, OUTPUT);
+  // we can save 1 pin by not using RW. Indicate by passing -1 instead of pin#
+  if (_rw_pin != -1) { 
+    pinMode(_rw_pin, OUTPUT);
+  }
   pinMode(_enable_pin, OUTPUT);
   
-  for (int i = 0; i < 8; i++)
-    pinMode(_data_pins[i], OUTPUT);
- 
-  command(0x38);  // function set: 8 bits, 1 line, 5x8 dots
-  command(0x0C);  // display control: turn display on, cursor off, no blinking
-  command(0x06);  // entry mode set: increment automatically, display shift, right shift
-  clear();
+  if (fourbitmode)
+    _displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
+  else 
+    _displayfunction = LCD_8BITMODE | LCD_1LINE | LCD_5x8DOTS;
+  
+  begin(16, 1);  
 }
 
-LiquidCrystal::LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
-  uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3) :
-  _four_bit_mode(1), _rs_pin(rs), _rw_pin(rw), _enable_pin(enable)
-{
-  _data_pins[0] = d0;
-  _data_pins[1] = d1;
-  _data_pins[2] = d2;
-  _data_pins[3] = d3; 
-  
-  pinMode(_rs_pin, OUTPUT);
-  pinMode(_rw_pin, OUTPUT);
-  pinMode(_enable_pin, OUTPUT);
+void LiquidCrystal::begin(uint8_t cols, uint8_t lines, uint8_t dotsize) {
+  if (lines > 1) {
+    _displayfunction |= LCD_2LINE;
+  }
+  _numlines = lines;
+  _currline = 0;
+
+  // for some 1 line displays you can select a 10 pixel high font
+  if ((dotsize != 0) && (lines == 1)) {
+    _displayfunction |= LCD_5x10DOTS;
+  }
+
+  // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION!
+  // according to datasheet, we need at least 40ms after power rises above 2.7V
+  // before sending commands. Arduino can turn on way befer 4.5V so we'll wait 50
+  delayMicroseconds(50000); 
+  // Now we pull both RS and R/W low to begin commands
+  digitalWrite(_rs_pin, LOW);
+  digitalWrite(_enable_pin, LOW);
+  if (_rw_pin != -1) { 
+    digitalWrite(_rw_pin, LOW);
+  }
   
-  for (int i = 0; i < 4; i++)
-    pinMode(_data_pins[i], OUTPUT);
- 
-  command(0x28);  // function set: 4 bits, 1 line, 5x8 dots
-  command(0x0C);  // display control: turn display on, cursor off, no blinking
-  command(0x06);  // entry mode set: increment automatically, display shift, right shift
+  //put the LCD into 4 bit or 8 bit mode
+  if (! (_displayfunction & LCD_8BITMODE)) {
+    // this is according to the hitachi HD44780 datasheet
+    // figure 24, pg 46
+
+    // we start in 8bit mode, try to set 4 bit mode
+    write4bits(0x03);
+    delayMicroseconds(4500); // wait min 4.1ms
+
+    // second try
+    write4bits(0x03);
+    delayMicroseconds(4500); // wait min 4.1ms
+    
+    // third go!
+    write4bits(0x03); 
+    delayMicroseconds(150);
+
+    // finally, set to 8-bit interface
+    write4bits(0x02); 
+  } else {
+    // this is according to the hitachi HD44780 datasheet
+    // page 45 figure 23
+
+    // Send function set command sequence
+    command(LCD_FUNCTIONSET | _displayfunction);
+    delayMicroseconds(4500);  // wait more than 4.1ms
+
+    // second try
+    command(LCD_FUNCTIONSET | _displayfunction);
+    delayMicroseconds(150);
+
+    // third go
+    command(LCD_FUNCTIONSET | _displayfunction);
+  }
+
+  // finally, set # lines, font size, etc.
+  command(LCD_FUNCTIONSET | _displayfunction);  
+
+  // turn the display on with no cursor or blinking default
+  _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;  
+  display();
+
+  // clear it off
   clear();
+
+  // Initialize to default text direction (for romance languages)
+  _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT;
+  // set the entry mode
+  command(LCD_ENTRYMODESET | _displaymode);
+
 }
 
+/********** high level commands, for the user! */
 void LiquidCrystal::clear()
 {
-  command(0x01);  // clear display, set cursor position to zero
-  delayMicroseconds(2000);
+  command(LCD_CLEARDISPLAY);  // clear display, set cursor position to zero
+  delayMicroseconds(2000);  // this command takes a long time!
 }
 
 void LiquidCrystal::home()
 {
-  command(0x02);  // set cursor position to zero
-  delayMicroseconds(2000);
+  command(LCD_RETURNHOME);  // set cursor position to zero
+  delayMicroseconds(2000);  // this command takes a long time!
 }
 
-void LiquidCrystal::setCursor(int col, int row)
+void LiquidCrystal::setCursor(uint8_t col, uint8_t row)
 {
   int row_offsets[] = { 0x00, 0x40, 0x14, 0x54 };
-  command(0x80 | (col + row_offsets[row]));
+  if ( row > _numlines ) {
+    row = _numlines-1;    // we count rows starting w/0
+  }
+  
+  command(LCD_SETDDRAMADDR | (col + row_offsets[row]));
+}
+
+// Turn the display on/off (quickly)
+void LiquidCrystal::noDisplay() {
+  _displaycontrol &= ~LCD_DISPLAYON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+void LiquidCrystal::display() {
+  _displaycontrol |= LCD_DISPLAYON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+
+// Turns the underline cursor on/off
+void LiquidCrystal::noCursor() {
+  _displaycontrol &= ~LCD_CURSORON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+void LiquidCrystal::cursor() {
+  _displaycontrol |= LCD_CURSORON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+
+// Turn on and off the blinking cursor
+void LiquidCrystal::noBlink() {
+  _displaycontrol &= ~LCD_BLINKON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+void LiquidCrystal::blink() {
+  _displaycontrol |= LCD_BLINKON;
+  command(LCD_DISPLAYCONTROL | _displaycontrol);
+}
+
+// These commands scroll the display without changing the RAM
+void LiquidCrystal::scrollDisplayLeft(void) {
+  command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT);
+}
+void LiquidCrystal::scrollDisplayRight(void) {
+  command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT);
+}
+
+// This is for text that flows Left to Right
+void LiquidCrystal::leftToRight(void) {
+  _displaymode |= LCD_ENTRYLEFT;
+  command(LCD_ENTRYMODESET | _displaymode);
+}
+
+// This is for text that flows Right to Left
+void LiquidCrystal::rightToLeft(void) {
+  _displaymode &= ~LCD_ENTRYLEFT;
+  command(LCD_ENTRYMODESET | _displaymode);
+}
+
+// This will 'right justify' text from the cursor
+void LiquidCrystal::autoscroll(void) {
+  _displaymode |= LCD_ENTRYSHIFTINCREMENT;
+  command(LCD_ENTRYMODESET | _displaymode);
 }
 
-void LiquidCrystal::command(uint8_t value) {
+// This will 'left justify' text from the cursor
+void LiquidCrystal::noAutoscroll(void) {
+  _displaymode &= ~LCD_ENTRYSHIFTINCREMENT;
+  command(LCD_ENTRYMODESET | _displaymode);
+}
+
+// Allows us to fill the first 8 CGRAM locations
+// with custom characters
+void LiquidCrystal::createChar(uint8_t location, uint8_t charmap[]) {
+  location &= 0x7; // we only have 8 locations 0-7
+  command(LCD_SETCGRAMADDR | (location << 3));
+  for (int i=0; i<8; i++) {
+    write(charmap[i]);
+  }
+}
+
+/*********** mid level commands, for sending data/cmds */
+
+inline void LiquidCrystal::command(uint8_t value) {
   send(value, LOW);
 }
 
-void LiquidCrystal::write(uint8_t value) {
+inline void LiquidCrystal::write(uint8_t value) {
   send(value, HIGH);
 }
 
+/************ low level data pushing commands **********/
+
+// write either command or data, with automatic 4/8-bit selection
 void LiquidCrystal::send(uint8_t value, uint8_t mode) {
   digitalWrite(_rs_pin, mode);
-  digitalWrite(_rw_pin, LOW);
-
-  if (_four_bit_mode) {
-    for (int i = 0; i < 4; i++) {
-      digitalWrite(_data_pins[i], (value >> (i + 4)) & 0x01);
-    }
-    
-    digitalWrite(_enable_pin, HIGH);
-    digitalWrite(_enable_pin, LOW);
-    
-    for (int i = 0; i < 4; i++) {
-      digitalWrite(_data_pins[i], (value >> i) & 0x01);
-    }
 
-    digitalWrite(_enable_pin, HIGH);
-    digitalWrite(_enable_pin, LOW);
+  // if there is a RW pin indicated, set it low to Write
+  if (_rw_pin != -1) { 
+    digitalWrite(_rw_pin, LOW);
+  }
+  
+  if (_displayfunction & LCD_8BITMODE) {
+    write8bits(value); 
   } else {
-    for (int i = 0; i < 8; i++) {
-      digitalWrite(_data_pins[i], (value >> i) & 0x01);
-    }
+    write4bits(value>>4);
+    write4bits(value);
+  }
+}
+
+void LiquidCrystal::pulseEnable(void) {
+  digitalWrite(_enable_pin, LOW);
+  delayMicroseconds(1);    
+  digitalWrite(_enable_pin, HIGH);
+  delayMicroseconds(1);    // enable pulse must be >450ns
+  digitalWrite(_enable_pin, LOW);
+  delayMicroseconds(100);   // commands need > 37us to settle
+}
 
-    digitalWrite(_enable_pin, HIGH);
-    digitalWrite(_enable_pin, LOW);
+void LiquidCrystal::write4bits(uint8_t value) {
+  for (int i = 0; i < 4; i++) {
+    pinMode(_data_pins[i], OUTPUT);
+    digitalWrite(_data_pins[i], (value >> i) & 0x01);
   }
+
+  pulseEnable();
+}
+
+void LiquidCrystal::write8bits(uint8_t value) {
+  for (int i = 0; i < 8; i++) {
+    pinMode(_data_pins[i], OUTPUT);
+    digitalWrite(_data_pins[i], (value >> i) & 0x01);
+  }
+  
+  pulseEnable();
 }
diff --git a/libraries/LiquidCrystal/LiquidCrystal.h b/libraries/LiquidCrystal/LiquidCrystal.h
index a5edc5f..f66ec1b 100755
--- a/libraries/LiquidCrystal/LiquidCrystal.h
+++ b/libraries/LiquidCrystal/LiquidCrystal.h
@@ -4,28 +4,101 @@
 #include <inttypes.h>
 #include "Print.h"
 
+// commands
+#define LCD_CLEARDISPLAY 0x01
+#define LCD_RETURNHOME 0x02
+#define LCD_ENTRYMODESET 0x04
+#define LCD_DISPLAYCONTROL 0x08
+#define LCD_CURSORSHIFT 0x10
+#define LCD_FUNCTIONSET 0x20
+#define LCD_SETCGRAMADDR 0x40
+#define LCD_SETDDRAMADDR 0x80
+
+// flags for display entry mode
+#define LCD_ENTRYRIGHT 0x00
+#define LCD_ENTRYLEFT 0x02
+#define LCD_ENTRYSHIFTINCREMENT 0x01
+#define LCD_ENTRYSHIFTDECREMENT 0x00
+
+// flags for display on/off control
+#define LCD_DISPLAYON 0x04
+#define LCD_DISPLAYOFF 0x00
+#define LCD_CURSORON 0x02
+#define LCD_CURSOROFF 0x00
+#define LCD_BLINKON 0x01
+#define LCD_BLINKOFF 0x00
+
+// flags for display/cursor shift
+#define LCD_DISPLAYMOVE 0x08
+#define LCD_CURSORMOVE 0x00
+#define LCD_MOVERIGHT 0x04
+#define LCD_MOVELEFT 0x00
+
+// flags for function set
+#define LCD_8BITMODE 0x10
+#define LCD_4BITMODE 0x00
+#define LCD_2LINE 0x08
+#define LCD_1LINE 0x00
+#define LCD_5x10DOTS 0x04
+#define LCD_5x8DOTS 0x00
+
 class LiquidCrystal : public Print {
 public:
-  LiquidCrystal(uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t);
-  LiquidCrystal(uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t, uint8_t,
-    uint8_t, uint8_t, uint8_t, uint8_t);
+  LiquidCrystal(uint8_t rs, uint8_t enable,
+		uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+		uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7);
+  LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
+		uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+		uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7);
+  LiquidCrystal(uint8_t rs, uint8_t rw, uint8_t enable,
+		uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3);
+  LiquidCrystal(uint8_t rs, uint8_t enable,
+		uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3);
+
+  void init(uint8_t fourbitmode, uint8_t rs, uint8_t rw, uint8_t enable,
+	    uint8_t d0, uint8_t d1, uint8_t d2, uint8_t d3,
+	    uint8_t d4, uint8_t d5, uint8_t d6, uint8_t d7);
+    
+  void begin(uint8_t cols, uint8_t rows, uint8_t charsize = LCD_5x8DOTS);
+
   void clear();
   void home();
-  void setCursor(int, int); 
-  /*
-  void shiftDisplayLeft();
-  void shiftDisplayRight();
-  */
+
+  void noDisplay();
+  void display();
+  void noBlink();
+  void blink();
+  void noCursor();
+  void cursor();
+  void scrollDisplayLeft();
+  void scrollDisplayRight();
+  void leftToRight();
+  void rightToLeft();
+  void autoscroll();
+  void noAutoscroll();
+
+  void createChar(uint8_t, uint8_t[]);
+  void setCursor(uint8_t, uint8_t); 
   virtual void write(uint8_t);
   void command(uint8_t);
 private:
   void send(uint8_t, uint8_t);
-  
-  uint8_t _four_bit_mode;
+  void write4bits(uint8_t);
+  void write8bits(uint8_t);
+  void pulseEnable();
+
   uint8_t _rs_pin; // LOW: command.  HIGH: character.
   uint8_t _rw_pin; // LOW: write to LCD.  HIGH: read from LCD.
   uint8_t _enable_pin; // activated by a HIGH pulse.
   uint8_t _data_pins[8];
+
+  uint8_t _displayfunction;
+  uint8_t _displaycontrol;
+  uint8_t _displaymode;
+
+  uint8_t _initialized;
+
+  uint8_t _numlines,_currline;
 };
 
 #endif
diff --git a/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.pde b/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.pde
new file mode 100644
index 0000000..bf33743
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/Autoscroll/Autoscroll.pde
@@ -0,0 +1,70 @@
+/*
+  LiquidCrystal Library - Autoscroll
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch demonstrates the use of the autoscroll()
+ and noAutoscroll() functions to make new text scroll or not.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of columns and rows: 
+  lcd.begin(16,2);
+}
+
+void loop() {
+  // set the cursor to (0,0):
+  lcd.setCursor(0, 0);
+  // print from 0 to 9:
+  for (int thisChar = 0; thisChar < 10; thisChar++) {
+   lcd.print(thisChar);
+   delay(500);
+  }
+
+  // set the cursor to (16,1):
+  lcd.setCursor(16,1);
+  // set the display to automatically scroll:
+  lcd.autoscroll();
+  // print from 0 to 9:
+  for (int thisChar = 0; thisChar < 10; thisChar++) {
+    lcd.print(thisChar);
+    delay(500);
+  }
+  // turn off automatic scrolling
+  lcd.noAutoscroll();
+  
+  // clear screen for the next loop:
+  lcd.clear();
+}
+
diff --git a/libraries/LiquidCrystal/examples/Blink/Blink.pde b/libraries/LiquidCrystal/examples/Blink/Blink.pde
new file mode 100644
index 0000000..83c79b4
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/Blink/Blink.pde
@@ -0,0 +1,58 @@
+/*
+  LiquidCrystal Library - Blink
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints "Hello World!" to the LCD and makes the 
+ cursor block blink.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+   * ends to +5V and ground
+   * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ 
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  // Print a message to the LCD.
+  lcd.print("hello, world!");
+}
+
+void loop() {
+  // Turn off the blinking cursor:
+  lcd.noBlink();
+  delay(3000);
+   // Turn on the blinking cursor:
+  lcd.blink();
+  delay(3000);
+}
+
+
diff --git a/libraries/LiquidCrystal/examples/Cursor/Cursor.pde b/libraries/LiquidCrystal/examples/Cursor/Cursor.pde
new file mode 100644
index 0000000..c0273f4
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/Cursor/Cursor.pde
@@ -0,0 +1,58 @@
+/*
+  LiquidCrystal Library - Cursor
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints "Hello World!" to the LCD and
+ uses the cursor()  and noCursor() methods to turn
+ on and off the cursor.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  // Print a message to the LCD.
+  lcd.print("hello, world!");
+}
+
+void loop() {
+  // Turn off the cursor:
+  lcd.noCursor();
+  delay(500);
+   // Turn on the cursor:
+  lcd.cursor();
+  delay(500);
+}
+
diff --git a/libraries/LiquidCrystal/examples/Display/Display.pde b/libraries/LiquidCrystal/examples/Display/Display.pde
new file mode 100644
index 0000000..b430539
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/Display/Display.pde
@@ -0,0 +1,58 @@
+/*
+  LiquidCrystal Library - display() and noDisplay()
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints "Hello World!" to the LCD and uses the 
+ display() and noDisplay() functions to turn on and off
+ the display.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  // Print a message to the LCD.
+  lcd.print("hello, world!");
+}
+
+void loop() {
+  // Turn off the display:
+  lcd.noDisplay();
+  delay(500);
+   // Turn on the display:
+  lcd.display();
+  delay(500);
+}
+
diff --git a/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.pde b/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.pde
index 2f244d0..76cd746 100644
--- a/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.pde
+++ b/libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.pde
@@ -1,18 +1,56 @@
+/*
+  LiquidCrystal Library - Hello World
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints "Hello World!" to the LCD
+ and shows the time.
+ 
+  The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe
+ modified 25 July 2009
+ by David A. Mellis
+ 
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
 #include <LiquidCrystal.h>
 
-// LiquidCrystal display with:
-// rs on pin 12
-// rw on pin 11
-// enable on pin 10
-// d4, d5, d6, d7 on pins 5, 4, 3, 2
-LiquidCrystal lcd(12, 11, 10, 5, 4, 3, 2);
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
 
-void setup()
-{
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
   // Print a message to the LCD.
   lcd.print("hello, world!");
 }
 
-void loop()
-{
+void loop() {
+  // set the cursor to column 0, line 1
+  // (note: line 1 is the second row, since counting begins with 0):
+  lcd.setCursor(0, 1);
+  // print the number of seconds since reset:
+  lcd.print(millis()/1000);
 }
+
diff --git a/libraries/LiquidCrystal/examples/Scroll/Scroll.pde b/libraries/LiquidCrystal/examples/Scroll/Scroll.pde
new file mode 100644
index 0000000..9276553
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/Scroll/Scroll.pde
@@ -0,0 +1,83 @@
+/*
+  LiquidCrystal Library - scrollDisplayLeft() and scrollDisplayRight()
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints "Hello World!" to the LCD and uses the
+ scrollDisplayLeft() and scrollDisplayRight() methods to scroll
+ the text.
+ 
+  The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  //lcd.setCursor(0,7);
+  // Print a message to the LCD.
+  lcd.print("hello, world!");
+  delay(1000);
+}
+
+void loop() {
+  // scroll 13 positions (string length) to the left 
+  // to move it offscreen left:
+  for (int positionCounter = 0; positionCounter < 13; positionCounter++) {
+    // scroll one position left:
+    lcd.scrollDisplayLeft(); 
+    // wait a bit:
+    delay(150);
+  }
+
+  // scroll 29 positions (string length + display length) to the right
+  // to move it offscreen right:
+  for (int positionCounter = 0; positionCounter < 29; positionCounter++) {
+    // scroll one position right:
+    lcd.scrollDisplayRight(); 
+    // wait a bit:
+    delay(150);
+  }
+  
+    // scroll 16 positions (display length + string length) to the left
+    // to move it back to center:
+  for (int positionCounter = 0; positionCounter < 16; positionCounter++) {
+    // scroll one position left:
+    lcd.scrollDisplayLeft(); 
+    // wait a bit:
+    delay(150);
+  }
+  
+  // delay at the end of the full loop:
+  delay(1000);
+
+}
+
diff --git a/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.pde b/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.pde
index 0c4ce35..a094c24 100644
--- a/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.pde
+++ b/libraries/LiquidCrystal/examples/SerialDisplay/SerialDisplay.pde
@@ -1,19 +1,47 @@
 /*
- * Displays text sent over the serial port (e.g. from the Serial Monitor) on
- * an attached LCD.
+  LiquidCrystal Library - Serial Input
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch displays text sent over the serial port 
+ (e.g. from the Serial Monitor) on an attached LCD.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
  */
 
+// include the library code:
 #include <LiquidCrystal.h>
 
-// LiquidCrystal display with:
-// rs on pin 12
-// rw on pin 11
-// enable on pin 10
-// d4, d5, d6, d7 on pins 5, 4, 3, 2
-LiquidCrystal lcd(12, 11, 10, 5, 4, 3, 2);
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
 
-void setup()
-{
+void setup(){
+    // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  // initialize the serial communications:
   Serial.begin(9600);
 }
 
diff --git a/libraries/LiquidCrystal/examples/TextDirection/TextDirection.pde b/libraries/LiquidCrystal/examples/TextDirection/TextDirection.pde
new file mode 100644
index 0000000..725eb0d
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/TextDirection/TextDirection.pde
@@ -0,0 +1,84 @@
+  /*
+  LiquidCrystal Library - TextDirection
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch demonstrates how to use leftToRight() and rightToLeft()
+ to move the cursor.
+ 
+ The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+ modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ 
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+int thisChar = 'a';
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(16, 2);
+  // turn on the cursor:
+  lcd.cursor();
+  Serial.begin(9600);
+}
+
+void loop() {
+  // reverse directions at 'm':
+  if (thisChar == 'm') {
+    // go right for the next letter
+    lcd.rightToLeft(); 
+  }
+  // reverse again at 's':
+  if (thisChar == 's') {
+    // go left for the next letter
+    lcd.leftToRight(); 
+  }
+  // reset at 'z':
+  if (thisChar > 'z') {
+    // go to (0,0):
+    lcd.home(); 
+    // start again at 0
+    thisChar = 'a';
+  }
+  // print the character
+  lcd.print(thisChar, BYTE);
+  // wait a second:
+  delay(1000);
+  // increment the letter:
+  thisChar++;
+}
+
+
+
+
+
+
+
+
diff --git a/libraries/LiquidCrystal/examples/setCursor/setCursor.pde b/libraries/LiquidCrystal/examples/setCursor/setCursor.pde
new file mode 100644
index 0000000..edd2e77
--- /dev/null
+++ b/libraries/LiquidCrystal/examples/setCursor/setCursor.pde
@@ -0,0 +1,68 @@
+/*
+  LiquidCrystal Library - setCursor
+ 
+ Demonstrates the use a 16x2 LCD display.  The LiquidCrystal
+ library works with all LCD displays that are compatible with the 
+ Hitachi HD44780 driver. There are many of them out there, and you
+ can usually tell them by the 16-pin interface.
+ 
+ This sketch prints to all the positions of the LCD using the
+ setCursor(0 method:
+ 
+  The circuit:
+ * LCD RS pin to digital pin 12
+ * LCD Enable pin to digital pin 11
+ * LCD D4 pin to digital pin 5
+ * LCD D5 pin to digital pin 4
+ * LCD D6 pin to digital pin 3
+ * LCD D7 pin to digital pin 2
+ * 10K resistor:
+ * ends to +5V and ground
+ * wiper to LCD VO pin (pin 3)
+ 
+ Library originally added 18 Apr 2008
+ by David A. Mellis
+ library modified 5 Jul 2009
+ by Limor Fried (http://www.ladyada.net)
+ example added 9 Jul 2009
+ by Tom Igoe 
+  modified 25 July 2009
+ by David A. Mellis
+ 
+ http://www.arduino.cc/en/Tutorial/LiquidCrystal
+ */
+
+// include the library code:
+#include <LiquidCrystal.h>
+
+// these constants won't change.  But you can change the size of
+// your LCD using them:
+const int numRows = 2;
+const int numCols = 16;
+
+// initialize the library with the numbers of the interface pins
+LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
+
+void setup() {
+  // set up the LCD's number of rows and columns: 
+  lcd.begin(numRows, numCols);
+}
+
+void loop() {
+  // loop from ASCII 'a' to ASCII 'z':
+  for (int thisLetter = 'a'; thisLetter <= 'z'; thisLetter++) {
+    // loop over the columns:
+    for (int thisCol = 0; thisCol < numRows; thisCol++) {
+      // loop over the rows:
+      for (int thisRow = 0; thisRow < numCols; thisRow++) {
+        // set the cursor position:
+        lcd.setCursor(thisRow,thisCol);
+        // print the letter:
+        lcd.print(thisLetter, BYTE);
+        delay(200);
+      }
+    }
+  }
+}
+
+
diff --git a/libraries/LiquidCrystal/keywords.txt b/libraries/LiquidCrystal/keywords.txt
index 367ab1f..132845c 100755
--- a/libraries/LiquidCrystal/keywords.txt
+++ b/libraries/LiquidCrystal/keywords.txt
@@ -12,10 +12,24 @@ LiquidCrystal	KEYWORD1
 # Methods and Functions (KEYWORD2)
 #######################################
 
+begin	KEYWORD2
 clear	KEYWORD2
 home	KEYWORD2
 print	KEYWORD2
 setCursor	KEYWORD2
+cursor	KEYWORD2
+noCursor	KEYWORD2
+blink	KEYWORD2
+noBlink	KEYWORD2
+display	KEYWORD2
+noDisplay	KEYWORD2
+autoscroll	KEYWORD2
+noAutoscroll	KEYWORD2
+leftToRight	KEYWORD2
+rightToLeft	KEYWORD2
+scrollDisplayLeft	KEYWORD2
+scrollDisplayRight	KEYWORD2
+createChar	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
diff --git a/libraries/Servo/Servo.cpp b/libraries/Servo/Servo.cpp
index 8578fef..9f58d64 100755
--- a/libraries/Servo/Servo.cpp
+++ b/libraries/Servo/Servo.cpp
@@ -1,133 +1,268 @@
-#include <avr/interrupt.h>
-#include <wiring.h>
-#include <Servo.h>
-
-/*
-  Servo.h - Hardware Servo Timer Library
-  Author: Jim Studt, jim@federated.com
-  Copyright (c) 2007 David A. Mellis.  All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-*/
-
-
-uint8_t Servo::attached9 = 0;
-uint8_t Servo::attached10 = 0;
-
-void Servo::seizeTimer1()
-{
-  uint8_t oldSREG = SREG;
-
-  cli();
-  TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
-  TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
-  | _BV(CS11) /* div 8 clock prescaler */
-  ;
-  OCR1A = 3000;
-  OCR1B = 3000;
-  ICR1 = clockCyclesPerMicrosecond()*(20000L/8);  // 20000 uS is a bit fast for the refresh, 20ms, but 
-                                                  // it keeps us from overflowing ICR1 at 20MHz clocks
-                                                  // That "/8" at the end is the prescaler.
-#if defined(__AVR_ATmega8__)
-  TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
-#else
-  TIMSK1 &=  ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
-#endif
-
-  SREG = oldSREG;  // undo cli()    
-}
-
-void Servo::releaseTimer1() {}
-
-#define NO_ANGLE (0xff)
-
-Servo::Servo() : pin(0), angle(NO_ANGLE) {}
-
-uint8_t Servo::attach(int pinArg)
-{
-  return attach(pinArg, 544, 2400);
-}
-
-uint8_t Servo::attach(int pinArg, int min, int max)
-{
-  if (pinArg != 9 && pinArg != 10) return 0;
-  
-  min16 = min / 16;
-  max16 = max / 16;
-
-  pin = pinArg;
-  angle = NO_ANGLE;
-  digitalWrite(pin, LOW);
-  pinMode(pin, OUTPUT);
-
-  if (!attached9 && !attached10) seizeTimer1();
-
-  if (pin == 9) {
-    attached9 = 1;
-    TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1);
-  }
-  
-  if (pin == 10) {
-    attached10 = 1;
-    TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1);
-  }
-  return 1;
-}
-
-void Servo::detach()
-{
-  // muck with timer flags
-  if (pin == 9) {
-    attached9 = 0;
-    TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1);
-    pinMode(pin, INPUT);
-  } 
-  
-  if (pin == 10) {
-    attached10 = 0;
-    TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1);
-    pinMode(pin, INPUT);
-  }
-
-  if (!attached9 && !attached10) releaseTimer1();
-}
-
-void Servo::write(int angleArg)
-{
-  uint16_t p;
-
-  if (angleArg < 0) angleArg = 0;
-  if (angleArg > 180) angleArg = 180;
-  angle = angleArg;
-
-  // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
-  // That 8L on the end is the TCNT1 prescaler, it will need to change if the clock's prescaler changes,
-  // but then there will likely be an overflow problem, so it will have to be handled by a human.
-  p = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/8L;
-  if (pin == 9) OCR1A = p;
-  if (pin == 10) OCR1B = p;
-}
-
-uint8_t Servo::read()
-{
-  return angle;
-}
-
-uint8_t Servo::attached()
-{
-  if (pin == 9 && attached9) return 1;
-  if (pin == 10 && attached10) return 1;
-  return 0;
-}
+/*

+ Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2

+ Copyright (c) 2009 Michael Margolis.  All right reserved.

+ 

+ This library is free software; you can redistribute it and/or

+ modify it under the terms of the GNU Lesser General Public

+ License as published by the Free Software Foundation; either

+ version 2.1 of the License, or (at your option) any later version.

+ 

+ This library is distributed in the hope that it will be useful,

+ but WITHOUT ANY WARRANTY; without even the implied warranty of

+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

+ Lesser General Public License for more details.

+ 

+ You should have received a copy of the GNU Lesser General Public

+ License along with this library; if not, write to the Free Software

+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

+ */

+

+/* 

+ 

+ A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.

+ The servos are pulsed in the background using the value most recently written using the write() method

+ 

+ Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.

+ Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.

+ 

+ The methods are:

+ 

+ Servo - Class for manipulating servo motors connected to Arduino pins.

+ 

+ attach(pin )  - Attaches a servo motor to an i/o pin.

+ attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds

+ default min is 544, max is 2400  

+ 

+ write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)

+ writeMicroseconds() - Sets the servo pulse width in microseconds 

+ read()      - Gets the last written servo pulse width as an angle between 0 and 180. 

+ readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)

+ attached()  - Returns true if there is a servo attached. 

+ detach()    - Stops an attached servos from pulsing its i/o pin. 

+ 

+*/

+

+#include <avr/interrupt.h>

+#include <WProgram.h> 

+

+

+#include "Servo.h"

+

+#define TICKS_PER_uS     (clockCyclesPerMicrosecond() / 8)  // number of timer ticks per microsecond with prescale of 8

+

+#define SERVOS_PER_TIMER   12                               // the maximum number of servos controlled by one timer 

+#define TRIM_DURATION     (SERVOS_PER_TIMER/2)              // compensation ticks to trim adjust for digitalWrite delays 

+

+#define NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)

+

+static servo_t servos[MAX_SERVOS];                         // static array of servo structures

+static volatile int8_t Channel[NBR_TIMERS];                // counter for the servo being pulsed for each timer (or -1 if refresh interval)

+#if defined(__AVR_ATmega1280__)

+typedef enum { _timer5, _timer1, _timer3, _timer4 } servoTimer_t; // this is the sequence for timer utilization on mega

+#else

+typedef enum { _timer1 } servoTimer_t;                     // this is the sequence for timer utilization on other controllers 

+#endif

+

+uint8_t ServoCount = 0;                                     // the total number of attached servos

+

+// convenience macros

+#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((servoTimer_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo

+#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer

+#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel

+#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel

+

+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo

+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo 

+

+/************ static functions common to all instances ***********************/

+

+static inline void handle_interrupts(servoTimer_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)

+{

+  if( Channel[timer] < 0 )

+    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer 

+  else{

+    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )  

+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated   

+  }

+

+  Channel[timer]++;    // increment to the next channel

+  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {

+    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;

+    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated

+      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high   

+  }  

+  else { 

+    // finished all channels so wait for the refresh period to expire before starting over 

+    if( (unsigned)*TCNTn < (((unsigned int)REFRESH_INTERVAL * TICKS_PER_uS) + 4) )  // allow a few ticks to ensure the next OCR1A not missed

+      *OCRnA = (unsigned int)REFRESH_INTERVAL * TICKS_PER_uS;  

+    else 

+      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed

+    Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel

+  }

+}

+

+SIGNAL (TIMER1_COMPA_vect) 

+{ 

+  handle_interrupts(_timer1, &TCNT1, &OCR1A); 

+}

+

+#if defined(__AVR_ATmega1280__)

+SIGNAL (TIMER3_COMPA_vect) 

+{ 

+  handle_interrupts(_timer3, &TCNT3, &OCR3A); 

+}

+SIGNAL (TIMER4_COMPA_vect) 

+{

+  handle_interrupts(_timer4, &TCNT4, &OCR4A); 

+}

+SIGNAL (TIMER5_COMPA_vect) 

+{

+  handle_interrupts(_timer5, &TCNT5, &OCR5A); 

+}

+#endif

+

+static void initISR(servoTimer_t timer)

+{  

+  if(timer == _timer1) {

+    TCCR1A = 0;             // normal counting mode 

+    TCCR1B = _BV(CS11);     // set prescaler of 8 

+    TCNT1 = 0;              // clear the timer count 

+#if defined(__AVR_ATmega8__)

+    TIFR |= _BV(OCF1A);      // clear any pending interrupts; 

+    TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt    

+#else

+    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 

+    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 

+#endif

+  } 

+#if defined(__AVR_ATmega1280__)

+  else if(timer == _timer3) {

+    TCCR3A = 0;             // normal counting mode 

+    TCCR3B = _BV(CS31);     // set prescaler of 8  

+    TCNT3 = 0;              // clear the timer count 

+    TIFR3 = _BV(OCF3A);     // clear any pending interrupts; 

+    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt      

+  }

+  else if(timer == _timer4) {

+    TCCR4A = 0;             // normal counting mode 

+    TCCR4B = _BV(CS41);     // set prescaler of 8  

+    TCNT4 = 0;              // clear the timer count 

+    TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 

+    TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt      

+  }

+  else if(timer == _timer5) {

+    TCCR5A = 0;             // normal counting mode 

+    TCCR5B = _BV(CS51);     // set prescaler of 8  

+    TCNT5 = 0;              // clear the timer count 

+    TIFR5 = _BV(OCF5A);     // clear any pending interrupts; 

+    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      

+  }

+#endif

+} 

+

+static boolean isTimerActive(servoTimer_t timer)

+{

+  // returns true if any servo is active on this timer

+  for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {

+    if(SERVO(timer,channel).Pin.isActive == true)

+      return true;

+  }

+  return false;

+}

+

+

+/****************** end of static functions ******************************/

+

+Servo::Servo()

+{

+  if( ServoCount < MAX_SERVOS) {

+    this->servoIndex = ServoCount++;                    // assign a servo index to this instance

+    servos[this->servoIndex].ticks = DEFAULT_PULSE_WIDTH * TICKS_PER_uS;   // store default values  

+  }

+  else

+    this->servoIndex = INVALID_SERVO ;  // too many servos 

+}

+

+uint8_t Servo::attach(int pin)

+{

+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);

+}

+

+uint8_t Servo::attach(int pin, int min, int max)

+{

+  if(this->servoIndex < MAX_SERVOS ) {

+    pinMode( pin, OUTPUT) ;                                   // set servo pin to output

+    servos[this->servoIndex].Pin.nbr = pin;  

+    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 

+    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS

+    this->max  = (MAX_PULSE_WIDTH - max)/4; 

+    // initialize the timer if it has not already been initialized 

+    servoTimer_t timer = SERVO_INDEX_TO_TIMER(servoIndex);

+    if(isTimerActive(timer) == false)

+      initISR(timer);    

+    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive

+  } 

+  return this->servoIndex ;

+}

+

+void Servo::detach()  

+{

+  servos[this->servoIndex].Pin.isActive = false;  

+

+#ifdef FREE_TIMERS

+  if(isTimerActive(SERVO_INDEX_TO_TIMER(servoIndex)) == false) {

+    ;// call to unimplimented function in wiring.c to re-init timer (set timer back to PWM mode) TODO? 

+  }

+#endif

+}

+

+void Servo::write(int value)

+{  

+  if(value < MIN_PULSE_WIDTH)

+  {  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)

+    if(value < 0) value = 0;

+    if(value > 180) value = 180;

+    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      

+  }

+  this->writeMicroseconds(value);

+}

+

+void Servo::writeMicroseconds(int value)

+{

+  // calculate and store the values for the given channel

+  byte channel = this->servoIndex;

+  if( (channel >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid

+  {  

+    if( value < SERVO_MIN() )          // ensure pulse width is valid

+      value = SERVO_MIN();

+    else if( value > SERVO_MAX() )

+      value = SERVO_MAX();   

+    

+    value = (value-TRIM_DURATION) * TICKS_PER_uS;  // convert to ticks after compensating for interrupt overhead

+    uint8_t oldSREG = SREG;

+    cli();

+    servos[channel].ticks = value;  

+    SREG = oldSREG;   

+  } 

+}

+

+int Servo::read() // return the value as degrees

+{

+  return  map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);     

+}

+

+int Servo::readMicroseconds()

+{

+  unsigned int pulsewidth;

+  if( this->servoIndex != INVALID_SERVO )

+    pulsewidth = (servos[this->servoIndex].ticks /  TICKS_PER_uS) + TRIM_DURATION ;

+  else 

+    pulsewidth  = 0;

+

+  return pulsewidth;   

+}

+

+bool Servo::attached()

+{

+  return servos[this->servoIndex].Pin.isActive ;

+}

diff --git a/libraries/Servo/Servo.h b/libraries/Servo/Servo.h
index 0b0e8db..9a25c65 100755
--- a/libraries/Servo/Servo.h
+++ b/libraries/Servo/Servo.h
@@ -1,10 +1,6 @@
-#ifndef Servo_h
-#define Servo_h
-
 /*
-  Servo.h - Hardware Servo Timer Library
-  Author: Jim Studt, jim@federated.com
-  Copyright (c) 2007 David A. Mellis.  All right reserved.
+  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
+  Copyright (c) 2009 Michael Margolis.  All right reserved.
 
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
@@ -21,32 +17,76 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
+/* 
+  
+  A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
+  The servos are pulsed in the background using the value most recently written using the write() method
+
+  Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
+  Timers are siezed as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
+
+  The methods are:
+
+   Servo - Class for manipulating servo motors connected to Arduino pins.
+
+   attach(pin )  - Attaches a servo motor to an i/o pin.
+   attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
+   default min is 544, max is 2400  
+ 
+   write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
+   writeMicroseconds() - Sets the servo pulse width in microseconds 
+   read()      - Gets the last written servo pulse width as an angle between 0 and 180. 
+   readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
+   attached()  - Returns true if there is a servo attached. 
+   detach()    - Stops an attached servos from pulsing its i/o pin. 
+ */
+
+#ifndef Servo_h
+#define Servo_h
+
 #include <inttypes.h>
 
+#define Servo_VERSION           2      // software version of this library
+
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds 
+
+#if defined(__AVR_ATmega1280__)
+#define MAX_SERVOS             48     // the maximum number of servos  (valid range is from 1 to 48)
+#else
+#define MAX_SERVOS             12     // this library supports up to 12 on a standard Arduino
+#endif
+
+#define INVALID_SERVO         255     // flag indicating an invalid servo index
+
+typedef struct  {
+  uint8_t nbr        :6 ;             // a pin number from 0 to 63
+  uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false 
+} ServoPin_t   ;  
+
+typedef struct {
+  ServoPin_t Pin;
+  unsigned int ticks;
+} servo_t;
+
 class Servo
 {
-  private:
-    uint8_t pin;
-    uint8_t angle;       // in degrees
-    uint8_t min16;       // minimum pulse, 16uS units  (default is 34)
-    uint8_t max16;       // maximum pulse, 16uS units, 0-4ms range (default is 150)
-    static void seizeTimer1();
-    static void releaseTimer1();
-    static uint8_t attached9;
-    static uint8_t attached10;
-  public:
-    Servo();
-    uint8_t attach(int);
-                             // pulse length for 0 degrees in microseconds, 544uS default
-                             // pulse length for 180 degrees in microseconds, 2400uS default
-    uint8_t attach(int, int, int);
-                             // attach to a pin, sets pinMode, returns 0 on failure, won't
-                             // position the servo until a subsequent write() happens
-                             // Only works for 9 and 10.
-    void detach();
-    void write(int);         // specify the angle in degrees, 0 to 180
-    uint8_t read();
-    uint8_t attached();
+public:
+  Servo();
+  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
+  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
+  void detach();
+  void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
+  void writeMicroseconds(int value); // Write pulse width in microseconds 
+  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+  int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
+  bool attached();                   // return true if this servo is attached, otherwise false 
+private:
+   uint8_t servoIndex;               // index into the channel data for this servo
+   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
+   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   
 };
 
-#endif
+#endif
\ No newline at end of file
diff --git a/libraries/Servo/keywords.txt b/libraries/Servo/keywords.txt
index 918c46a..ca5ba79 100755
--- a/libraries/Servo/keywords.txt
+++ b/libraries/Servo/keywords.txt
@@ -6,16 +6,18 @@
 # Datatypes (KEYWORD1)
 #######################################
 
-Servo  KEYWORD1
+Servo	KEYWORD1
 
 #######################################
 # Methods and Functions (KEYWORD2)
 #######################################
-attach KEYWORD2
-detach KEYWORD2
-write KEYWORD2
-read KEYWORD2
-attached KEYWORD2
+attach	KEYWORD2
+detach	KEYWORD2
+write	KEYWORD2
+read	KEYWORD2
+attached	KEYWORD2
+writeMicroseconds	KEYWORD2
+readMicroseconds	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)
diff --git a/libraries/Wire/utility/twi.c b/libraries/Wire/utility/twi.c
index 82a25c0..2ad2a71 100644
--- a/libraries/Wire/utility/twi.c
+++ b/libraries/Wire/utility/twi.c
@@ -87,9 +87,9 @@ void twi_init(void)
   It is 72 for a 16mhz Wiring board with 100kHz TWI */
 
   // enable twi module, acks, and twi interrupt
-	TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
 	
-	// allocate buffers
+  // allocate buffers
   twi_masterBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
   twi_txBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
   twi_rxBuffer = (uint8_t*) calloc(TWI_BUFFER_LENGTH, sizeof(uint8_t));
@@ -135,29 +135,34 @@ uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length)
 
   // initialize buffer iteration vars
   twi_masterBufferIndex = 0;
-  twi_masterBufferLength = length;
+  twi_masterBufferLength = length-1;  // This is not intuitive, read on...
+  // On receive, the previously configured ACK/NACK setting is transmitted in
+  // response to the received byte before the interrupt is signalled. 
+  // Therefor we must actually set NACK when the _next_ to last byte is
+  // received, causing that NACK to be sent in response to receiving the last
+  // expected byte of data.
 
   // build sla+w, slave device address + w bit
   twi_slarw = TW_READ;
-	twi_slarw |= address << 1;
+  twi_slarw |= address << 1;
 
   // send start condition
-	TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
 
-	// wait for read operation to complete
-	while(TWI_MRX == twi_state){
-	  continue;
-	}
+  // wait for read operation to complete
+  while(TWI_MRX == twi_state){
+    continue;
+  }
 
   if (twi_masterBufferIndex < length)
-	length = twi_masterBufferIndex;
+    length = twi_masterBufferIndex;
 
   // copy twi buffer to data
   for(i = 0; i < length; ++i){
     data[i] = twi_masterBuffer[i];
   }
 	
-	return length;
+  return length;
 }
 
 /* 
@@ -202,24 +207,24 @@ uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait
   
   // build sla+w, slave device address + w bit
   twi_slarw = TW_WRITE;
-	twi_slarw |= address << 1;
+  twi_slarw |= address << 1;
   
   // send start condition
-	TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
 
-	// wait for write operation to complete
-	while(wait && (TWI_MTX == twi_state)){
-	  continue;
-	}
-	
-	if (twi_error == 0xFF)
-		return 0;	// success
-	else if (twi_error == TW_MT_SLA_NACK)
-		return 2;	// error: address send, nack received
-	else if (twi_error == TW_MT_DATA_NACK)
-		return 3;	// error: data send, nack received
-	else
-		return 4;	// other twi error
+  // wait for write operation to complete
+  while(wait && (TWI_MTX == twi_state)){
+    continue;
+  }
+  
+  if (twi_error == 0xFF)
+    return 0;	// success
+  else if (twi_error == TW_MT_SLA_NACK)
+    return 2;	// error: address send, nack received
+  else if (twi_error == TW_MT_DATA_NACK)
+    return 3;	// error: data send, nack received
+  else
+    return 4;	// other twi error
 }
 
 /* 
@@ -285,9 +290,9 @@ void twi_attachSlaveTxEvent( void (*function)(void) )
  */
 void twi_reply(uint8_t ack)
 {
-	// transmit master read ready signal, with or without ack
-	if(ack){
-	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
+  // transmit master read ready signal, with or without ack
+  if(ack){
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
   }else{
 	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
   }