Merged 1.0.5

75 files changed, 11159 insertions, 0 deletions

diff --git a/libraries/Robot_Control/Adafruit_GFX.cpp b/libraries/Robot_Control/Adafruit_GFX.cpp
new file mode 100644
index 0000000..acfed1d
--- /dev/null
+++ b/libraries/Robot_Control/Adafruit_GFX.cpp
@@ -0,0 +1,688 @@
+/******************************************************************
+ This is the core graphics library for all our displays, providing
+ basic graphics primitives (points, lines, circles, etc.). It needs
+ to be paired with a hardware-specific library for each display
+ device we carry (handling the lower-level functions).
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+#include "Adafruit_GFX.h"
+#include "glcdfont.c"
+#include <avr/pgmspace.h>
+
+void Adafruit_GFX::constructor(int16_t w, int16_t h) {
+  _width = WIDTH = w;
+  _height = HEIGHT = h;
+
+  rotation = 0;    
+  cursor_y = cursor_x = 0;
+  textsize = 1;
+  textcolor = textbgcolor = 0xFFFF;
+  wrap = true;
+  
+  strokeColor = 0;
+  useStroke = true;
+  fillColor = 0;
+  useFill = false;
+
+}
+
+
+// draw a circle outline
+void Adafruit_GFX::drawCircle(int16_t x0, int16_t y0, int16_t r, 
+			      uint16_t color) {
+  int16_t f = 1 - r;
+  int16_t ddF_x = 1;
+  int16_t ddF_y = -2 * r;
+  int16_t x = 0;
+  int16_t y = r;
+
+  drawPixel(x0, y0+r, color);
+  drawPixel(x0, y0-r, color);
+  drawPixel(x0+r, y0, color);
+  drawPixel(x0-r, y0, color);
+
+  while (x<y) {
+    if (f >= 0) {
+      y--;
+      ddF_y += 2;
+      f += ddF_y;
+    }
+    x++;
+    ddF_x += 2;
+    f += ddF_x;
+  
+    drawPixel(x0 + x, y0 + y, color);
+    drawPixel(x0 - x, y0 + y, color);
+    drawPixel(x0 + x, y0 - y, color);
+    drawPixel(x0 - x, y0 - y, color);
+    drawPixel(x0 + y, y0 + x, color);
+    drawPixel(x0 - y, y0 + x, color);
+    drawPixel(x0 + y, y0 - x, color);
+    drawPixel(x0 - y, y0 - x, color);
+    
+  }
+}
+
+void Adafruit_GFX::drawCircleHelper( int16_t x0, int16_t y0,
+               int16_t r, uint8_t cornername, uint16_t color) {
+  int16_t f     = 1 - r;
+  int16_t ddF_x = 1;
+  int16_t ddF_y = -2 * r;
+  int16_t x     = 0;
+  int16_t y     = r;
+
+  while (x<y) {
+    if (f >= 0) {
+      y--;
+      ddF_y += 2;
+      f     += ddF_y;
+    }
+    x++;
+    ddF_x += 2;
+    f     += ddF_x;
+    if (cornername & 0x4) {
+      drawPixel(x0 + x, y0 + y, color);
+      drawPixel(x0 + y, y0 + x, color);
+    } 
+    if (cornername & 0x2) {
+      drawPixel(x0 + x, y0 - y, color);
+      drawPixel(x0 + y, y0 - x, color);
+    }
+    if (cornername & 0x8) {
+      drawPixel(x0 - y, y0 + x, color);
+      drawPixel(x0 - x, y0 + y, color);
+    }
+    if (cornername & 0x1) {
+      drawPixel(x0 - y, y0 - x, color);
+      drawPixel(x0 - x, y0 - y, color);
+    }
+  }
+}
+
+void Adafruit_GFX::fillCircle(int16_t x0, int16_t y0, int16_t r, 
+			      uint16_t color) {
+  drawFastVLine(x0, y0-r, 2*r+1, color);
+  fillCircleHelper(x0, y0, r, 3, 0, color);
+}
+
+// used to do circles and roundrects!
+void Adafruit_GFX::fillCircleHelper(int16_t x0, int16_t y0, int16_t r,
+				    uint8_t cornername, int16_t delta, uint16_t color) {
+
+  int16_t f     = 1 - r;
+  int16_t ddF_x = 1;
+  int16_t ddF_y = -2 * r;
+  int16_t x     = 0;
+  int16_t y     = r;
+
+  while (x<y) {
+    if (f >= 0) {
+      y--;
+      ddF_y += 2;
+      f     += ddF_y;
+    }
+    x++;
+    ddF_x += 2;
+    f     += ddF_x;
+
+    if (cornername & 0x1) {
+      drawFastVLine(x0+x, y0-y, 2*y+1+delta, color);
+      drawFastVLine(x0+y, y0-x, 2*x+1+delta, color);
+    }
+    if (cornername & 0x2) {
+      drawFastVLine(x0-x, y0-y, 2*y+1+delta, color);
+      drawFastVLine(x0-y, y0-x, 2*x+1+delta, color);
+    }
+  }
+}
+
+// bresenham's algorithm - thx wikpedia
+void Adafruit_GFX::drawLine(int16_t x0, int16_t y0, 
+			    int16_t x1, int16_t y1, 
+			    uint16_t color) {
+  int16_t steep = abs(y1 - y0) > abs(x1 - x0);
+  if (steep) {
+    swap(x0, y0);
+    swap(x1, y1);
+  }
+
+  if (x0 > x1) {
+    swap(x0, x1);
+    swap(y0, y1);
+  }
+
+  int16_t dx, dy;
+  dx = x1 - x0;
+  dy = abs(y1 - y0);
+
+  int16_t err = dx / 2;
+  int16_t ystep;
+
+  if (y0 < y1) {
+    ystep = 1;
+  } else {
+    ystep = -1;
+  }
+
+  for (; x0<=x1; x0++) {
+    if (steep) {
+      drawPixel(y0, x0, color);
+    } else {
+      drawPixel(x0, y0, color);
+    }
+    err -= dy;
+    if (err < 0) {
+      y0 += ystep;
+      err += dx;
+    }
+  }
+}
+
+
+// draw a rectangle
+void Adafruit_GFX::drawRect(int16_t x, int16_t y, 
+			    int16_t w, int16_t h, 
+			    uint16_t color) {
+  drawFastHLine(x, y, w, color);
+  drawFastHLine(x, y+h-1, w, color);
+  drawFastVLine(x, y, h, color);
+  drawFastVLine(x+w-1, y, h, color);
+}
+
+void Adafruit_GFX::drawFastVLine(int16_t x, int16_t y, 
+				 int16_t h, uint16_t color) {
+  // stupidest version - update in subclasses if desired!
+  drawLine(x, y, x, y+h-1, color);
+}
+
+
+void Adafruit_GFX::drawFastHLine(int16_t x, int16_t y, 
+				 int16_t w, uint16_t color) {
+  // stupidest version - update in subclasses if desired!
+  drawLine(x, y, x+w-1, y, color);
+}
+
+void Adafruit_GFX::fillRect(int16_t x, int16_t y, int16_t w, int16_t h, 
+			    uint16_t color) {
+  // stupidest version - update in subclasses if desired!
+  for (int16_t i=x; i<x+w; i++) {
+    drawFastVLine(i, y, h, color); 
+  }
+}
+
+
+void Adafruit_GFX::fillScreen(uint16_t color) {
+  fillRect(0, 0, _width, _height, color);
+}
+
+// draw a rounded rectangle!
+void Adafruit_GFX::drawRoundRect(int16_t x, int16_t y, int16_t w,
+  int16_t h, int16_t r, uint16_t color) {
+  // smarter version
+  drawFastHLine(x+r  , y    , w-2*r, color); // Top
+  drawFastHLine(x+r  , y+h-1, w-2*r, color); // Bottom
+  drawFastVLine(  x    , y+r  , h-2*r, color); // Left
+  drawFastVLine(  x+w-1, y+r  , h-2*r, color); // Right
+  // draw four corners
+  drawCircleHelper(x+r    , y+r    , r, 1, color);
+  drawCircleHelper(x+w-r-1, y+r    , r, 2, color);
+  drawCircleHelper(x+w-r-1, y+h-r-1, r, 4, color);
+  drawCircleHelper(x+r    , y+h-r-1, r, 8, color);
+}
+
+// fill a rounded rectangle!
+void Adafruit_GFX::fillRoundRect(int16_t x, int16_t y, int16_t w,
+				 int16_t h, int16_t r, uint16_t color) {
+  // smarter version
+  fillRect(x+r, y, w-2*r, h, color);
+
+  // draw four corners
+  fillCircleHelper(x+w-r-1, y+r, r, 1, h-2*r-1, color);
+  fillCircleHelper(x+r    , y+r, r, 2, h-2*r-1, color);
+}
+
+// draw a triangle!
+void Adafruit_GFX::drawTriangle(int16_t x0, int16_t y0,
+				int16_t x1, int16_t y1, 
+				int16_t x2, int16_t y2, uint16_t color) {
+  drawLine(x0, y0, x1, y1, color);
+  drawLine(x1, y1, x2, y2, color);
+  drawLine(x2, y2, x0, y0, color);
+}
+
+// fill a triangle!
+void Adafruit_GFX::fillTriangle ( int16_t x0, int16_t y0,
+				  int16_t x1, int16_t y1, 
+				  int16_t x2, int16_t y2, uint16_t color) {
+
+  int16_t a, b, y, last;
+
+  // Sort coordinates by Y order (y2 >= y1 >= y0)
+  if (y0 > y1) {
+    swap(y0, y1); swap(x0, x1);
+  }
+  if (y1 > y2) {
+    swap(y2, y1); swap(x2, x1);
+  }
+  if (y0 > y1) {
+    swap(y0, y1); swap(x0, x1);
+  }
+
+  if(y0 == y2) { // Handle awkward all-on-same-line case as its own thing
+    a = b = x0;
+    if(x1 < a)      a = x1;
+    else if(x1 > b) b = x1;
+    if(x2 < a)      a = x2;
+    else if(x2 > b) b = x2;
+    drawFastHLine(a, y0, b-a+1, color);
+    return;
+  }
+
+  int16_t
+    dx01 = x1 - x0,
+    dy01 = y1 - y0,
+    dx02 = x2 - x0,
+    dy02 = y2 - y0,
+    dx12 = x2 - x1,
+    dy12 = y2 - y1,
+    sa   = 0,
+    sb   = 0;
+
+  // For upper part of triangle, find scanline crossings for segments
+  // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
+  // is included here (and second loop will be skipped, avoiding a /0
+  // error there), otherwise scanline y1 is skipped here and handled
+  // in the second loop...which also avoids a /0 error here if y0=y1
+  // (flat-topped triangle).
+  if(y1 == y2) last = y1;   // Include y1 scanline
+  else         last = y1-1; // Skip it
+
+  for(y=y0; y<=last; y++) {
+    a   = x0 + sa / dy01;
+    b   = x0 + sb / dy02;
+    sa += dx01;
+    sb += dx02;
+    /* longhand:
+    a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
+    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+    */
+    if(a > b) swap(a,b);
+    drawFastHLine(a, y, b-a+1, color);
+  }
+
+  // For lower part of triangle, find scanline crossings for segments
+  // 0-2 and 1-2.  This loop is skipped if y1=y2.
+  sa = dx12 * (y - y1);
+  sb = dx02 * (y - y0);
+  for(; y<=y2; y++) {
+    a   = x1 + sa / dy12;
+    b   = x0 + sb / dy02;
+    sa += dx12;
+    sb += dx02;
+    /* longhand:
+    a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
+    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
+    */
+    if(a > b) swap(a,b);
+    drawFastHLine(a, y, b-a+1, color);
+  }
+}
+
+void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, 
+			      const uint8_t *bitmap, int16_t w, int16_t h,
+			      uint16_t color) {
+
+  int16_t i, j, byteWidth = (w + 7) / 8;
+
+  for(j=0; j<h; j++) {
+    for(i=0; i<w; i++ ) {
+      if(pgm_read_byte(bitmap + j * byteWidth + i / 8) & (128 >> (i & 7))) {
+	drawPixel(x+i, y+j, color);
+      }
+    }
+  }
+}
+
+
+#if ARDUINO >= 100
+size_t Adafruit_GFX::write(uint8_t c) {
+#else
+void Adafruit_GFX::write(uint8_t c) {
+#endif
+  if (c == '\n') {
+    cursor_y += textsize*8;
+    cursor_x = 0;
+  } else if (c == '\r') {
+    // skip em
+  } else {
+    drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize);
+    cursor_x += textsize*6;
+    if (wrap && (cursor_x > (_width - textsize*6))) {
+      cursor_y += textsize*8;
+      cursor_x = 0;
+    }
+  }
+#if ARDUINO >= 100
+  return 1;
+#endif
+}
+
+// draw a character
+void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,
+			    uint16_t color, uint16_t bg, uint8_t size) {
+
+  if((x >= _width)            || // Clip right
+     (y >= _height)           || // Clip bottom
+     ((x + 5 * size - 1) < 0) || // Clip left
+     ((y + 8 * size - 1) < 0))   // Clip top
+    return;
+
+  for (int8_t i=0; i<6; i++ ) {
+    uint8_t line;
+    if (i == 5) 
+      line = 0x0;
+    else 
+      line = pgm_read_byte(font+(c*5)+i);
+    for (int8_t j = 0; j<8; j++) {
+      if (line & 0x1) {
+        if (size == 1) // default size
+          drawPixel(x+i, y+j, color);
+        else {  // big size
+          fillRect(x+(i*size), y+(j*size), size, size, color);
+        } 
+      } else if (bg != color) {
+        if (size == 1) // default size
+          drawPixel(x+i, y+j, bg);
+        else {  // big size
+          fillRect(x+i*size, y+j*size, size, size, bg);
+        } 	
+      }
+      line >>= 1;
+    }
+  }
+}
+
+void Adafruit_GFX::setCursor(int16_t x, int16_t y) {
+  cursor_x = x;
+  cursor_y = y;
+}
+
+
+void Adafruit_GFX::setTextSize(uint8_t s) {
+  textsize = (s > 0) ? s : 1;
+}
+
+
+void Adafruit_GFX::setTextColor(uint16_t c) {
+  textcolor = c;
+  textbgcolor = c; 
+  // for 'transparent' background, we'll set the bg 
+  // to the same as fg instead of using a flag
+}
+
+ void Adafruit_GFX::setTextColor(uint16_t c, uint16_t b) {
+   textcolor = c;
+   textbgcolor = b; 
+ }
+
+void Adafruit_GFX::setTextWrap(boolean w) {
+  wrap = w;
+}
+
+uint8_t Adafruit_GFX::getRotation(void) {
+  rotation %= 4;
+  return rotation;
+}
+
+void Adafruit_GFX::setRotation(uint8_t x) {
+  x %= 4;  // cant be higher than 3
+  rotation = x;
+  switch (x) {
+  case 0:
+  case 2:
+    _width = WIDTH;
+    _height = HEIGHT;
+    break;
+  case 1:
+  case 3:
+    _width = HEIGHT;
+    _height = WIDTH;
+    break;
+  }
+}
+
+void Adafruit_GFX::invertDisplay(boolean i) {
+  // do nothing, can be subclassed
+}
+
+
+// return the size of the display which depends on the rotation!
+int16_t Adafruit_GFX::width(void) { 
+  return _width; 
+}
+ 
+int16_t Adafruit_GFX::height(void) { 
+  return _height; 
+}
+
+
+
+uint16_t Adafruit_GFX::newColor(uint8_t r, uint8_t g, uint8_t b) {
+  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+}
+
+
+void Adafruit_GFX::background(uint8_t red, uint8_t green, uint8_t blue) {
+  background(newColor(red, green, blue));
+}
+
+void Adafruit_GFX::background(color c) {
+  fillScreen(c);
+}
+
+void Adafruit_GFX::stroke(uint8_t red, uint8_t green, uint8_t blue) {
+  stroke(newColor(red, green, blue));
+}
+
+void Adafruit_GFX::stroke(color c) {
+  useStroke = true;
+  strokeColor = c;
+  setTextColor(c);
+}
+
+void Adafruit_GFX::noStroke() {
+  useStroke = false;
+}
+
+void Adafruit_GFX::noFill() {
+  useFill = false;
+}
+
+void Adafruit_GFX::fill(uint8_t red, uint8_t green, uint8_t blue) {
+  fill(newColor(red, green, blue));
+}
+
+void Adafruit_GFX::fill(color c) {
+  useFill = true;
+  fillColor = c;
+}
+
+void Adafruit_GFX::text(int value, uint8_t x, uint8_t y){
+  if (!useStroke)
+    return;
+  
+  setTextWrap(false);
+  setTextColor(strokeColor);
+  setCursor(x, y);
+  print(value);
+}
+void Adafruit_GFX::text(long value, uint8_t x, uint8_t y){
+  if (!useStroke)
+    return;
+  
+  setTextWrap(false);
+  setTextColor(strokeColor);
+  setCursor(x, y);
+  print(value);
+}
+void Adafruit_GFX::text(char value, uint8_t x, uint8_t y){
+  if (!useStroke)
+    return;
+  
+  setTextWrap(false);
+  setTextColor(strokeColor);
+  setCursor(x, y);
+  print(value);
+}
+
+void Adafruit_GFX::text(const char * text, int16_t x, int16_t y) {
+  if (!useStroke)
+    return;
+  
+  setTextWrap(false);
+  setTextColor(strokeColor);
+  setCursor(x, y);
+  print(text);
+}
+
+void Adafruit_GFX::textWrap(const char * text, int16_t x, int16_t y) {
+  if (!useStroke)
+    return;
+  
+  setTextWrap(true);
+  setTextColor(strokeColor);
+  setCursor(x, y);
+  print(text);
+}
+
+
+void Adafruit_GFX::textSize(uint8_t size) {
+  setTextSize(size);
+}
+
+void Adafruit_GFX::point(int16_t x, int16_t y) {
+  if (!useStroke)
+    return;
+  
+  drawPixel(x, y, strokeColor);
+}
+
+void Adafruit_GFX::line(int16_t x1, int16_t y1, int16_t x2, int16_t y2) {
+  if (!useStroke)
+    return;
+  
+  if (x1 == x2) {
+    drawFastVLine(x1, y1, y2 - y1, strokeColor);
+  }
+  else if (y1 == y2) {
+    drawFastHLine(x1, y1, x2 - x1, strokeColor);
+  }
+  else {
+    drawLine(x1, y1, x2, y2, strokeColor);
+  }
+}
+
+void Adafruit_GFX::rect(int16_t x, int16_t y, int16_t width, int16_t height) {
+  if (useFill) {
+    fillRect(x, y, width, height, fillColor);
+  }
+  if (useStroke) {
+    drawRect(x, y, width, height, strokeColor);
+  }
+}
+
+void Adafruit_GFX::rect(int16_t x, int16_t y, int16_t width, int16_t height, int16_t radius) {
+  if (radius == 0) {
+    rect(x, y, width, height);
+  }
+  if (useFill) {
+    fillRoundRect(x, y, width, height, radius, fillColor);
+  }
+  if (useStroke) {
+    drawRoundRect(x, y, width, height, radius, strokeColor);
+  }
+}
+
+void Adafruit_GFX::circle(int16_t x, int16_t y, int16_t r) {
+  if (r == 0)
+    return;
+  
+  if (useFill) {
+    fillCircle(x, y, r, fillColor);
+  }
+  if (useStroke) {
+    drawCircle(x, y, r, strokeColor);
+  }
+}
+
+void Adafruit_GFX::triangle(int16_t x1, int16_t y1, int16_t x2, int16_t y2, int16_t x3, int16_t y3) {
+  if (useFill) {
+    fillTriangle(x1, y1, x2, y2, x3, y3, fillColor);
+  }
+  if (useStroke) {
+    drawTriangle(x1, y1, x2, y2, x3, y3, strokeColor);
+  }
+}
+
+#define BUFFPIXEL 20
+/*
+void Adafruit_GFX::image(PImage & img, uint16_t x, uint16_t y) {
+  int      w, h, row, col;
+  uint8_t  r, g, b;
+  uint32_t pos = 0;
+  uint8_t  sdbuffer[3*BUFFPIXEL]; // pixel buffer (R+G+B per pixel)
+  uint8_t  buffidx = sizeof(sdbuffer); // Current position in sdbuffer
+
+  // Crop area to be loaded
+  w = img._bmpWidth;
+  h = img._bmpHeight;
+  if((x+w-1) >= width())  w = width()  - x;
+  if((y+h-1) >= height()) h = height() - y;
+
+  
+  // Set TFT address window to clipped image bounds
+  //setAddrWindow(x, y, x+w-1, y+h-1);
+  
+  
+  for (row=0; row<h; row++) { // For each scanline...
+    // Seek to start of scan line.  It might seem labor-
+    // intensive to be doing this on every line, but this
+    // method covers a lot of gritty details like cropping
+    // and scanline padding.  Also, the seek only takes
+    // place if the file position actually needs to change
+    // (avoids a lot of cluster math in SD library).
+    if(img._flip) // Bitmap is stored bottom-to-top order (normal BMP)
+      pos = img._bmpImageoffset + (img._bmpHeight - 1 - row) * img._rowSize;
+    else     // Bitmap is stored top-to-bottom
+      pos = img._bmpImageoffset + row * img._rowSize;
+    if(img._bmpFile.position() != pos) { // Need seek?
+      img._bmpFile.seek(pos);
+      buffidx = sizeof(sdbuffer); // Force buffer reload
+    }
+
+    for (col=0; col<w; col++) { // For each pixel...
+      // Time to read more pixel data?
+      if (buffidx >= sizeof(sdbuffer)) { // Indeed
+        img._bmpFile.read(sdbuffer, sizeof(sdbuffer));
+        buffidx = 0; // Set index to beginning
+      }
+
+      // Convert pixel from BMP to TFT format, push to display
+      b = sdbuffer[buffidx++];
+      g = sdbuffer[buffidx++];
+      r = sdbuffer[buffidx++];
+      //pushColor(tft.Color565(r,g,b));
+      drawPixel(x + col, y + row, newColor(r, g, b));
+      
+    } // end pixel
+  } // end scanline
+
+}*/
diff --git a/libraries/Robot_Control/Adafruit_GFX.h b/libraries/Robot_Control/Adafruit_GFX.h
new file mode 100644
index 0000000..1f6b8d8
--- /dev/null
+++ b/libraries/Robot_Control/Adafruit_GFX.h
@@ -0,0 +1,190 @@
+/******************************************************************
+ This is the core graphics library for all our displays, providing
+ basic graphics primitives (points, lines, circles, etc.). It needs
+ to be paired with a hardware-specific library for each display
+ device we carry (handling the lower-level functions).
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+#ifndef _ADAFRUIT_GFX_H
+#define _ADAFRUIT_GFX_H
+
+#if ARDUINO >= 100
+ #include "Arduino.h"
+ #include "Print.h"
+#else
+ #include "WProgram.h"
+#endif
+
+//#include "PImage.h"
+
+#define swap(a, b) { int16_t t = a; a = b; b = t; }
+
+/* TODO
+enum RectMode {
+  CORNER,
+  CORNERS,
+  RADIUS,
+  CENTER
+};
+*/
+
+typedef uint16_t color;
+
+class Adafruit_GFX : public Print {
+ public:
+
+  //Adafruit_GFX();
+  // i have no idea why we have to formally call the constructor. kinda sux
+  void constructor(int16_t w, int16_t h);
+
+  // this must be defined by the subclass
+  virtual void drawPixel(int16_t x, int16_t y, uint16_t color);
+  virtual void invertDisplay(boolean i);
+
+  // these are 'generic' drawing functions, so we can share them!
+  virtual void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, 
+		uint16_t color);
+  virtual void drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color);
+  virtual void drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color);
+  virtual void drawRect(int16_t x, int16_t y, int16_t w, int16_t h, 
+		uint16_t color);
+  virtual void fillRect(int16_t x, int16_t y, int16_t w, int16_t h, 
+		uint16_t color);
+  virtual void fillScreen(uint16_t color);
+
+  void drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+  void drawCircleHelper(int16_t x0, int16_t y0,
+			int16_t r, uint8_t cornername, uint16_t color);
+  void fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color);
+  void fillCircleHelper(int16_t x0, int16_t y0, int16_t r,
+		      uint8_t cornername, int16_t delta, uint16_t color);
+
+  void drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
+		    int16_t x2, int16_t y2, uint16_t color);
+  void fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
+		    int16_t x2, int16_t y2, uint16_t color);
+  void drawRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h,
+		     int16_t radius, uint16_t color);
+  void fillRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h,
+		     int16_t radius, uint16_t color);
+
+  void drawBitmap(int16_t x, int16_t y, 
+		  const uint8_t *bitmap, int16_t w, int16_t h,
+		  uint16_t color);
+  void drawChar(int16_t x, int16_t y, unsigned char c,
+		uint16_t color, uint16_t bg, uint8_t size);
+#if ARDUINO >= 100
+  virtual size_t write(uint8_t);
+#else
+  virtual void   write(uint8_t);
+#endif
+  void setCursor(int16_t x, int16_t y);
+  void setTextColor(uint16_t c);
+  void setTextColor(uint16_t c, uint16_t bg);
+  void setTextSize(uint8_t s);
+  void setTextWrap(boolean w);
+
+  int16_t height(void);
+  int16_t width(void);
+
+  void setRotation(uint8_t r);
+  uint8_t getRotation(void);
+
+  
+  /*
+   * Processing-like graphics primitives
+   */
+  
+  /// transforms a color in 16-bit form given the RGB components.
+  /// The default implementation makes a 5-bit red, a 6-bit
+  /// green and a 5-bit blue (MSB to LSB). Devices that use
+  /// different scheme should override this. 
+  virtual uint16_t newColor(uint8_t red, uint8_t green, uint8_t blue);
+  
+  
+  // http://processing.org/reference/background_.html
+  void background(uint8_t red, uint8_t green, uint8_t blue);
+  void background(color c);
+
+  // http://processing.org/reference/fill_.html
+  void fill(uint8_t red, uint8_t green, uint8_t blue);
+  void fill(color c);
+
+  // http://processing.org/reference/noFill_.html
+  void noFill();
+
+  // http://processing.org/reference/stroke_.html
+  void stroke(uint8_t red, uint8_t green, uint8_t blue);
+  void stroke(color c);
+
+  // http://processing.org/reference/noStroke_.html
+  void noStroke();
+  
+  void text(const char * text, int16_t x, int16_t y);
+  void text(int value, uint8_t posX, uint8_t posY);
+  void text(long value, uint8_t posX, uint8_t posY);
+  void text(char value, uint8_t posX, uint8_t posY);
+  
+  void textWrap(const char * text, int16_t x, int16_t y);
+
+  void textSize(uint8_t size);
+  
+  // similar to ellipse() in Processing, but with
+  // a single radius.
+  // http://processing.org/reference/ellipse_.html
+  void circle(int16_t x, int16_t y, int16_t r);
+  
+  void point(int16_t x, int16_t y);
+  
+  void line(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+  
+  void quad(int16_t x1, int16_t y1, int16_t x2, int16_t y2, int16_t x3, int16_t y3, int16_t x4, int16_t y4);
+  
+  void rect(int16_t x, int16_t y, int16_t width, int16_t height);
+
+  void rect(int16_t x, int16_t y, int16_t width, int16_t height, int16_t radius);
+  
+  void triangle(int16_t x1, int16_t y1, int16_t x2, int16_t y2, int16_t x3, int16_t y3);
+  
+  /* TODO
+  void rectMode(RectMode mode);
+  
+  void pushStyle();
+  void popStyle();
+  */
+
+//  PImage loadImage(const char * fileName) { return PImage::loadImage(fileName); }
+  
+//  void image(PImage & img, uint16_t x, uint16_t y);
+  
+ protected:
+  int16_t  WIDTH, HEIGHT;   // this is the 'raw' display w/h - never changes
+  int16_t  _width, _height; // dependent on rotation
+  int16_t  cursor_x, cursor_y;
+  uint16_t textcolor, textbgcolor;
+  uint8_t  textsize;
+  uint8_t  rotation;
+  boolean  wrap; // If set, 'wrap' text at right edge of display
+  
+  /*
+   * Processing-style graphics state
+   */
+  
+  color strokeColor;
+  bool useStroke;
+  color fillColor;
+  bool useFill;
+};
+
+
+
+
+#endif
diff --git a/libraries/Robot_Control/ArduinoRobot.cpp b/libraries/Robot_Control/ArduinoRobot.cpp
new file mode 100644
index 0000000..3adac73
--- /dev/null
+++ b/libraries/Robot_Control/ArduinoRobot.cpp
@@ -0,0 +1,40 @@
+#include "ArduinoRobot.h"
+#include "Multiplexer.h"
+#include "Wire.h"
+#include "EasyTransfer2.h"
+
+//RobotControl::RobotControl(){}
+
+RobotControl::RobotControl():Arduino_LCD(LCD_CS,DC_LCD,RST_LCD){
+
+}
+
+void RobotControl::begin(){
+	Wire.begin();
+	//Compass
+	//nothing here
+		
+	//TK sensors
+	uint8_t MuxPins[]={MUXA,MUXB,MUXC,MUXD};
+	Multiplexer::begin(MuxPins,MUX_IN,4);
+	
+	//piezo
+	pinMode(BUZZ,OUTPUT);
+	
+	//communication
+	Serial1.begin(9600);
+	messageOut.begin(&Serial1);
+	messageIn.begin(&Serial1);
+	
+	//TFT initialization
+	//Arduino_LCD::initR(INITR_GREENTAB);
+}
+
+void RobotControl::setMode(uint8_t mode){
+	messageOut.writeByte(COMMAND_SWITCH_MODE);
+	messageOut.writeByte(mode);
+	messageOut.sendData();
+}
+
+
+RobotControl Robot=RobotControl();
\ No newline at end of file
diff --git a/libraries/Robot_Control/ArduinoRobot.h b/libraries/Robot_Control/ArduinoRobot.h
new file mode 100644
index 0000000..becdca8
--- /dev/null
+++ b/libraries/Robot_Control/ArduinoRobot.h
@@ -0,0 +1,360 @@
+#ifndef ArduinoRobot_h
+#define ArduinoRobot_h
+
+#include "Arduino_LCD.h" // Hardware-specific library
+//#include "FormattedText.h"
+#include "SquawkSD.h"
+#include "Multiplexer.h"
+#include "EasyTransfer2.h"
+#include "EEPROM_I2C.h"
+#include "Compass.h"
+#include "Fat16.h"
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+
+#define BUTTON_NONE   -1
+#define BUTTON_LEFT  0
+#define BUTTON_DOWN      1
+#define BUTTON_UP     2
+#define BUTTON_RIGHT    3
+#define BUTTON_MIDDLE     4
+#define NUMBER_BUTTONS 5
+
+//beep length
+#define BEEP_SIMPLE   0
+#define BEEP_DOUBLE    1
+#define BEEP_LONG    2
+
+// image locations on the EEPROM
+ #define HOME_BMP	0
+#define BATTERY_BMP	2048
+#define COMPASS_BMP	4096
+#define CONTROL_BMP	6144
+#define GEARS_BMP	8192
+#define LIGHT_BMP	10240
+#define OSCILLO_BMP	12288
+#define VOLT_BMP	14336
+#define INICIO_BMP	16384  // this is a full screen splash
+
+//Command code
+#define COMMAND_SWITCH_MODE 0
+#define COMMAND_RUN	10
+#define COMMAND_MOTORS_STOP	11
+#define COMMAND_ANALOG_WRITE	20
+#define COMMAND_DIGITAL_WRITE	30
+#define COMMAND_ANALOG_READ	40
+#define COMMAND_ANALOG_READ_RE	41
+#define COMMAND_DIGITAL_READ	50
+#define COMMAND_DIGITAL_READ_RE	51
+#define COMMAND_READ_IR	60
+#define COMMAND_READ_IR_RE	61
+#define COMMAND_ACTION_DONE 70
+#define COMMAND_READ_TRIM 80
+#define COMMAND_READ_TRIM_RE 81
+#define COMMAND_PAUSE_MODE 90
+#define COMMAND_LINE_FOLLOW_CONFIG 100
+
+//component codename
+#define CN_LEFT_MOTOR	0
+#define CN_RIGHT_MOTOR	1
+#define CN_IR 2
+
+//motor board modes
+#define MODE_SIMPLE 0
+#define MODE_LINE_FOLLOW 1
+#define MODE_ADJUST_MOTOR 2
+#define MODE_IR_CONTROL 3
+
+//port types, for R/W
+#define TYPE_TOP_TK 0
+#define TYPE_TOP_TKD 1
+#define TYPE_BOTTOM_TK 2
+
+//top TKs
+#define TK0 100
+#define TK1 101
+#define TK2 102
+#define TK3 103
+#define TK4 104
+#define TK5 105
+#define TK6 106
+#define TK7 107
+
+//bottom TKs, just for communication purpose
+#define B_TK1 201
+#define B_TK2 202
+#define B_TK3 203
+#define B_TK4 204
+
+//bottom IRs, for communication purpose
+#define B_IR0 210
+#define B_IR1 211
+#define B_IR2 212
+#define B_IR3 213
+#define B_IR4 214
+
+#ifndef LED1
+#define LED1 17
+#endif
+
+//320 - 337 username, 
+#define ADDRESS_USERNAME 320
+//338 - 355 robotname, 
+#define ADDRESS_ROBOTNAME 338
+//356 - 373 cityname, 
+#define ADDRESS_CITYNAME 356
+ //374- 391 countryname, 
+#define ADDRESS_COUNTRYNAME 374
+//508-511 robot info
+#define ADDRESS_ROBOTINFO 508
+
+#define	BLACK	ILI9163C_BLACK   
+#define	BLUE	ILI9163C_BLUE    
+#define	RED		ILI9163C_RED     
+#define	GREEN	ILI9163C_GREEN   
+#define CYAN	ILI9163C_CYAN    
+#define MAGENTA	ILI9163C_MAGENTA 
+#define YELLOW	ILI9163C_YELLOW    
+#define WHITE	ILI9163C_WHITE   
+ 
+//A data structure for storing the current state of motor board
+struct MOTOR_BOARD_DATA{
+	int _B_TK1;
+	int _B_TK2;
+	int _B_TK3;
+	int _B_TK4;
+	
+	/*int _B_IR0;
+	int _B_IR1;
+	int _B_IR2;
+	int _B_IR3;
+	int _B_IR4;*/
+};
+
+/*
+A message structure will be:
+switch mode:
+	byte COMMAND_SWITCH_MODE, byte mode
+run:
+	byte COMMAND_RUN, int speedL, int speedR
+analogWrite:
+	byte COMMAND_ANALOG_WRITE, byte codename, byte value;
+digitalWrite:
+	byte COMMAND_DIGITAL_WRITE, byte codename, byte value;
+analogRead:
+	byte COMMAND_ANALOG_READ, byte codename;
+analogRead return:
+	byte COMMAND_ANALOG_READ_RE, byte codename, int value;
+digitalRead return:
+	byte COMMAND_DIGITAL_READ_RE, byte codename, byte value;
+read IR:
+	byte COMMAND_READ_IR, int valueA, int valueB, int valueC, int valueD;
+
+
+*/
+#define NUM_EEPROM_BMP 10
+struct EEPROM_BMP{
+	char name[8];
+	uint8_t width;
+	uint8_t height;
+	uint16_t address;
+};
+
+//if you call #undef USE_SQUAWK_SYNTH_SD at the beginning of your sketch,
+//it's going to remove anything regarding sound playing
+
+class RobotControl:public Multiplexer, 
+public EEPROM_I2C, 
+public Compass, 
+public SquawkSynthSD, 
+//public FormattedText
+public Arduino_LCD
+{
+	public:
+		RobotControl();
+		void begin();
+		void setMode(uint8_t mode);
+		
+		//Read & Write, TK0 - TK7, TKD0 - TKD1, bottom TK0 - TK4
+		bool digitalRead(uint8_t port);
+		int analogRead(uint8_t port);
+		void digitalWrite(uint8_t port, bool value);
+		void analogWrite(uint8_t port, uint8_t value);//It's not available, as there's no pin can be used for analog write
+		
+		//IR sensors from the bottom board
+		//define an array as "int arr[4];", and supply the arry name here
+		uint16_t IRarray[5];
+		void updateIR();
+		
+		//on board Potentiometor
+		int knobRead();
+		//Potentiometor of the motor board
+		int trimRead();
+		
+		//on board piezo
+		void beginSpeaker(uint16_t frequency=44100);
+		void playMelody(char* script);
+		void playFile(char* filename);
+		void stopPlayFile();
+		void beep(int beep_length=BEEP_SIMPLE);
+		void tempoWrite(int tempo);
+		void tuneWrite(float tune);
+
+		//compass
+		uint16_t compassRead();
+		void drawCompass(uint16_t value);
+		void drawBase();
+		void drawDire(int16_t dire);
+		
+		//keyboard
+		void keyboardCalibrate(int *vals);
+		int8_t keyboardRead();//return the key that is being pressed?Has been pressed(with _processKeyboard)?
+		
+		//movement
+		void moveForward(int speed);
+		void moveBackward(int speed);
+		void turnLeft(int speed);
+		void turnRight(int speed);
+		void motorsStop();
+		void motorsWritePct(int speedLeftPct, int speedRightPct);
+		
+		void motorsWrite(int speedLeft,int speedRight);
+		void pointTo(int degrees);//turn to an absolute angle from the compass
+		void turn(int degress);//turn certain degrees from the current heading
+		
+		//Line Following
+		void lineFollowConfig(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime);//default 11 5 50 10
+		
+		//TFT LCD
+		//use the same commands as Arduino_LCD
+		void beginTFT(uint16_t foreGround=BLACK, uint16_t background=WHITE);
+		/*void text(int value, uint8_t posX, uint8_t posY, bool EW);
+		void text(long value, uint8_t posX, uint8_t posY, bool EW);
+		void text(char* value, uint8_t posX, uint8_t posY, bool EW);
+		void text(char value, uint8_t posX, uint8_t posY, bool EW);*/
+		void debugPrint(long value, uint8_t x=0, uint8_t y=0);
+		void clearScreen();
+		
+		void drawBMP(char* filename, uint8_t x, uint8_t y);//detect if draw with EEPROM or SD, and draw it
+		void _drawBMP(uint32_t iconOffset, uint8_t x, uint8_t y, uint8_t width, uint8_t height);//draw from EEPROM
+		void _drawBMP(char* filename, uint8_t x, uint8_t y);//draw from SD
+		void beginBMPFromEEPROM();
+		void endBMPFromEEPROM();
+		
+		uint16_t foreGround;//foreground color
+		uint16_t backGround;//background color
+
+		
+		//SD card
+		void beginSD();
+		
+		//Information
+		void userNameRead(char* container);
+		void robotNameRead(char* container);
+		void cityNameRead(char* container);
+		void countryNameRead(char* container);
+		
+		void userNameWrite(char* text);
+		void robotNameWrite(char* text);
+		void cityNameWrite(char* text);
+		void countryNameWrite(char* text);
+		
+		//Others
+		bool isActionDone();
+		void pauseMode(uint8_t onOff);
+		void displayLogos();
+		void waitContinue(uint8_t key=BUTTON_MIDDLE);
+		
+	private:
+		//Read & Write
+		uint8_t _getTypeCode(uint8_t port);//different ports need different actions
+		uint8_t _portToTopMux(uint8_t port);//get the number for multiplexer within top TKs
+		uint8_t _topDPortToAPort(uint8_t port);//get the corrensponding analogIn pin for top TKDs
+		
+		bool _digitalReadTopMux(uint8_t port);//TK0 - TK7
+		int _analogReadTopMux(uint8_t port);
+		
+		bool _digitalReadTopPin(uint8_t port);
+		int _analogReadTopPin(uint8_t port);
+		void _digitalWriteTopPin(uint8_t port, bool value);
+		
+		MOTOR_BOARD_DATA motorBoardData;
+		int* parseMBDPort(uint8_t port);
+		int get_motorBoardData(uint8_t port);
+		void set_motorBoardData(uint8_t port, int value);
+		
+		bool _requestDigitalRead(uint8_t port);
+		int _requestAnalogRead(uint8_t port);
+		void _requestDigitalWrite(uint8_t port, uint8_t value);
+				
+		//LCD
+		void _enableLCD();
+		void _setWrite(uint8_t posX, uint8_t posY);
+		void _setErase(uint8_t posX, uint8_t posY);
+		
+		
+		//SD
+		SdCard card;
+		Fat16 file;
+		Fat16 melody;
+		void _enableSD();
+		
+		//keyboard
+		void _processKeyboard(); //need to run in loop, detect if the key is actually pressed
+		int averageAnalogInput(int pinNum);
+		
+		//Ultrasonic ranger
+		//uint8_t pinTrigger_UR;
+		//uint8_t pinEcho_UR;
+		
+		//Melody
+		void playNote(byte period, word length, char modifier); 
+		
+		//Communication
+		
+		EasyTransfer2 messageOut;
+		EasyTransfer2 messageIn;
+		
+		//TFT LCD
+		bool _isEEPROM_BMP_Allocated;
+		EEPROM_BMP * _eeprom_bmp;
+		void _drawBMP_EEPROM(uint16_t address, uint8_t width, uint8_t height);
+		void _drawBMP_SD(char* filename, uint8_t x, uint8_t y);
+
+		
+};
+
+inline void RobotControl::userNameRead(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_USERNAME,(uint8_t*)container,18);
+}
+inline void RobotControl::robotNameRead(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_ROBOTNAME,(uint8_t*)container,18);
+}
+inline void RobotControl::cityNameRead(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_CITYNAME,(uint8_t*)container,18);
+}
+inline void RobotControl::countryNameRead(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_COUNTRYNAME,(uint8_t*)container,18);
+}
+
+inline void RobotControl::userNameWrite(char* text){
+	EEPROM_I2C::writePage(ADDRESS_USERNAME,(uint8_t*)text,18);
+}
+inline void RobotControl::robotNameWrite(char* text){
+	EEPROM_I2C::writePage(ADDRESS_ROBOTNAME,(uint8_t*)text,18);
+}
+inline void RobotControl::cityNameWrite(char* text){
+	EEPROM_I2C::writePage(ADDRESS_CITYNAME,(uint8_t*)text,18);
+}
+inline void RobotControl::countryNameWrite(char* text){
+	EEPROM_I2C::writePage(ADDRESS_COUNTRYNAME,(uint8_t*)text,18);
+}
+
+extern RobotControl Robot;
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/Arduino_LCD.cpp b/libraries/Robot_Control/Arduino_LCD.cpp
new file mode 100644
index 0000000..db28cd0
--- /dev/null
+++ b/libraries/Robot_Control/Arduino_LCD.cpp
@@ -0,0 +1,706 @@
+/*************************************************** 
+  This is a library for the Adafruit 1.8" SPI display.
+  This library works with the Adafruit 1.8" TFT Breakout w/SD card
+  ----> http://www.adafruit.com/products/358
+  as well as Adafruit raw 1.8" TFT display
+  ----> http://www.adafruit.com/products/618
+ 
+  Check out the links above for our tutorials and wiring diagrams
+  These displays use SPI to communicate, 4 or 5 pins are required to
+  interface (RST is optional)
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit and open-source hardware by purchasing
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+#include "Arduino_LCD.h"
+//#include <avr/pgmspace.h>
+#include <limits.h>
+//#include "pins_arduino.h"
+#include "wiring_private.h"
+#include <SPI.h>
+
+
+// Constructor when using software SPI.  All output pins are configurable.
+Arduino_LCD::Arduino_LCD(uint8_t cs, uint8_t rs, uint8_t sid,
+ uint8_t sclk, uint8_t rst) {
+  _cs   = cs;
+  _rs   = rs;
+  _sid  = sid;
+  _sclk = sclk;
+  _rst  = rst;
+  hwSPI = false;
+}
+
+
+// Constructor when using hardware SPI.  Faster, but must use SPI pins
+// specific to each board type (e.g. 11,13 for Uno, 51,52 for Mega, etc.)
+Arduino_LCD::Arduino_LCD(uint8_t cs, uint8_t rs, uint8_t rst) {
+  _cs   = cs;
+  _rs   = rs;
+  _rst  = rst;
+  hwSPI = true;
+  _sid  = _sclk = 0;
+}
+
+
+inline void Arduino_LCD::spiwrite(uint8_t c) {
+
+  //Serial.println(c, HEX);
+
+/*  if (hwSPI) {
+    SPDR = c;
+    while(!(SPSR & _BV(SPIF)));
+  } else {
+    // Fast SPI bitbang swiped from LPD8806 library
+    for(uint8_t bit = 0x80; bit; bit >>= 1) {
+      if(c & bit) *dataport |=  datapinmask;
+      else        *dataport &= ~datapinmask;
+      *clkport |=  clkpinmask;
+      *clkport &= ~clkpinmask;
+    }
+  }
+*/
+SPI.transfer(c);
+}
+
+
+void Arduino_LCD::writecommand(uint8_t c) {
+//  *rsport &= ~rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, LOW);
+digitalWrite(_cs, LOW);
+
+  //Serial.print("C ");
+  spiwrite(c);
+//SPI.transfer(c);
+//  *csport |= cspinmask;
+digitalWrite(_cs, HIGH);
+}
+
+
+void Arduino_LCD::writedata(uint8_t c) {
+//  *rsport &= ~rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+digitalWrite(_cs, LOW);
+
+  //Serial.print("D ");
+  spiwrite(c);
+//SPI.transfer(c);
+//  *csport |= cspinmask;
+digitalWrite(_cs, HIGH);
+} 
+
+
+// Rather than a bazillion writecommand() and writedata() calls, screen
+// initialization commands and arguments are organized in these tables
+// stored in PROGMEM.  The table may look bulky, but that's mostly the
+// formatting -- storage-wise this is hundreds of bytes more compact
+// than the equivalent code.  Companion function follows.
+#define DELAY 0x80
+//PROGMEM static prog_uchar
+/*uint8_t
+  Bcmd[] = {                  // Initialization commands for 7735B screens
+   18,                       // 18 commands in list:
+    ILI9163C_SWRESET,   DELAY,  //  1: Software reset, no args, w/delay
+      50,                     //     50 ms delay
+    ILI9163C_SLPOUT ,   DELAY,  //  2: Out of sleep mode, no args, w/delay
+      255,                    //     255 = 500 ms delay
+    ILI9163C_COLMOD , 1+DELAY,  //  3: Set color mode, 1 arg + delay: // I THINK THERE WAS SOMETHING HERE BECAUSE THE COMMAND IS CALLED 3A on Adafruits
+      0x05,                   //     16-bit color
+      10,                     //     10 ms delay
+    ILI9163C_FRMCTR1, 3+DELAY,  //  4: Frame rate control, 3 args + delay:
+      0x00,                   //     fastest refresh
+      0x06,                   //     6 lines front porch
+      0x03,                   //     3 lines back porch
+      10,                     //     10 ms delay
+    ILI9163C_MADCTL , 1      ,  //  5: Memory access ctrl (directions), 1 arg:
+      0x08,                   //     Row addr/col addr, bottom to top refresh
+    ILI9163C_DISSET5, 2      ,  //  6: Display settings #5, 2 args, no delay:
+      0x15,                   //     1 clk cycle nonoverlap, 2 cycle gate
+                              //     rise, 3 cycle osc equalize
+      0x02,                   //     Fix on VTL
+    ILI9163C_INVCTR , 1      ,  //  7: Display inversion control, 1 arg:
+      0x0,                    //     Line inversion
+    ILI9163C_PWCTR1 , 2+DELAY,  //  8: Power control, 2 args + delay:
+      0x02,                   //     GVDD = 4.7V
+      0x70,                   //     1.0uA
+      10,                     //     10 ms delay
+    ILI9163C_PWCTR2 , 1      ,  //  9: Power control, 1 arg, no delay:
+      0x05,                   //     VGH = 14.7V, VGL = -7.35V
+    ILI9163C_PWCTR3 , 2      ,  // 10: Power control, 2 args, no delay:
+      0x01,                   //     Opamp current small
+      0x02,                   //     Boost frequency
+    ILI9163C_VMCTR1 , 2+DELAY,  // 11: Power control, 2 args + delay:
+      0x3C,                   //     VCOMH = 4V
+      0x38,                   //     VCOML = -1.1V
+      10,                     //     10 ms delay
+    ILI9163C_PWCTR6 , 2      ,  // 12: Power control, 2 args, no delay:
+      0x11, 0x15,
+    ILI9163C_GMCTRP1,16      ,  // 13: Magical unicorn dust, 16 args, no delay:
+      0x09, 0x16, 0x09, 0x20, //     (seriously though, not sure what
+      0x21, 0x1B, 0x13, 0x19, //      these config values represent)
+      0x17, 0x15, 0x1E, 0x2B,
+      0x04, 0x05, 0x02, 0x0E,
+    ILI9163C_GMCTRN1,16+DELAY,  // 14: Sparkles and rainbows, 16 args + delay:
+      0x0B, 0x14, 0x08, 0x1E, //     (ditto)
+      0x22, 0x1D, 0x18, 0x1E,
+      0x1B, 0x1A, 0x24, 0x2B,
+      0x06, 0x06, 0x02, 0x0F,
+      10,                     //     10 ms delay
+    ILI9163C_CASET  , 4      ,  // 15: Column addr set, 4 args, no delay:
+      0x00, 0x02,             //     XSTART = 2
+      0x00, 0x81,             //     XEND = 129
+    ILI9163C_RASET  , 4      ,  // 16: Row addr set, 4 args, no delay:
+      0x00, 0x02,             //     XSTART = 1
+      0x00, 0x81,             //     XEND = 160
+    ILI9163C_NORON  ,   DELAY,  // 17: Normal display on, no args, w/delay
+      10,                     //     10 ms delay
+    ILI9163C_DISPON ,   DELAY,  // 18: Main screen turn on, no args, w/delay
+      255 },                  //     255 = 500 ms delay
+*/
+uint8_t
+  Bcmd[] = {                    // Initialization commands for 7735B screens
+   19,                          // 19 commands in list:
+    ILI9163C_SWRESET,   DELAY,  // 1: Software reset, no args, w/delay
+      50,                       //     50 ms delay
+    0x11 ,   DELAY,  		// 2: Out of sleep mode, no args, w/delay
+      100,                      //     255 = 500 ms delay
+    0x26 , 1,  			// 3: Set default gamma
+      0x04,                     //     16-bit color
+    0xb1, 2,              	// 4: Frame Rate
+      0x0b,
+      0x14,
+    0xc0, 2,                    // 5: VRH1[4:0] & VC[2:0]
+      0x08,
+      0x00,
+    0xc1, 1,                    // 6: BT[2:0]
+      0x05,
+    0xc5, 2,                    // 7: VMH[6:0] & VML[6:0]
+      0x41,
+      0x30,
+    0xc7, 1,                    // 8: LCD Driving control
+      0xc1,
+    0xEC, 1,                    // 9: Set pumping color freq
+      0x1b,
+    0x3a , 1 + DELAY,  	        // 10: Set color format
+      0x55,                     //     16-bit color
+      100,
+    0x2a, 4,                    // 11: Set Column Address
+      0x00,
+      0x00,
+      0x00,
+      0x7f,
+    0x2b, 4,                    // 12: Set Page Address
+      0x00,
+      0x00,
+      0x00,
+      0x9f,
+    0x36, 1,                    // 12+1: Set Scanning Direction
+      0xc8,
+    0xb7, 1,			// 14: Set Source Output Direciton
+      0x00,
+    0xf2, 1,			// 15: Enable Gamma bit
+      0x01,
+    0xe0, 15 + DELAY,		// 16: magic
+      0x28, 0x24, 0x22, 0x31,
+      0x2b, 0x0e, 0x53, 0xa5,
+      0x42, 0x16, 0x18, 0x12,
+      0x1a, 0x14, 0x03,
+      50,
+    0xe1, 15 + DELAY,		// 17: more magic
+      0x17, 0x1b, 0x1d, 0x0e,
+      0x14, 0x11, 0x2c, 0xa5,
+      0x3d, 0x09, 0x27, 0x2d,
+      0x25, 0x2b, 0x3c, 
+      50, 
+    ILI9163C_NORON  ,    DELAY, // 18: Normal display on, no args, w/delay
+      10,                       //     10 ms delay
+    ILI9163C_DISPON ,    DELAY, // 19: Main screen turn on, no args w/delay
+      100 },                    //     100 ms delay
+Rcmd1[] = {                   // Init for 7735R, part 1 (red or green tab)
+    15,                       // 15 commands in list:
+    ILI9163C_SWRESET,   DELAY,  //  1: Software reset, 0 args, w/delay
+      150,                    //     150 ms delay
+    ILI9163C_SLPOUT ,   DELAY,  //  2: Out of sleep mode, 0 args, w/delay
+      255,                    //     500 ms delay
+    ILI9163C_FRMCTR1, 3      ,  //  3: Frame rate ctrl - normal mode, 3 args:
+      0x01, 0x2C, 0x2D,       //     Rate = fosc/(1x2+40) * (LINE+2C+2D)
+    ILI9163C_FRMCTR2, 3      ,  //  4: Frame rate control - idle mode, 3 args:
+      0x01, 0x2C, 0x2D,       //     Rate = fosc/(1x2+40) * (LINE+2C+2D)
+    ILI9163C_FRMCTR3, 6      ,  //  5: Frame rate ctrl - partial mode, 6 args:
+      0x01, 0x2C, 0x2D,       //     Dot inversion mode
+      0x01, 0x2C, 0x2D,       //     Line inversion mode
+    ILI9163C_INVCTR , 1      ,  //  6: Display inversion ctrl, 1 arg, no delay:
+      0x07,                   //     No inversion
+    ILI9163C_PWCTR1 , 3      ,  //  7: Power control, 3 args, no delay:
+      0xA2,
+      0x02,                   //     -4.6V
+      0x84,                   //     AUTO mode
+    ILI9163C_PWCTR2 , 1      ,  //  8: Power control, 1 arg, no delay:
+      0xC5,                   //     VGH25 = 2.4C VGSEL = -10 VGH = 3 * AVDD
+    ILI9163C_PWCTR3 , 2      ,  //  9: Power control, 2 args, no delay:
+      0x0A,                   //     Opamp current small
+      0x00,                   //     Boost frequency
+    ILI9163C_PWCTR4 , 2      ,  // 10: Power control, 2 args, no delay:
+      0x8A,                   //     BCLK/2, Opamp current small & Medium low
+      0x2A,  
+    ILI9163C_PWCTR5 , 2      ,  // 11: Power control, 2 args, no delay:
+      0x8A, 0xEE,
+    ILI9163C_VMCTR1 , 1      ,  // 12: Power control, 1 arg, no delay:
+      0x0E,
+    ILI9163C_INVOFF , 0      ,  // 13: Don't invert display, no args, no delay
+    ILI9163C_MADCTL , 1      ,  // 14: Memory access control (directions), 1 arg:
+      0xC8,                   //     row addr/col addr, bottom to top refresh
+    ILI9163C_COLMOD , 1      ,  // 15: set color mode, 1 arg, no delay:
+      0x05 },                 //     16-bit color
+
+  Rcmd2green[] = {            // Init for 7735R, part 2 (green tab only)
+    2,                        //  2 commands in list:
+    ILI9163C_CASET  , 4      ,  //  1: Column addr set, 4 args, no delay:
+      0x00, 0x02,             //     XSTART = 0
+      0x00, 0x7F+0x02,        //     XEND = 127
+    ILI9163C_RASET  , 4      ,  //  2: Row addr set, 4 args, no delay:
+      0x00, 0x01,             //     XSTART = 0
+      0x00, 0x9F+0x01 },      //     XEND = 159
+  Rcmd2red[] = {              // Init for 7735R, part 2 (red tab only)
+    2,                        //  2 commands in list:
+    ILI9163C_CASET  , 4      ,  //  1: Column addr set, 4 args, no delay:
+      0x00, 0x00,             //     XSTART = 0
+      0x00, 0x7F,             //     XEND = 127
+    ILI9163C_RASET  , 4      ,  //  2: Row addr set, 4 args, no delay:
+      0x00, 0x00,             //     XSTART = 0
+      0x00, 0x9F },           //     XEND = 159
+
+  Rcmd3[] = {                 // Init for 7735R, part 3 (red or green tab)
+    4,                        //  4 commands in list:
+    ILI9163C_GMCTRP1, 16      , //  1: Magical unicorn dust, 16 args, no delay:
+      0x02, 0x1c, 0x07, 0x12,
+      0x37, 0x32, 0x29, 0x2d,
+      0x29, 0x25, 0x2B, 0x39,
+      0x00, 0x01, 0x03, 0x10,
+    ILI9163C_GMCTRN1, 16      , //  2: Sparkles and rainbows, 16 args, no delay:
+      0x03, 0x1d, 0x07, 0x06,
+      0x2E, 0x2C, 0x29, 0x2D,
+      0x2E, 0x2E, 0x37, 0x3F,
+      0x00, 0x00, 0x02, 0x10,
+    ILI9163C_NORON  ,    DELAY, //  3: Normal display on, no args, w/delay
+      10,                     //     10 ms delay
+    ILI9163C_DISPON ,    DELAY, //  4: Main screen turn on, no args w/delay
+      100 };                  //     100 ms delay
+
+
+// Companion code to the above tables.  Reads and issues
+// a series of LCD commands stored in PROGMEM byte array.
+//void Arduino_LCD::commandList(prog_uchar *addr) {
+void Arduino_LCD::commandList(uint8_t *addr) {
+
+  uint8_t  numCommands, numArgs;
+  uint16_t ms;
+
+  numCommands = *addr++;   // Number of commands to follow
+  while(numCommands--) {                 // For each command...
+    writecommand(*addr++); //   Read, issue command
+    numArgs  = *addr++;    //   Number of args to follow
+    ms       = numArgs & DELAY;          //   If hibit set, delay follows args
+    numArgs &= ~DELAY;                   //   Mask out delay bit
+    while(numArgs--) {                   //   For each argument...
+      writedata(*addr++);  //     Read, issue argument
+    }
+
+    if(ms) {
+      ms = *addr++; // Read post-command delay time (ms)
+      if(ms == 255) ms = 500;     // If 255, delay for 500 ms
+      delay(ms);
+    }
+  }
+}
+
+
+// Initialization code common to both 'B' and 'R' type displays
+//void Arduino_LCD::commonInit(prog_uchar *cmdList) {
+void Arduino_LCD::commonInit(uint8_t *cmdList) {
+
+  constructor(ILI9163C_TFTWIDTH, ILI9163C_TFTHEIGHT);
+  colstart  = rowstart = 0; // May be overridden in init func
+
+  pinMode(_rs, OUTPUT);
+  pinMode(_cs, OUTPUT);
+/*
+  csport    = portOutputRegister(digitalPinToPort(_cs));
+  cspinmask = digitalPinToBitMask(_cs);
+  rsport    = portOutputRegister(digitalPinToPort(_rs));
+  rspinmask = digitalPinToBitMask(_rs);
+*/
+
+//  if(hwSPI) { // Using hardware SPI
+    SPI.begin();
+    SPI.setClockDivider(21); // 4 MHz (half speed)
+//    SPI.setClockDivider(SPI_CLOCK_DIV4); // 4 MHz (half speed)
+//    SPI.setBitOrder(MSBFIRST);
+// there is no setBitOrder on the SPI library for the Due
+    SPI.setDataMode(SPI_MODE0);
+/*
+  } else {
+    pinMode(_sclk, OUTPUT);
+    pinMode(_sid , OUTPUT);
+    clkport     = portOutputRegister(digitalPinToPort(_sclk));
+    clkpinmask  = digitalPinToBitMask(_sclk);
+    dataport    = portOutputRegister(digitalPinToPort(_sid));
+    datapinmask = digitalPinToBitMask(_sid);
+    *clkport   &= ~clkpinmask;
+    *dataport  &= ~datapinmask;
+  }
+*/
+
+  // toggle RST low to reset; CS low so it'll listen to us
+//  *csport &= ~cspinmask;
+  digitalWrite(_cs, LOW);
+  if (_rst) {
+    pinMode(_rst, OUTPUT);
+    digitalWrite(_rst, HIGH);
+    delay(500);
+    digitalWrite(_rst, LOW);
+    delay(500);
+    digitalWrite(_rst, HIGH);
+    delay(500);
+  }
+
+  if(cmdList) commandList(cmdList);
+}
+
+
+// Initialization for ST7735B screens
+void Arduino_LCD::initB(void) {
+  commonInit(Bcmd);
+  commandList(Rcmd3);
+}
+
+
+// Initialization for ST7735R screens (green or red tabs)
+void Arduino_LCD::initR(uint8_t options) {
+  commonInit(Rcmd1);
+  if(options == INITR_GREENTAB) {
+    commandList(Rcmd2green);
+    colstart = 2;
+    rowstart = 1;
+  } else {
+    // colstart, rowstart left at default '0' values
+    commandList(Rcmd2red);
+  }
+  commandList(Rcmd3);
+}
+
+
+void Arduino_LCD::setAddrWindow(uint8_t x0, uint8_t y0, uint8_t x1,
+ uint8_t y1) {
+
+  writecommand(ILI9163C_CASET); // Column addr set
+  writedata(0x00);
+  writedata(x0+colstart);     // XSTART 
+  writedata(0x00);
+  writedata(x1+colstart);     // XEND
+
+  writecommand(ILI9163C_RASET); // Row addr set
+  writedata(0x00);
+  writedata(y0+rowstart);     // YSTART
+  writedata(0x00);
+  writedata(y1+rowstart);     // YEND
+
+  writecommand(ILI9163C_RAMWR); // write to RAM
+}
+
+
+void Arduino_LCD::fillScreen(uint16_t color) {
+
+  uint8_t x, y, hi = color >> 8, lo = color;
+
+  setAddrWindow(0, 0, _width-1, _height-1);
+
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+  digitalWrite(_cs, LOW);
+
+  for(y=_height; y>0; y--) {
+    for(x=_width; x>0; x--) {
+//SPI.transfer(hi);
+//SPI.transfer(lo);
+      spiwrite(hi);
+      spiwrite(lo);
+    }
+  }
+
+//  *csport |= cspinmask;
+  digitalWrite(_cs, HIGH);
+}
+
+
+void Arduino_LCD::pushColor(uint16_t color) {
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+  digitalWrite(_cs, LOW);
+
+  spiwrite(color >> 8);
+  spiwrite(color);
+//SPI.transfer(color>>8);
+//SPI.transfer(color);
+
+//  *csport |= cspinmask;
+  digitalWrite(_cs, HIGH);
+}
+
+
+void Arduino_LCD::drawPixel(int16_t x, int16_t y, uint16_t color) {
+
+  if((x < 0) ||(x >= _width) || (y < 0) || (y >= _height)) return;
+
+  setAddrWindow(x,y,x+1,y+1);
+
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+  digitalWrite(_cs, LOW);
+
+  spiwrite(color >> 8);
+  spiwrite(color);
+//SPI.transfer(color>>8);
+//SPI.transfer(color);
+
+//  *csport |= cspinmask;
+  digitalWrite(_cs, HIGH);
+}
+
+
+void Arduino_LCD::drawFastVLine(int16_t x, int16_t y, int16_t h,
+ uint16_t color) {
+
+  // Rudimentary clipping
+  if((x >= _width) || (y >= _height)) return;
+  if((y+h-1) >= _height) h = _height-y;
+  setAddrWindow(x, y, x, y+h-1);
+
+  uint8_t hi = color >> 8, lo = color;
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+  digitalWrite(_cs, LOW);
+  while (h--) {
+    spiwrite(hi);
+    spiwrite(lo);
+//SPI.transfer(hi);
+//SPI.transfer(lo);
+  }
+//  *csport |= cspinmask;
+  digitalWrite(_cs, HIGH);
+}
+
+
+void Arduino_LCD::drawFastHLine(int16_t x, int16_t y, int16_t w,
+  uint16_t color) {
+
+  // Rudimentary clipping
+  if((x >= _width) || (y >= _height)) return;
+  if((x+w-1) >= _width)  w = _width-x;
+  setAddrWindow(x, y, x+w-1, y);
+
+  uint8_t hi = color >> 8, lo = color;
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+  digitalWrite(_cs, LOW);
+  while (w--) {
+    spiwrite(hi);
+    spiwrite(lo);
+//SPI.transfer(hi);
+//SPI.transfer(lo);
+  }
+//  *csport |= cspinmask;
+  digitalWrite(_cs, HIGH);
+}
+
+
+// fill a rectangle
+void Arduino_LCD::fillRect(int16_t x, int16_t y, int16_t w, int16_t h,
+  uint16_t color) {
+
+  // rudimentary clipping (drawChar w/big text requires this)
+  if((x >= _width) || (y >= _height)) return;
+  if((x + w - 1) >= _width)  w = _width  - x;
+  if((y + h - 1) >= _height) h = _height - y;
+
+  setAddrWindow(x, y, x+w-1, y+h-1);
+
+  uint8_t hi = color >> 8, lo = color;
+//  *rsport |=  rspinmask;
+//  *csport &= ~cspinmask;
+digitalWrite(_rs, HIGH);
+digitalWrite(_cs, LOW);
+  for(y=h; y>0; y--) {
+    for(x=w; x>0; x--) {
+      spiwrite(hi);
+      spiwrite(lo);
+//SPI.transfer(hi);
+//SPI.transfer(lo);
+    }
+  }
+
+//  *csport |= cspinmask;
+digitalWrite(_cs, HIGH);
+}
+
+
+// Pass 8-bit (each) R,G,B, get back 16-bit packed color
+uint16_t Arduino_LCD::Color565(uint8_t r, uint8_t g, uint8_t b) {
+  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+}
+
+
+#define MADCTL_MY  0x80
+#define MADCTL_MX  0x40
+#define MADCTL_MV  0x20
+#define MADCTL_ML  0x10
+#define MADCTL_RGB 0x08
+#define MADCTL_MH  0x04
+
+void Arduino_LCD::setRotation(uint8_t m) {
+
+  writecommand(ILI9163C_MADCTL);
+  rotation = m % 4; // can't be higher than 3
+  switch (rotation) {
+   case 0:
+    writedata(MADCTL_MX | MADCTL_MY | MADCTL_RGB);
+    _width  = ILI9163C_TFTWIDTH;
+    _height = ILI9163C_TFTHEIGHT;
+    break;
+   case 1:
+    writedata(MADCTL_MY | MADCTL_MV | MADCTL_RGB);
+    _width  = ILI9163C_TFTHEIGHT;
+    _height = ILI9163C_TFTWIDTH;
+    break;
+   case 2:
+    writedata(MADCTL_RGB);
+    _width  = ILI9163C_TFTWIDTH;
+    _height = ILI9163C_TFTHEIGHT;
+    break;
+   case 3:
+    writedata(MADCTL_MX | MADCTL_MV | MADCTL_RGB);
+    _width  = ILI9163C_TFTHEIGHT;
+    _height = ILI9163C_TFTWIDTH;
+    break;
+  }
+}
+
+
+void Arduino_LCD::invertDisplay(boolean i) {
+  writecommand(i ? ILI9163C_INVON : ILI9163C_INVOFF);
+}
+
+/* 
+  18, // there are 17 commands
+    ILI9163C_SWRESET,   DELAY,  //  1: Software reset, no args, w/delay
+      50,                     //     50 ms delay
+
+	0x11, 			//Exit Sleep
+	DELAY,50,
+
+	0x26, 			//Set Default Gamma
+	0x104,
+
+	//0xF2, //E0h & E1h Enable/Disable
+	//0x100,
+
+	0xB1,
+	0x10C,
+	0x114,
+
+	0xC0, //Set VRH1[4:0] & VC[2:0] for VCI1 & GVDD
+	0x10C,
+	0x105,
+
+	0xC1, //Set BT[2:0] for AVDD & VCL & VGH & VGL
+	0x102,
+
+	0xC5, //Set VMH[6:0] & VML[6:0] for VOMH & VCOML
+	0x129,
+	0x143,
+
+	0xC7,
+	0x140,
+
+	0x3a, //Set Color Format
+	0x105,
+
+	0x2A, //Set Column Address
+	0x100,
+	0x100,
+	0x100,
+	0x17F,
+
+	0x2B, //Set Page Address
+	0x100,
+	0x100,
+	0x100,
+	0x19F,
+
+	0x36, //Set Scanning Direction, RGB
+	0x1C0,
+
+	0xB7, //Set Source Output Direction
+	0x100,
+
+	0xf2, //Enable Gamma bit
+	0x101,
+
+	0xE0,
+	0x136,//p1
+	0x129,//p2
+	0x112,//p3
+	0x122,//p4
+	0x11C,//p5
+	0x115,//p6
+	0x142,//p7
+	0x1B7,//p8
+	0x12F,//p9
+	0x113,//p10
+	0x112,//p11
+	0x10A,//p12
+	0x111,//p13
+	0x10B,//p14
+	0x106,//p15
+
+	0xE1,
+	0x109,//p1
+	0x116,//p2
+	0x12D,//p3
+	0x10D,//p4
+	0x113,//p5
+	0x115,//p6
+	0x140,//p7
+	0x148,//p8
+	0x153,//p9
+	0x10C,//p10
+	0x11D,//p11
+	0x125,//p12
+	0x12E,//p13
+	0x134,//p14
+	0x139,//p15
+
+	0x33, // scroll setup
+	0x100,
+	0x100,
+	0x100,
+	0x1C1,
+	0x100,
+	0x100,
+
+	0x29,	// Display On
+	0x2C},	// write gram
+
+*/
+
diff --git a/libraries/Robot_Control/Arduino_LCD.h b/libraries/Robot_Control/Arduino_LCD.h
new file mode 100644
index 0000000..a518133
--- /dev/null
+++ b/libraries/Robot_Control/Arduino_LCD.h
@@ -0,0 +1,141 @@
+/***************************************************
+  This is a library for the Adafruit 1.8" SPI display.
+  This library works with the Adafruit 1.8" TFT Breakout w/SD card
+  ----> http://www.adafruit.com/products/358
+  as well as Adafruit raw 1.8" TFT display
+  ----> http://www.adafruit.com/products/618
+ 
+  Check out the links above for our tutorials and wiring diagrams
+  These displays use SPI to communicate, 4 or 5 pins are required to
+  interface (RST is optional)
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit and open-source hardware by purchasing
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+#ifndef _ARDUINO_LCDH_
+#define _ARDUINO_LCDH_
+
+#if ARDUINO >= 100
+ #include "Arduino.h"
+ #include "Print.h"
+#else
+ #include "WProgram.h"
+#endif
+#include "Adafruit_GFX.h"
+//#include <avr/pgmspace.h>
+
+// some flags for initR() :(
+#define INITR_GREENTAB 0x0
+#define INITR_REDTAB   0x1
+
+#define ILI9163C_TFTWIDTH  128
+#define ILI9163C_TFTHEIGHT 160
+
+#define ILI9163C_NOP     0x00
+#define ILI9163C_SWRESET 0x01
+#define ILI9163C_RDDID   0x04
+#define ILI9163C_RDDST   0x09
+
+#define ILI9163C_SLPIN   0x10
+#define ILI9163C_SLPOUT  0x11
+#define ILI9163C_PTLON   0x12
+#define ILI9163C_NORON   0x13
+
+#define ILI9163C_INVOFF  0x20
+#define ILI9163C_INVON   0x21
+#define ILI9163C_DISPOFF 0x28
+#define ILI9163C_DISPON  0x29
+#define ILI9163C_CASET   0x2A
+#define ILI9163C_RASET   0x2B
+#define ILI9163C_RAMWR   0x2C
+#define ILI9163C_RAMRD   0x2E
+
+#define ILI9163C_PTLAR   0x30
+#define ILI9163C_COLMOD  0x3A // this is interface pixel format, this might be the issue
+#define ILI9163C_MADCTL  0x36
+
+#define ILI9163C_FRMCTR1 0xB1
+#define ILI9163C_FRMCTR2 0xB2
+#define ILI9163C_FRMCTR3 0xB3
+#define ILI9163C_INVCTR  0xB4
+#define ILI9163C_DISSET5 0xB6
+
+#define ILI9163C_PWCTR1  0xC0
+#define ILI9163C_PWCTR2  0xC1
+#define ILI9163C_PWCTR3  0xC2
+#define ILI9163C_PWCTR4  0xC3
+#define ILI9163C_PWCTR5  0xC4
+#define ILI9163C_VMCTR1  0xC5
+
+#define ILI9163C_RDID1   0xDA
+#define ILI9163C_RDID2   0xDB
+#define ILI9163C_RDID3   0xDC
+#define ILI9163C_RDID4   0xDD
+
+#define ILI9163C_PWCTR6  0xFC
+
+#define ILI9163C_GMCTRP1 0xE0
+#define ILI9163C_GMCTRN1 0xE1
+
+// Color definitions
+#define	ILI9163C_BLACK   0x0000
+#define	ILI9163C_BLUE    0x001F
+#define	ILI9163C_RED     0xF800
+#define	ILI9163C_GREEN   0x07E0
+#define ILI9163C_CYAN    0x07FF
+#define ILI9163C_MAGENTA 0xF81F
+#define ILI9163C_YELLOW  0xFFE0  
+#define ILI9163C_WHITE   0xFFFF
+
+
+class Arduino_LCD : public Adafruit_GFX {
+
+ public:
+
+  Arduino_LCD(uint8_t CS, uint8_t RS, uint8_t SID, uint8_t SCLK, uint8_t RST);
+  Arduino_LCD(uint8_t CS, uint8_t RS, uint8_t RST);
+
+  void     initB(void),                             // for ST7735B displays
+           initR(uint8_t options = INITR_GREENTAB), // for ST7735R
+           setAddrWindow(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1),
+           pushColor(uint16_t color),
+           fillScreen(uint16_t color),
+           drawPixel(int16_t x, int16_t y, uint16_t color),
+           drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color),
+           drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color),
+           fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color),
+           setRotation(uint8_t r),
+           invertDisplay(boolean i);
+  uint16_t Color565(uint8_t r, uint8_t g, uint8_t b);
+
+  /* These are not for current use, 8-bit protocol only!
+  uint8_t  readdata(void),
+           readcommand8(uint8_t);
+  uint16_t readcommand16(uint8_t);
+  uint32_t readcommand32(uint8_t);
+  void     dummyclock(void);
+  */
+
+ private:
+
+  void     spiwrite(uint8_t),
+           writecommand(uint8_t c),
+           writedata(uint8_t d),
+//           commandList(prog_uchar *addr),
+//           commonInit(prog_uchar *cmdList);
+           commandList(uint8_t *addr),
+           commonInit(uint8_t *cmdList);
+//uint8_t  spiread(void);
+
+  boolean  hwSPI;
+  volatile uint8_t *dataport, *clkport, *csport, *rsport;
+  uint8_t  _cs, _rs, _rst, _sid, _sclk,
+           datapinmask, clkpinmask, cspinmask, rspinmask,
+           colstart, rowstart; // some displays need this changed
+};
+
+#endif
diff --git a/libraries/Robot_Control/Compass.cpp b/libraries/Robot_Control/Compass.cpp
new file mode 100644
index 0000000..1b1ef31
--- /dev/null
+++ b/libraries/Robot_Control/Compass.cpp
@@ -0,0 +1,34 @@
+#include "Compass.h"
+#include <Wire.h>
+
+void Compass::begin(){
+	Wire.begin();
+}
+float Compass::getReading(){
+	_beginTransmission();
+	_endTransmission();
+	
+  //time delays required by HMC6352 upon receipt of the command
+  //Get Data. Compensate and Calculate New Heading : 6ms
+  delay(6);
+
+  Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB
+
+  //"The heading output data will be the value in tenths of degrees
+  //from zero to 3599 and provided in binary format over the two bytes."
+  byte MSB = Wire.read();
+  byte LSB = Wire.read();
+
+  float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
+  float headingInt = headingSum / 10; 
+  
+  return headingInt;
+}
+
+void Compass::_beginTransmission(){
+  Wire.beginTransmission(HMC6352SlaveAddress);
+  Wire.write(HMC6352ReadAddress);
+}
+void Compass::_endTransmission(){
+  Wire.endTransmission();
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/Compass.h b/libraries/Robot_Control/Compass.h
new file mode 100644
index 0000000..aa085a9
--- /dev/null
+++ b/libraries/Robot_Control/Compass.h
@@ -0,0 +1,24 @@
+#ifndef Compass_h
+#define Compass_h
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+//0x21==0x42>>1, from bildr's code
+#define HMC6352SlaveAddress 0x21
+#define HMC6352ReadAddress 0x41
+
+class Compass{
+	public:
+		void begin();
+		float getReading();
+	private:
+		void _beginTransmission();
+		void _endTransmission();
+
+};
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/EEPROM_I2C.cpp b/libraries/Robot_Control/EEPROM_I2C.cpp
new file mode 100644
index 0000000..dd12695
--- /dev/null
+++ b/libraries/Robot_Control/EEPROM_I2C.cpp
@@ -0,0 +1,62 @@
+#include "EEPROM_I2C.h"
+#include <Wire.h>
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+void EEPROM_I2C::begin(){
+  Wire.begin();
+}
+
+void EEPROM_I2C::writeByte(unsigned int eeaddress, byte data){
+  int rdata = data;
+  this->_beginTransmission(eeaddress);
+  Wire.write(rdata);
+  this->_endTransmission();
+}
+
+byte EEPROM_I2C::readByte(unsigned int eeaddress){
+  int rdata;
+  this->_beginTransmission(eeaddress);
+  this->_endTransmission();
+  
+  Wire.requestFrom(DEVICEADDRESS,1);
+  if (Wire.available()) rdata = Wire.read();
+  return rdata;
+}
+
+void EEPROM_I2C::writePage(unsigned int eeaddress, byte* data, byte length ){
+  this->_beginTransmission(eeaddress);
+  
+  byte c;
+  
+  for ( c = 0; c < length; c++)
+    Wire.write(data[c]);
+	
+  this->_endTransmission();
+  
+  delay(10);                           // need some delay
+}
+
+void EEPROM_I2C::readBuffer(unsigned int eeaddress, byte *buffer, int length ){
+  this->_beginTransmission(eeaddress);
+  this->_endTransmission();
+  Wire.requestFrom(DEVICEADDRESS,length);
+
+  for ( int c = 0; c < length; c++ )
+    if (Wire.available()) buffer[c] = Wire.read();
+}
+
+
+
+void EEPROM_I2C::_beginTransmission(unsigned int eeaddress){
+  Wire.beginTransmission(DEVICEADDRESS);
+  Wire.write((eeaddress >> 8));    // Address High Byte
+  Wire.write((eeaddress & 0xFF));  // Address Low Byte
+}
+void EEPROM_I2C::_endTransmission(){
+  Wire.endTransmission();
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/EEPROM_I2C.h b/libraries/Robot_Control/EEPROM_I2C.h
new file mode 100644
index 0000000..9bd0f6a
--- /dev/null
+++ b/libraries/Robot_Control/EEPROM_I2C.h
@@ -0,0 +1,31 @@
+#ifndef EEPROM_I2C_h
+#define EEPROM_I2C_h
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#define EE24LC512MAXBYTES 64000
+#define DEVICEADDRESS 0x50
+
+class EEPROM_I2C{
+	public:
+		void begin();
+		
+		void writeByte(unsigned int eeaddresspage, byte data);
+		byte readByte(unsigned int eeaddresspage);
+		
+		void writePage(unsigned int eeaddresspage, byte* data, byte length );
+		void readBuffer(unsigned int eeaddress, byte *buffer, int length );
+		
+		//uint16_t readPixel(uint16_t theMemoryAddress);
+		//void readImage(uint16_t theMemoryAddress, int width, int height);
+			
+	protected:
+		void _beginTransmission(unsigned int eeaddress);
+		void _endTransmission();
+};
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/EasyTransfer2.cpp b/libraries/Robot_Control/EasyTransfer2.cpp
new file mode 100644
index 0000000..24427cc
--- /dev/null
+++ b/libraries/Robot_Control/EasyTransfer2.cpp
@@ -0,0 +1,152 @@
+#include "EasyTransfer2.h"
+
+
+
+
+//Captures address and size of struct
+void EasyTransfer2::begin(HardwareSerial *theSerial){
+	_serial = theSerial;
+	
+	//dynamic creation of rx parsing buffer in RAM
+	//rx_buffer = (uint8_t*) malloc(size);
+	
+	resetData();
+}
+
+void EasyTransfer2::writeByte(uint8_t dat){
+	if(position<20)
+		data[position++]=dat;
+		size++;
+}
+void EasyTransfer2::writeInt(int dat){
+	if(position<19){
+		data[position++]=dat>>8;
+		data[position++]=dat;
+		size+=2;
+	}
+}
+uint8_t EasyTransfer2::readByte(){
+	if(position>=size)return 0;
+	return data[position++];
+}
+int EasyTransfer2::readInt(){
+	if(position+1>=size)return 0;
+	int dat_1=data[position++]<<8;
+	int dat_2=data[position++];
+	int dat= dat_1 | dat_2;
+	return dat;
+}
+
+void EasyTransfer2::resetData(){
+	for(int i=0;i<20;i++){
+		data[i]=0;
+	}
+	size=0;
+	position=0;
+}
+
+//Sends out struct in binary, with header, length info and checksum
+void EasyTransfer2::sendData(){
+  uint8_t CS = size;
+  _serial->write(0x06);
+  _serial->write(0x85);
+  _serial->write(size);
+  for(int i = 0; i<size; i++){
+    CS^=*(data+i);
+    _serial->write(*(data+i));
+	//Serial.print(*(data+i));
+	//Serial.print(",");
+  }
+  //Serial.println("");
+  _serial->write(CS);
+  
+  resetData();
+}
+
+boolean EasyTransfer2::receiveData(){
+  
+  //start off by looking for the header bytes. If they were already found in a previous call, skip it.
+  if(rx_len == 0){
+  //this size check may be redundant due to the size check below, but for now I'll leave it the way it is.
+    if(_serial->available() >= 3){
+	//this will block until a 0x06 is found or buffer size becomes less then 3.
+      while(_serial->read() != 0x06) {
+		//This will trash any preamble junk in the serial buffer
+		//but we need to make sure there is enough in the buffer to process while we trash the rest
+		//if the buffer becomes too empty, we will escape and try again on the next call
+		if(_serial->available() < 3)
+			return false;
+		}
+		//Serial.println("head");
+      if (_serial->read() == 0x85){
+        rx_len = _serial->read();
+		//Serial.print("rx_len:");
+		//Serial.println(rx_len);
+		resetData();
+
+		//make sure the binary structs on both Arduinos are the same size.
+        /*if(rx_len != size){
+          rx_len = 0;
+          return false;
+        }*/
+      }
+    }
+	//Serial.println("nothing");
+  }
+  
+  //we get here if we already found the header bytes, the struct size matched what we know, and now we are byte aligned.
+  if(rx_len != 0){
+	
+    while(_serial->available() && rx_array_inx <= rx_len){
+      data[rx_array_inx++] = _serial->read();
+    }
+    
+    if(rx_len == (rx_array_inx-1)){
+      //seem to have got whole message
+      //last uint8_t is CS
+      calc_CS = rx_len;
+	  //Serial.print("len:");
+	  //Serial.println(rx_len);
+      for (int i = 0; i<rx_len; i++){
+        calc_CS^=data[i];
+		//Serial.print("m");
+		//Serial.print(data[i]);
+		//Serial.print(",");
+      } 
+	  //Serial.println();
+      //Serial.print(data[rx_array_inx-1]);
+	  //Serial.print(" ");
+	  //Serial.println(calc_CS);
+	  
+      if(calc_CS == data[rx_array_inx-1]){//CS good
+		//resetData();
+        //memcpy(data,d,rx_len);
+		for(int i=0;i<20;i++){
+			//Serial.print(data[i]);
+			//Serial.print(",");
+		}
+		//Serial.println("");
+		size=rx_len;
+		rx_len = 0;
+		rx_array_inx = 0;
+		return true;
+		}
+		
+	  else{
+  		//Serial.println("CS");
+		resetData();
+	  //failed checksum, need to clear this out anyway
+		rx_len = 0;
+		rx_array_inx = 0;
+		return false;
+	  }
+        
+    }
+  }
+  //Serial.print(rx_len);
+  //Serial.print(" ");
+  //Serial.print(rx_array_inx);
+  //Serial.print(" ");
+  //Serial.println("Short");
+  return false;
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/EasyTransfer2.h b/libraries/Robot_Control/EasyTransfer2.h
new file mode 100644
index 0000000..3369a51
--- /dev/null
+++ b/libraries/Robot_Control/EasyTransfer2.h
@@ -0,0 +1,76 @@
+/******************************************************************
+*  EasyTransfer Arduino Library 
+*		details and example sketch: 
+*			http://www.billporter.info/easytransfer-arduino-library/
+*
+*		Brought to you by:
+*              Bill Porter
+*              www.billporter.info
+*
+*		See Readme for other info and version history
+*	
+*  
+*This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or(at your option) any later version.
+This program 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 General Public License for more details.
+<http://www.gnu.org/licenses/>
+*
+*This work is licensed under the Creative Commons Attribution-ShareAlike 3.0 Unported License. 
+*To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/ or
+*send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+******************************************************************/
+#ifndef EasyTransfer2_h
+#define EasyTransfer2_h
+
+
+//make it a little prettier on the front end. 
+#define details(name) (byte*)&name,sizeof(name)
+
+//Not neccessary, but just in case. 
+#if ARDUINO > 22
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+#include "HardwareSerial.h"
+//#include <NewSoftSerial.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <avr/io.h>
+
+class EasyTransfer2 {
+public:
+void begin(HardwareSerial *theSerial);
+//void begin(uint8_t *, uint8_t, NewSoftSerial *theSerial);
+void sendData();
+boolean receiveData();
+
+void writeByte(uint8_t dat);
+void writeInt(int dat);
+uint8_t readByte();
+int readInt();
+
+
+private:
+HardwareSerial *_serial;
+
+void resetData();
+
+uint8_t data[20];	//data storage, for both read and send
+uint8_t position;
+uint8_t size; //size of data in bytes. Both for read and send
+//uint8_t * address;  //address of struct
+//uint8_t size;       //size of struct
+//uint8_t * rx_buffer; //address for temporary storage and parsing buffer
+//uint8_t rx_buffer[20];
+uint8_t rx_array_inx;  //index for RX parsing buffer
+uint8_t rx_len;		//RX packet length according to the packet
+uint8_t calc_CS;	   //calculated Chacksum
+};
+
+
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/Fat16.cpp b/libraries/Robot_Control/Fat16.cpp
new file mode 100644
index 0000000..aa8f585
--- /dev/null
+++ b/libraries/Robot_Control/Fat16.cpp
@@ -0,0 +1,990 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include <avr/pgmspace.h>
+#if ARDUINO < 100
+#include <WProgram.h>
+#else  // ARDUINO
+#include <Arduino.h>
+#endif  // ARDUINO
+#include <Fat16.h>
+//-----------------------------------------------------------------------------
+// volume info
+uint8_t  Fat16::volumeInitialized_ = 0;  // true if FAT16 volume is valid
+uint8_t  Fat16::fatCount_;               // number of file allocation tables
+uint8_t  Fat16::blocksPerCluster_;       // must be power of 2
+uint16_t Fat16::rootDirEntryCount_;      // should be 512 for FAT16
+fat_t    Fat16::blocksPerFat_;           // number of blocks in one FAT
+fat_t    Fat16::clusterCount_;           // total clusters in volume
+uint32_t Fat16::fatStartBlock_;          // start of first FAT
+uint32_t Fat16::rootDirStartBlock_;      // start of root dir
+uint32_t Fat16::dataStartBlock_;         // start of data clusters
+//------------------------------------------------------------------------------
+// raw block cache
+SdCard *Fat16::rawDev_ = 0;             // class for block read and write
+uint32_t Fat16::cacheBlockNumber_ = 0XFFFFFFFF;  // init to invalid block number
+cache16_t  Fat16::cacheBuffer_;         // 512 byte cache for SdCard
+uint8_t  Fat16::cacheDirty_ = 0;        // cacheFlush() will write block if true
+uint32_t Fat16::cacheMirrorBlock_ = 0;  // mirror  block for second FAT
+//------------------------------------------------------------------------------
+// callback function for date/time
+void (*Fat16::dateTime_)(uint16_t* date, uint16_t* time) = NULL;
+
+#if ALLOW_DEPRECATED_FUNCTIONS
+void (*Fat16::oldDateTime_)(uint16_t& date, uint16_t& time) = NULL;  // NOLINT
+#endif  // ALLOW_DEPRECATED_FUNCTIONS
+//------------------------------------------------------------------------------
+// format 8.3 name for directory entry
+static uint8_t make83Name(const char* str, uint8_t* name) {
+  uint8_t c;
+  uint8_t n = 7;  // max index for part before dot
+  uint8_t i = 0;
+  // blank fill name and extension
+  while (i < 11) name[i++] = ' ';
+  i = 0;
+  while ((c = *str++) != '\0') {
+    if (c == '.') {
+      if (n == 10) return false;  // only one dot allowed
+      n = 10;  // max index for full 8.3 name
+      i = 8;   // place for extension
+    } else {
+      // illegal FAT characters
+      PGM_P p = PSTR("|<>^+=?/[];,*\"\\");
+      uint8_t b;
+      while ((b = pgm_read_byte(p++))) if (b == c) return false;
+      // check length and only allow ASCII printable characters
+      if (i > n || c < 0X21 || c > 0X7E) return false;
+      // only upper case allowed in 8.3 names - convert lower to upper
+      name[i++] = c < 'a' || c > 'z' ?  c : c + ('A' - 'a');
+    }
+  }
+  // must have a file name, extension is optional
+  return name[0] != ' ';
+}
+//==============================================================================
+// Fat16 member functions
+//------------------------------------------------------------------------------
+uint8_t Fat16::addCluster(void) {
+  // start search after last cluster of file or at cluster two in FAT
+  fat_t freeCluster = curCluster_ ? curCluster_ : 1;
+  for (fat_t i = 0; ; i++) {
+    // return no free clusters
+    if (i >= clusterCount_) return false;
+    // Fat has clusterCount + 2 entries
+    if (freeCluster > clusterCount_) freeCluster = 1;
+    freeCluster++;
+    fat_t value;
+    if (!fatGet(freeCluster, &value)) return false;
+    if (value == 0) break;
+  }
+  // mark cluster allocated
+  if (!fatPut(freeCluster, FAT16EOC)) return false;
+
+  if (curCluster_ != 0) {
+    // link cluster to chain
+    if (!fatPut(curCluster_, freeCluster)) return false;
+  } else {
+    // first cluster of file so update directory entry
+    flags_ |= F_FILE_DIR_DIRTY;
+    firstCluster_ = freeCluster;
+  }
+  curCluster_ = freeCluster;
+  return true;
+}
+//------------------------------------------------------------------------------
+//
+dir_t* Fat16::cacheDirEntry(uint16_t index, uint8_t action) {
+  if (index >= rootDirEntryCount_) return NULL;
+  if (!cacheRawBlock(rootDirStartBlock_ + (index >> 4), action)) return NULL;
+  return &cacheBuffer_.dir[index & 0XF];
+}
+//------------------------------------------------------------------------------
+//
+uint8_t Fat16::cacheFlush(void) {
+  if (cacheDirty_) {
+    if (!rawDev_->writeBlock(cacheBlockNumber_, cacheBuffer_.data)) {
+      return false;
+    }
+    // mirror FAT tables
+    if (cacheMirrorBlock_) {
+      if (!rawDev_->writeBlock(cacheMirrorBlock_, cacheBuffer_.data)) {
+        return false;
+      }
+      cacheMirrorBlock_ = 0;
+    }
+    cacheDirty_ = 0;
+  }
+  return true;
+}
+//------------------------------------------------------------------------------
+//
+uint8_t Fat16::cacheRawBlock(uint32_t blockNumber, uint8_t action) {
+  if (cacheBlockNumber_ != blockNumber) {
+    if (!cacheFlush()) return false;
+    if (!rawDev_->readBlock(blockNumber, cacheBuffer_.data)) return false;
+    cacheBlockNumber_ = blockNumber;
+  }
+  cacheDirty_ |= action;
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ *  Close a file and force cached data and directory information
+ *  to be written to the storage device.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include no file is open or an I/O error.
+ */
+uint8_t Fat16::close(void) {
+  if (!sync()) return false;
+  flags_ = 0;
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ * Return a files directory entry
+ *
+ * \param[out] dir Location for return of the files directory entry.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t Fat16::dirEntry(dir_t* dir) {
+  if (!sync()) return false;
+  dir_t* p = cacheDirEntry(dirEntryIndex_, CACHE_FOR_WRITE);
+  if (!p) return false;
+  memcpy(dir, p, sizeof(dir_t));
+  return true;
+}
+//------------------------------------------------------------------------------
+uint8_t Fat16::fatGet(fat_t cluster, fat_t* value) {
+  if (cluster > (clusterCount_ + 1)) return false;
+  uint32_t lba = fatStartBlock_ + (cluster >> 8);
+  if (lba != cacheBlockNumber_) {
+    if (!cacheRawBlock(lba)) return false;
+  }
+  *value = cacheBuffer_.fat[cluster & 0XFF];
+  return true;
+}
+//------------------------------------------------------------------------------
+uint8_t Fat16::fatPut(fat_t cluster, fat_t value) {
+  if (cluster < 2) return false;
+  if (cluster > (clusterCount_ + 1)) return false;
+  uint32_t lba = fatStartBlock_ + (cluster >> 8);
+  if (lba != cacheBlockNumber_) {
+    if (!cacheRawBlock(lba)) return false;
+  }
+  cacheBuffer_.fat[cluster & 0XFF] = value;
+  cacheSetDirty();
+  // mirror second FAT
+  if (fatCount_ > 1) cacheMirrorBlock_ = lba + blocksPerFat_;
+  return true;
+}
+//------------------------------------------------------------------------------
+// free a cluster chain
+uint8_t Fat16::freeChain(fat_t cluster) {
+  while (1) {
+    fat_t next;
+    if (!fatGet(cluster, &next)) return false;
+    if (!fatPut(cluster, 0)) return false;
+    if (isEOC(next)) return true;
+    cluster = next;
+  }
+}
+//------------------------------------------------------------------------------
+/**
+ *  Initialize a FAT16 volume.
+ *
+ * \param[in] dev The SdCard where the volume is located.
+ *
+ * \param[in] part The partition to be used.  Legal values for \a part are
+ * 1-4 to use the corresponding partition on a device formatted with
+ * a MBR, Master Boot Record, or zero if the device is formatted as
+ * a super floppy with the FAT boot sector in block zero.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.  reasons for
+ * failure include not finding a valid FAT16 file system in the
+ * specified partition, a call to init() after a volume has
+ * been successful initialized or an I/O error.
+ *
+ */
+uint8_t Fat16::init(SdCard* dev, uint8_t part) {
+  // error if invalid partition
+  if (part > 4) return false;
+  rawDev_ = dev;
+  uint32_t volumeStartBlock = 0;
+  // if part == 0 assume super floppy with FAT16 boot sector in block zero
+  // if part > 0 assume mbr volume with partition table
+  if (part) {
+    if (!cacheRawBlock(volumeStartBlock)) return false;
+    volumeStartBlock = cacheBuffer_.mbr.part[part - 1].firstSector;
+  }
+  if (!cacheRawBlock(volumeStartBlock)) return false;
+  // check boot block signature
+  if (cacheBuffer_.data[510] != BOOTSIG0 ||
+      cacheBuffer_.data[511] != BOOTSIG1) return false;
+  bpb_t* bpb = &cacheBuffer_.fbs.bpb;
+  fatCount_ = bpb->fatCount;
+  blocksPerCluster_ = bpb->sectorsPerCluster;
+  blocksPerFat_ = bpb->sectorsPerFat16;
+  rootDirEntryCount_ = bpb->rootDirEntryCount;
+  fatStartBlock_ = volumeStartBlock + bpb->reservedSectorCount;
+  rootDirStartBlock_ = fatStartBlock_ + bpb->fatCount*bpb->sectorsPerFat16;
+  dataStartBlock_ = rootDirStartBlock_
+                    + ((32*bpb->rootDirEntryCount + 511)/512);
+  uint32_t totalBlocks = bpb->totalSectors16 ?
+                               bpb->totalSectors16 : bpb->totalSectors32;
+  clusterCount_ = (totalBlocks - (dataStartBlock_ - volumeStartBlock))
+                  /bpb->sectorsPerCluster;
+  // verify valid FAT16 volume
+  if (bpb->bytesPerSector != 512       // only allow 512 byte blocks
+     || bpb->sectorsPerFat16 == 0      // zero for FAT32
+     || clusterCount_ < 4085           // FAT12 if true
+     || totalBlocks > 0X800000         // Max size for FAT16 volume
+     || bpb->reservedSectorCount == 0  // invalid volume
+     || bpb->fatCount == 0             // invalid volume
+     || bpb->sectorsPerFat16 < (clusterCount_ >> 8)  // invalid volume
+     || bpb->sectorsPerCluster == 0   // invalid volume
+        // power of 2 test
+     || bpb->sectorsPerCluster & (bpb->sectorsPerCluster - 1)) {
+    // not a usable FAT16 bpb
+    return false;
+  }
+  volumeInitialized_ = 1;
+  return true;
+}
+//------------------------------------------------------------------------------
+/** List directory contents to Serial.
+ *
+ * \param[in] flags The inclusive OR of
+ *
+ * LS_DATE - %Print file modification date
+ *
+ * LS_SIZE - %Print file size.
+ */
+void Fat16::ls(uint8_t flags) {
+  dir_t d;
+  for (uint16_t index = 0; readDir(&d, &index, DIR_ATT_VOLUME_ID); index++) {
+    // print file name with possible blank fill
+    printDirName(d, flags & (LS_DATE | LS_SIZE) ? 14 : 0);
+
+    // print modify date/time if requested
+    if (flags & LS_DATE) {
+       printFatDate(d.lastWriteDate);
+       Serial.write(' ');
+       printFatTime(d.lastWriteTime);
+    }
+
+    // print size if requested
+    if (DIR_IS_FILE(&d) && (flags & LS_SIZE)) {
+      Serial.write(' ');
+      Serial.print(d.fileSize);
+    }
+    Serial.println();
+  }
+}
+//------------------------------------------------------------------------------
+/**
+ * Open a file by file name.
+ *
+ * \note The file must be in the root directory and must have a DOS
+ * 8.3 name.
+ *
+ * \param[in] fileName A valid 8.3 DOS name for a file in the root directory.
+ *
+ * \param[in] oflag Values for \a oflag are constructed by a bitwise-inclusive
+ *  OR of flags from the following list
+ *
+ * O_READ - Open for reading.
+ *
+ * O_RDONLY - Same as O_READ.
+ *
+ * O_WRITE - Open for writing.
+ *
+ * O_WRONLY - Same as O_WRITE.
+ *
+ * O_RDWR - Open for reading and writing.
+ *
+ * O_APPEND - If set, the file offset shall be set to the end of the
+ * file prior to each write.
+ *
+ * O_CREAT - If the file exists, this flag has no effect except as noted
+ * under O_EXCL below. Otherwise, the file shall be created
+ *
+ * O_EXCL - If O_CREAT and O_EXCL are set, open() shall fail if the file exists.
+ *
+ * O_SYNC - Call sync() after each write.  This flag should not be used with
+ * write(uint8_t), write_P(PGM_P), writeln_P(PGM_P), or the Arduino Print class.
+ * These functions do character a time writes so sync() will be called
+ * after each byte.
+ *
+ * O_TRUNC - If the file exists and is a regular file, and the file is
+ * successfully opened and is not read only, its length shall be truncated to 0.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include the FAT volume has not been initialized,
+ * a file is already open, \a fileName is invalid, the file does not exist,
+ * is a directory, or can't be opened in the access mode specified by oflag.
+ */
+uint8_t Fat16::open(const char* fileName, uint8_t oflag) {
+  uint8_t dname[11];   // name formated for dir entry
+  int16_t empty = -1;  // index of empty slot
+  dir_t* p;            // pointer to cached dir entry
+
+  if (!volumeInitialized_ || isOpen()) return false;
+
+  // error if invalid name
+  if (!make83Name(fileName, dname)) return false;
+
+  for (uint16_t index = 0; index < rootDirEntryCount_; index++) {
+    if (!(p = cacheDirEntry(index))) return false;
+    if (p->name[0] == DIR_NAME_FREE || p->name[0] == DIR_NAME_DELETED) {
+      // remember first empty slot
+      if (empty < 0) empty = index;
+      // done if no entries follow
+      if (p->name[0] == DIR_NAME_FREE) break;
+    } else if (!memcmp(dname, p->name, 11)) {
+      // don't open existing file if O_CREAT and O_EXCL
+      if ((oflag & (O_CREAT | O_EXCL)) == (O_CREAT | O_EXCL)) return false;
+
+      // open existing file
+      return open(index, oflag);
+    }
+  }
+  // error if directory is full
+  if (empty < 0) return false;
+
+  // only create file if O_CREAT and O_WRITE
+  if ((oflag & (O_CREAT | O_WRITE)) != (O_CREAT | O_WRITE)) return false;
+
+  if (!(p = cacheDirEntry(empty, CACHE_FOR_WRITE))) return false;
+
+  // initialize as empty file
+  memset(p, 0, sizeof(dir_t));
+  memcpy(p->name, dname, 11);
+
+  // set timestamps
+  if (dateTime_) {
+    // call user function
+    dateTime_(&p->creationDate, &p->creationTime);
+  } else {
+    // use default date/time
+    p->creationDate = FAT_DEFAULT_DATE;
+    p->creationTime = FAT_DEFAULT_TIME;
+  }
+  p->lastAccessDate = p->creationDate;
+  p->lastWriteDate = p->creationDate;
+  p->lastWriteTime = p->creationTime;
+
+  // insure created directory entry will be written to storage device
+  if (!cacheFlush()) return false;
+
+  // open entry
+  return open(empty, oflag);
+}
+//------------------------------------------------------------------------------
+/**
+ * Open a file by file index.
+ *
+ * \param[in] index The root directory index of the file to be opened.  See \link
+ *  Fat16::readDir() readDir()\endlink.
+ *
+ * \param[in] oflag  See \link Fat16::open(const char*, uint8_t)\endlink.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include the FAT volume has not been initialized,
+ * a file is already open, \a index is invalid or is not the index of a
+ * file or the file cannot be opened in the access mode specified by oflag.
+ */
+uint8_t Fat16::open(uint16_t index, uint8_t oflag) {
+  if (!volumeInitialized_ || isOpen()) return false;
+  if ((oflag & O_TRUNC) && !(oflag & O_WRITE)) return false;
+  dir_t* d = cacheDirEntry(index);
+  // if bad file index or I/O error
+  if (!d) return false;
+
+  // error if unused entry
+  if (d->name[0] == DIR_NAME_FREE || d->name[0] == DIR_NAME_DELETED) {
+    return false;
+  }
+  // error if long name, volume label or subdirectory
+  if ((d->attributes & (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY)) != 0) {
+    return false;
+  }
+  // don't allow write or truncate if read-only
+  if (d->attributes & DIR_ATT_READ_ONLY) {
+    if (oflag & (O_WRITE | O_TRUNC)) return false;
+  }
+
+  curCluster_ = 0;
+  curPosition_ = 0;
+  dirEntryIndex_ = index;
+  fileSize_ = d->fileSize;
+  firstCluster_ = d->firstClusterLow;
+  flags_ = oflag & (O_ACCMODE | O_SYNC | O_APPEND);
+
+  if (oflag & O_TRUNC ) return truncate(0);
+  return true;
+}
+//------------------------------------------------------------------------------
+/** %Print the name field of a directory entry in 8.3 format to Serial.
+ *
+ * \param[in] dir The directory structure containing the name.
+ * \param[in] width Blank fill name if length is less than \a width.
+ */
+void Fat16::printDirName(const dir_t& dir, uint8_t width) {
+  uint8_t w = 0;
+  for (uint8_t i = 0; i < 11; i++) {
+    if (dir.name[i] == ' ') continue;
+    if (i == 8) {
+      Serial.write('.');
+      w++;
+    }
+    Serial.write(dir.name[i]);
+    w++;
+  }
+  if (DIR_IS_SUBDIR(&dir)) {
+    Serial.write('/');
+    w++;
+  }
+  while (w < width) {
+    Serial.write(' ');
+    w++;
+  }
+}
+//------------------------------------------------------------------------------
+/** %Print a directory date field to Serial.
+ *
+ *  Format is yyyy-mm-dd.
+ *
+ * \param[in] fatDate The date field from a directory entry.
+ */
+void Fat16::printFatDate(uint16_t fatDate) {
+  Serial.print(FAT_YEAR(fatDate));
+  Serial.write('-');
+  printTwoDigits(FAT_MONTH(fatDate));
+  Serial.write('-');
+  printTwoDigits(FAT_DAY(fatDate));
+}
+//------------------------------------------------------------------------------
+/** %Print a directory time field to Serial.
+ *
+ * Format is hh:mm:ss.
+ *
+ * \param[in] fatTime The time field from a directory entry.
+ */
+void Fat16::printFatTime(uint16_t fatTime) {
+  printTwoDigits(FAT_HOUR(fatTime));
+  Serial.write(':');
+  printTwoDigits(FAT_MINUTE(fatTime));
+  Serial.write(':');
+  printTwoDigits(FAT_SECOND(fatTime));
+}
+
+//------------------------------------------------------------------------------
+/** %Print a value as two digits to Serial.
+ *
+ * \param[in] v Value to be printed, 0 <= \a v <= 99
+ */
+void Fat16::printTwoDigits(uint8_t v) {
+  char str[3];
+  str[0] = '0' + v/10;
+  str[1] = '0' + v % 10;
+  str[2] = 0;
+  Serial.print(str);
+}
+//------------------------------------------------------------------------------
+/**
+ * Read the next byte from a file.
+ *
+ * \return For success read returns the next byte in the file as an int.
+ * If an error occurs or end of file is reached -1 is returned.
+ */
+int16_t Fat16::read(void) {
+  uint8_t b;
+  return read(&b, 1) == 1 ? b : -1;
+}
+//------------------------------------------------------------------------------
+/**
+ * Read data from a file at starting at the current file position.
+ *
+ * \param[out] buf Pointer to the location that will receive the data.
+ *
+ * \param[in] nbyte Maximum number of bytes to read.
+ *
+ * \return For success read returns the number of bytes read.
+ * A value less than \a nbyte, including zero, may be returned
+ * if end of file is reached.
+ * If an error occurs, read returns -1.  Possible errors include
+ * read called before a file has been opened, the file has not been opened in
+ * read mode, a corrupt file system, or an I/O error.
+ */
+int16_t Fat16::read(void* buf, uint16_t nbyte) {
+  // convert void pointer to uin8_t pointer
+  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
+
+  // error if not open for read
+  if (!(flags_ & O_READ)) return -1;
+
+  // don't read beyond end of file
+  if ((curPosition_ + nbyte) > fileSize_) nbyte = fileSize_ - curPosition_;
+
+  // bytes left to read in loop
+  uint16_t nToRead = nbyte;
+  while (nToRead > 0) {
+    uint8_t blkOfCluster = blockOfCluster(curPosition_);
+    uint16_t blockOffset = cacheDataOffset(curPosition_);
+    if (blkOfCluster == 0 && blockOffset == 0) {
+      // start next cluster
+      if (curCluster_ == 0) {
+        curCluster_ = firstCluster_;
+      } else {
+        if (!fatGet(curCluster_, &curCluster_)) return -1;
+      }
+      // return error if bad cluster chain
+      if (curCluster_ < 2 || isEOC(curCluster_)) return -1;
+    }
+    // cache data block
+    if (!cacheRawBlock(dataBlockLba(curCluster_, blkOfCluster))) return -1;
+
+    // location of data in cache
+    uint8_t* src = cacheBuffer_.data + blockOffset;
+
+    // max number of byte available in block
+    uint16_t n = 512 - blockOffset;
+
+    // lesser of available and amount to read
+    if (n > nToRead) n = nToRead;
+
+    // copy data to caller
+    memcpy(dst, src, n);
+
+    curPosition_ += n;
+    dst += n;
+    nToRead -= n;
+  }
+  return nbyte;
+}
+//------------------------------------------------------------------------------
+/**
+ *  Read the next short, 8.3, directory entry.
+ *
+ *  Unused entries and entries for long names are skipped.
+ *
+ * \param[out] dir Location that will receive the entry.
+ *
+ * \param[in,out] index The search starts at \a index and \a index is
+ * updated with the root directory index of the found directory entry.
+ * If the entry is a file, it may be opened by calling
+ * \link Fat16::open(uint16_t, uint8_t) \endlink.
+ *
+ * \param[in] skip Skip entries that have these attributes. If \a skip
+ * is not specified, the default is to skip the volume label and directories.
+ *
+ * \return The value one, true, is returned for success and the value zero,
+ * false, is returned if an error occurs or the end of the root directory is
+ * reached.  On success, \a entry is set to the index of the found directory
+ * entry.
+ */
+uint8_t Fat16::readDir(dir_t* dir, uint16_t* index, uint8_t skip) {
+  dir_t* p;
+  for (uint16_t i = *index; ; i++) {
+    if (i >= rootDirEntryCount_) return false;
+    if (!(p = cacheDirEntry(i))) return false;
+
+    // done if beyond last used entry
+    if (p->name[0] == DIR_NAME_FREE) return false;
+
+    // skip deleted entry
+    if (p->name[0] == DIR_NAME_DELETED) continue;
+
+    // skip long names
+    if ((p->attributes & DIR_ATT_LONG_NAME_MASK) == DIR_ATT_LONG_NAME) continue;
+
+    // skip if attribute match
+    if (p->attributes & skip) continue;
+
+    // return found index
+    *index = i;
+    break;
+  }
+  memcpy(dir, p, sizeof(dir_t));
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ * Remove a file.  The directory entry and all data for the file are deleted.
+ *
+ * \note This function should not be used to delete the 8.3 version of a
+ * file that has a long name. For example if a file has the long name
+ * "New Text Document.txt" you should not delete the 8.3 name "NEWTEX~1.TXT".
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include the file is not open for write
+ * or an I/O error occurred.
+ */
+uint8_t Fat16::remove(void) {
+  // error if file is not open for write
+  if (!(flags_ & O_WRITE)) return false;
+  if (firstCluster_) {
+    if (!freeChain(firstCluster_)) return false;
+  }
+  dir_t* d = cacheDirEntry(dirEntryIndex_, CACHE_FOR_WRITE);
+  if (!d) return false;
+  d->name[0] = DIR_NAME_DELETED;
+  flags_ = 0;
+  return cacheFlush();
+}
+//------------------------------------------------------------------------------
+/**
+ * Remove a file.
+ *
+ * The directory entry and all data for the file are deleted.
+ *
+ * \param[in] fileName The name of the file to be removed.
+ *
+ * \note This function should not be used to delete the 8.3 version of a
+ * file that has a long name. For example if a file has the long name
+ * "New Text Document.txt" you should not delete the 8.3 name "NEWTEX~1.TXT".
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include the file is read only, \a fileName is not found
+ * or an I/O error occurred.
+ */
+uint8_t Fat16::remove(const char* fileName) {
+  Fat16 file;
+  if (!file.open(fileName, O_WRITE)) return false;
+  return file.remove();
+}
+//------------------------------------------------------------------------------
+/**
+ * Sets the file's read/write position.
+ *
+ * \param[in] pos The new position in bytes from the beginning of the file.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t Fat16::seekSet(uint32_t pos) {
+  // error if file not open or seek past end of file
+  if (!isOpen() || pos > fileSize_) return false;
+  if (pos == 0) {
+    // set position to start of file
+    curCluster_ = 0;
+    curPosition_ = 0;
+    return true;
+  }
+  fat_t n = ((pos - 1) >> 9)/blocksPerCluster_;
+  if (pos < curPosition_ || curPosition_ == 0) {
+    // must follow chain from first cluster
+    curCluster_ = firstCluster_;
+  } else {
+    // advance from curPosition
+    n -= ((curPosition_ - 1) >> 9)/blocksPerCluster_;
+  }
+  while (n--) {
+    if (!fatGet(curCluster_, &curCluster_)) return false;
+  }
+  curPosition_ = pos;
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ *  The sync() call causes all modified data and directory fields
+ *  to be written to the storage device.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include a call to sync() before a file has been
+ * opened or an I/O error.
+ */
+uint8_t Fat16::sync(void) {
+  if (flags_ & F_FILE_DIR_DIRTY) {
+    // cache directory entry
+    dir_t* d = cacheDirEntry(dirEntryIndex_, CACHE_FOR_WRITE);
+    if (!d) return false;
+
+    // update file size and first cluster
+    d->fileSize = fileSize_;
+    d->firstClusterLow = firstCluster_;
+
+    // set modify time if user supplied a callback date/time function
+    if (dateTime_) {
+      dateTime_(&d->lastWriteDate, &d->lastWriteTime);
+      d->lastAccessDate = d->lastWriteDate;
+    }
+    flags_ &= ~F_FILE_DIR_DIRTY;
+  }
+  return cacheFlush();
+}
+//------------------------------------------------------------------------------
+/**
+ * The timestamp() call sets a file's timestamps in its directory entry.
+ *
+ * \param[in] flags Values for \a flags are constructed by a bitwise-inclusive
+ * OR of flags from the following list
+ *
+ * T_ACCESS - Set the file's last access date.
+ *
+ * T_CREATE - Set the file's creation date and time.
+ *
+ * T_WRITE - Set the file's last write/modification date and time.
+ *
+ * \param[in] year Valid range 1980 - 2107 inclusive.
+ *
+ * \param[in] month Valid range 1 - 12 inclusive.
+ *
+ * \param[in] day Valid range 1 - 31 inclusive.
+ *
+ * \param[in] hour Valid range 0 - 23 inclusive.
+ *
+ * \param[in] minute Valid range 0 - 59 inclusive.
+ *
+ * \param[in] second Valid range 0 - 59 inclusive
+ *
+ * \note It is possible to set an invalid date since there is no check for
+ * the number of days in a month.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t Fat16::timestamp(uint8_t flags, uint16_t year, uint8_t month,
+         uint8_t day, uint8_t hour, uint8_t minute, uint8_t second) {
+  if (!isOpen()
+    || year < 1980
+    || year > 2107
+    || month < 1
+    || month > 12
+    || day < 1
+    || day > 31
+    || hour > 23
+    || minute > 59
+    || second > 59) {
+      return false;
+  }
+  dir_t* d = cacheDirEntry(dirEntryIndex_, CACHE_FOR_WRITE);
+  if (!d) return false;
+  uint16_t dirDate = FAT_DATE(year, month, day);
+  uint16_t dirTime = FAT_TIME(hour, minute, second);
+  if (flags & T_ACCESS) {
+    d->lastAccessDate = dirDate;
+  }
+  if (flags & T_CREATE) {
+    d->creationDate = dirDate;
+    d->creationTime = dirTime;
+    // seems to be units of 1/100 second not 1/10 as Microsoft standard states
+    d->creationTimeTenths = second & 1 ? 100 : 0;
+  }
+  if (flags & T_WRITE) {
+    d->lastWriteDate = dirDate;
+    d->lastWriteTime = dirTime;
+  }
+  cacheSetDirty();
+  return sync();
+}
+//------------------------------------------------------------------------------
+/**
+ * Truncate a file to a specified length.  The current file position
+ * will be maintained if it is less than or equal to \a length otherwise
+ * it will be set to end of file.
+ *
+ * \param[in] length The desired length for the file.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ * Reasons for failure include file is read only, file is a directory,
+ * \a length is greater than the current file size or an I/O error occurs.
+ */
+uint8_t Fat16::truncate(uint32_t length) {
+  // error if file is not open for write
+  if (!(flags_ & O_WRITE)) return false;
+
+  if (length > fileSize_) return false;
+
+  // fileSize and length are zero - nothing to do
+  if (fileSize_ == 0) return true;
+  uint32_t newPos = curPosition_ > length ? length : curPosition_;
+  if (length == 0) {
+    // free all clusters
+    if (!freeChain(firstCluster_)) return false;
+    curCluster_ = firstCluster_ = 0;
+  } else {
+    fat_t toFree;
+    if (!seekSet(length)) return false;
+    if (!fatGet(curCluster_, &toFree)) return false;
+    if (!isEOC(toFree)) {
+      // free extra clusters
+      if (!fatPut(curCluster_, FAT16EOC)) return false;
+      if (!freeChain(toFree)) return false;
+    }
+  }
+  fileSize_ = length;
+  flags_ |= F_FILE_DIR_DIRTY;
+  if (!sync()) return false;
+  return seekSet(newPos);
+}
+//------------------------------------------------------------------------------
+/**
+ * Write data at the current position of an open file.
+ *
+ * \note Data is moved to the cache but may not be written to the
+ * storage device until sync() is called.
+ *
+ * \param[in] buf Pointer to the location of the data to be written.
+ *
+ * \param[in] nbyte Number of bytes to write.
+ *
+ * \return For success write() returns the number of bytes written, always
+ * \a nbyte.  If an error occurs, write() returns -1.  Possible errors include
+ * write() is called before a file has been opened, the file has not been opened
+ * for write, device is full, a corrupt file system or an I/O error.
+ *
+ */
+int16_t Fat16::write(const void* buf, uint16_t nbyte) {
+  uint16_t nToWrite = nbyte;
+  const uint8_t* src = reinterpret_cast<const uint8_t*>(buf);
+
+  // error if file is not open for write
+  if (!(flags_ & O_WRITE)) goto writeErrorReturn;
+
+  // go to end of file if O_APPEND
+  if ((flags_ & O_APPEND) && curPosition_ != fileSize_) {
+    if (!seekEnd()) goto writeErrorReturn;
+  }
+  while (nToWrite > 0) {
+    uint8_t blkOfCluster = blockOfCluster(curPosition_);
+    uint16_t blockOffset = cacheDataOffset(curPosition_);
+    if (blkOfCluster == 0 && blockOffset == 0) {
+      // start of new cluster
+      if (curCluster_ == 0) {
+        if (firstCluster_ == 0) {
+          // allocate first cluster of file
+          if (!addCluster()) goto writeErrorReturn;
+        } else {
+          curCluster_ = firstCluster_;
+        }
+      } else {
+        fat_t next;
+        if (!fatGet(curCluster_, &next)) goto writeErrorReturn;
+        if (isEOC(next)) {
+          // add cluster if at end of chain
+          if (!addCluster()) goto writeErrorReturn;
+        } else {
+          curCluster_ = next;
+        }
+      }
+    }
+    uint32_t lba = dataBlockLba(curCluster_, blkOfCluster);
+    if (blockOffset == 0 && curPosition_ >= fileSize_) {
+      // start of new block don't need to read into cache
+      if (!cacheFlush()) goto writeErrorReturn;
+      cacheBlockNumber_ = lba;
+      cacheSetDirty();
+    } else {
+      // rewrite part of block
+      if (!cacheRawBlock(lba, CACHE_FOR_WRITE)) return -1;
+    }
+    uint8_t* dst = cacheBuffer_.data + blockOffset;
+
+    // max space in block
+    uint16_t n = 512 - blockOffset;
+
+    // lesser of space and amount to write
+    if (n > nToWrite) n = nToWrite;
+
+    // copy data to cache
+    memcpy(dst, src, n);
+
+    curPosition_ += n;
+    nToWrite -= n;
+    src += n;
+  }
+  if (curPosition_ > fileSize_) {
+    // update fileSize and insure sync will update dir entry
+    fileSize_ = curPosition_;
+    flags_ |= F_FILE_DIR_DIRTY;
+  } else if (dateTime_ && nbyte) {
+    // insure sync will update modified date and time
+    flags_ |= F_FILE_DIR_DIRTY;
+  }
+
+  if (flags_ & O_SYNC) {
+    if (!sync()) goto writeErrorReturn;
+  }
+  return nbyte;
+
+ writeErrorReturn:
+  writeError = true;
+  return -1;
+}
+//------------------------------------------------------------------------------
+/**
+ * Write a byte to a file. Required by the Arduino Print class.
+ *
+ *  Use Fat16::writeError to check for errors.
+ */
+#if ARDUINO < 100
+void Fat16::write(uint8_t b) {
+  write(&b, 1);
+}
+#else  // ARDUINO < 100
+size_t Fat16::write(uint8_t b) {
+  return write(&b, 1) == 1 ? 1 : 0;
+}
+#endif  // ARDUINO < 100
+//------------------------------------------------------------------------------
+/**
+ * Write a string to a file. Used by the Arduino Print class.
+ *
+ * Use Fat16::writeError to check for errors.
+ */
+#if ARDUINO < 100
+void Fat16::write(const char* str) {
+  write(str, strlen(str));
+}
+#else  // ARDUINO < 100
+int16_t Fat16::write(const char* str) {
+  return write(str, strlen(str));
+}
+#endif  // ARDUINO < 100
+//------------------------------------------------------------------------------
+/**
+ * Write a PROGMEM string to a file.
+ *
+ * Use Fat16::writeError to check for errors.
+ */
+void Fat16::write_P(PGM_P str) {
+  for (uint8_t c; (c = pgm_read_byte(str)); str++) write(c);
+}
+//------------------------------------------------------------------------------
+/**
+ * Write a PROGMEM string followed by CR/LF to a file.
+ *
+ * Use Fat16::writeError to check for errors.
+ */
+void Fat16::writeln_P(PGM_P str) {
+  write_P(str);
+  println();
+}
diff --git a/libraries/Robot_Control/Fat16.h b/libraries/Robot_Control/Fat16.h
new file mode 100644
index 0000000..935b9b0
--- /dev/null
+++ b/libraries/Robot_Control/Fat16.h
@@ -0,0 +1,378 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#ifndef Fat16_h
+#define Fat16_h
+/**
+ * \file
+ * Fat16 class
+ */
+#include <string.h>
+#include <avr/pgmspace.h>
+#include <Print.h>
+#include <SdCard.h>
+#include <FatStructs.h>
+#include <Fat16Config.h>
+//------------------------------------------------------------------------------
+/** Fat16 version YYYYMMDD */
+#define FAT16_VERSION 20111205
+//------------------------------------------------------------------------------
+// flags for ls()
+/** ls() flag to print modify date */
+uint8_t const LS_DATE = 1;
+/** ls() flag to print file size */
+uint8_t const LS_SIZE = 2;
+
+// use the gnu style oflags
+/** open for reading */
+uint8_t const O_READ    = 0X01;
+/** same as O_READ */
+uint8_t const O_RDONLY  = O_READ;
+/** open for write */
+uint8_t const O_WRITE   = 0X02;
+/** same as O_WRITE */
+uint8_t const O_WRONLY  = O_WRITE;
+/** open for reading and writing */
+uint8_t const O_RDWR    = O_READ | O_WRITE;
+/** mask for access modes */
+uint8_t const O_ACCMODE = O_READ | O_WRITE;
+/** The file offset shall be set to the end of the file prior to each write. */
+uint8_t const O_APPEND  = 0X04;
+/** synchronous writes - call sync() after each write */
+uint8_t const O_SYNC    = 0X08;
+/** create the file if nonexistent */
+uint8_t const O_CREAT   = 0X10;
+/** If O_CREAT and O_EXCL are set, open() shall fail if the file exists */
+uint8_t const O_EXCL    = 0X20;
+/** truncate the file to zero length */
+uint8_t const O_TRUNC   = 0X40;
+
+// flags for timestamp
+/** set the file's last access date */
+uint8_t const T_ACCESS = 1;
+/** set the file's creation date and time */
+uint8_t const T_CREATE = 2;
+/** Set the file's write date and time */
+uint8_t const T_WRITE  = 4;
+
+/** date field for FAT directory entry */
+static inline uint16_t FAT_DATE(uint16_t year, uint8_t month, uint8_t day) {
+  return (year - 1980) << 9 | month << 5 | day;
+}
+/** year part of FAT directory date field */
+static inline uint16_t FAT_YEAR(uint16_t fatDate) {
+  return 1980 + (fatDate >> 9);
+}
+/** month part of FAT directory date field */
+static inline uint8_t FAT_MONTH(uint16_t fatDate) {
+  return (fatDate >> 5) & 0XF;
+}
+/** day part of FAT directory date field */
+static inline uint8_t FAT_DAY(uint16_t fatDate) {
+  return fatDate & 0X1F;
+}
+/** time field for FAT directory entry */
+static inline uint16_t FAT_TIME(uint8_t hour, uint8_t minute, uint8_t second) {
+  return hour << 11 | minute << 5 | second >> 1;
+}
+/** hour part of FAT directory time field */
+static inline uint8_t FAT_HOUR(uint16_t fatTime) {
+  return fatTime >> 11;
+}
+/** minute part of FAT directory time field */
+static inline uint8_t FAT_MINUTE(uint16_t fatTime) {
+  return(fatTime >> 5) & 0X3F;
+}
+/** second part of FAT directory time field */
+static inline uint8_t FAT_SECOND(uint16_t fatTime) {
+  return 2*(fatTime & 0X1F);
+}
+/** Default date for file timestamps is 1 Jan 2000 */
+uint16_t const FAT_DEFAULT_DATE = ((2000 - 1980) << 9) | (1 << 5) | 1;
+/** Default time for file timestamp is 1 am */
+uint16_t const FAT_DEFAULT_TIME = (1 << 11);
+//------------------------------------------------------------------------------
+/**
+ * \typedef fat_t
+ *
+ * \brief Type for FAT16 entry
+ */
+typedef uint16_t fat_t;
+/**
+ * \union cache16_t
+ *
+ * \brief Cache buffer data type
+ *
+ */
+union cache16_t {
+          /** Used to access cached file data blocks. */
+  uint8_t data[512];
+          /** Used to access cached FAT entries. */
+  fat_t   fat[256];
+          /** Used to access cached directory entries. */
+  dir_t   dir[16];
+          /** Used to access a cached Master Boot Record. */
+  mbr_t   mbr;
+          /** Used to access to a cached FAT16 boot sector. */
+  fbs_t   fbs;
+};
+//------------------------------------------------------------------------------
+/** \class Fat16
+ * \brief Fat16 implements a minimal Arduino FAT16 Library
+ *
+ * Fat16 does not support subdirectories or long file names.
+ */
+class Fat16 : public Print {
+ public:
+  /*
+   * Public functions
+   */
+  /** create with file closed */
+  Fat16(void) : flags_(0) {}
+  /** \return The current cluster number. */
+  fat_t curCluster(void) const {return curCluster_;}
+  uint8_t close(void);
+  /** \return The count of clusters in the FAT16 volume. */
+  static fat_t clusterCount(void) {return clusterCount_;}
+  /** \return The number of 512 byte blocks in a cluster */
+  static uint8_t clusterSize(void) {return blocksPerCluster_;}
+  /** \return The current file position. */
+  uint32_t curPosition(void) const {return curPosition_;}
+  /**
+   * Set the date/time callback function
+   *
+   * \param[in] dateTime The user's callback function.  The callback
+   * function is of the form:
+   *
+   * \code
+   * void dateTime(uint16_t* date, uint16_t* time) {
+   *   uint16_t year;
+   *   uint8_t month, day, hour, minute, second;
+   *
+   *   // User gets date and time from GPS or real-time clock here
+   *
+   *   // return date using FAT_DATE macro to format fields
+   *   *date = FAT_DATE(year, month, day);
+   *
+   *   // return time using FAT_TIME macro to format fields
+   *   *time = FAT_TIME(hour, minute, second);
+   * }
+   * \endcode
+   *
+   * Sets the function that is called when a file is created or when
+   * a file's directory entry is modified by sync(). All timestamps,
+   * access, creation, and modify, are set when a file is created.
+   * sync() maintains the last access date and last modify date/time.
+   *
+   * See the timestamp() function.
+   */
+  static void dateTimeCallback(
+    void (*dateTime)(uint16_t* date, uint16_t* time)) {
+    dateTime_ = dateTime;
+  }
+  /**
+   * Cancel the date/time callback function.
+   */
+  static void dateTimeCallbackCancel(void) {dateTime_ = NULL;}
+  uint8_t dirEntry(dir_t* dir);
+
+  /** \return The file's size in bytes. */
+  uint32_t fileSize(void) const {return fileSize_;}
+  static uint8_t init(SdCard* dev, uint8_t part);
+  /**
+   *  Initialize a FAT16 volume.
+   *
+   *  First try partition 1 then try super floppy format.
+   *
+   * \param[in] dev The SdCard where the volume is located.
+   *
+   * \return The value one, true, is returned for success and
+   * the value zero, false, is returned for failure.  reasons for
+   * failure include not finding a valid FAT16 file system, a call
+   * to init() after a volume has been successful initialized or
+   * an I/O error.
+   *
+   */
+  static uint8_t init(SdCard* dev) {
+    return init(dev, 1) ? true : init(dev, 0);
+  }
+  /**
+   * Checks the file's open/closed status for this instance of Fat16.
+   * \return The value true if a file is open otherwise false;
+   */
+  uint8_t isOpen(void) const {return (flags_ & O_ACCMODE) != 0;}
+  static void ls(uint8_t flags = 0);
+  uint8_t open(const char* fileName, uint8_t oflag);
+  uint8_t open(uint16_t entry, uint8_t oflag);
+  static void printDirName(const dir_t& dir, uint8_t width);
+  static void printFatDate(uint16_t fatDate);
+  static void printFatTime(uint16_t fatTime);
+  static void printTwoDigits(uint8_t v);
+  int16_t read(void);
+  int16_t read(void* buf, uint16_t nbyte);
+  static uint8_t readDir(dir_t* dir, uint16_t* index,
+                    uint8_t skip = (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY));
+
+  uint8_t remove(void);
+  static uint8_t remove(const char* fileName);
+  /** Sets the file's current position to zero. */
+  void rewind(void) {curPosition_ = curCluster_ = 0;}
+  /** \return The number of entries in the root directory. */
+  static uint16_t rootDirEntryCount(void) {return rootDirEntryCount_;}
+  /**  Seek to current position plus \a pos bytes. See Fat16::seekSet(). */
+  uint8_t seekCur(uint32_t pos) {return seekSet(curPosition_ + pos);}
+  /** Seek to end of file.  See Fat16::seekSet(). */
+  uint8_t seekEnd(void) {return seekSet(fileSize_);}
+  uint8_t seekSet(uint32_t pos);
+  uint8_t sync(void);
+  uint8_t timestamp(uint8_t flag, uint16_t year, uint8_t month, uint8_t day,
+          uint8_t hour, uint8_t minute, uint8_t second);
+  uint8_t truncate(uint32_t size);
+  /** Fat16::writeError is set to true if an error occurs during a write().
+   * Set Fat16::writeError to false before calling print() and/or write() and check
+   * for true after calls to write() and/or print().
+   */
+  bool writeError;
+  int16_t write(const void *buf, uint16_t nbyte);
+#if ARDUINO < 100
+  void write(uint8_t b);
+  void write(const char* str);
+#else  // ARDUINO < 100
+  size_t write(uint8_t b);
+  int16_t write(const char* str);
+#endif  // ARDUINO < 100
+  void write_P(PGM_P str);
+  void writeln_P(PGM_P str);
+//------------------------------------------------------------------------------
+#if FAT16_DEBUG_SUPPORT
+  /** For debug only.  Do not use in applications. */
+  static cache16_t* dbgBufAdd(void) {return &cacheBuffer_;}
+  /** For debug only.  Do not use in applications. */
+  static void dbgSetDev(SdCard* dev) {rawDev_ = dev;}
+  /** For debug only.  Do not use in applications. */
+  static uint8_t* dbgCacheBlock(uint32_t blockNumber) {
+    return cacheRawBlock(blockNumber) ? cacheBuffer_.data : 0; }
+  /** For debug only.  Do not use in applications. */
+  static dir_t* dbgCacheDir(uint16_t index) {
+    return cacheDirEntry(index);}
+#endif  // FAT16_DEBUG_SUPPORT
+//------------------------------------------------------------------------------
+#if ALLOW_DEPRECATED_FUNCTIONS
+// Deprecated functions - suppress cpplint messages with NOLINT comment
+ public:
+  /**
+   * Deprecated - Use:
+   * static void Fat16::dateTimeCallback(
+   *        void (*dateTime)(uint16_t* date, uint16_t* time));
+   */
+  static void dateTimeCallback(
+    void (*dateTime)(uint16_t& date, uint16_t& time)) {  // NOLINT
+    oldDateTime_ = dateTime;
+    dateTime_ = dateTime ? oldToNew : 0;
+  }
+  /** Deprecated - Use: uint8_t Fat16::dirEntry(dir_t* dir); */
+  uint8_t dirEntry(dir_t& dir) {  // NOLINT
+    return dirEntry(&dir);
+  }
+  /** Deprecated - Use: static uint8_t Fat16::init(SdCard *dev); */
+  static uint8_t init(SdCard& dev) {return init(&dev);}  // NOLINT
+
+  /** Deprecated - Use: static uint8_t Fat16::init(SdCard *dev, uint8_t part) */
+  static uint8_t init(SdCard& dev, uint8_t part) {  // NOLINT
+    return init(&dev, part);
+  }
+  /**
+   * Deprecated - Use:
+   * uint8_t Fat16::readDir(dir_t* dir, uint16_t* index, uint8_t skip);
+   */
+  static uint8_t readDir(dir_t& dir, uint16_t& index,  // NOLINT
+         uint8_t skip = (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY)) {
+    return readDir(&dir, &index, skip);
+  }
+//------------------------------------------------------------------------------
+ private:
+  static void (*oldDateTime_)(uint16_t& date, uint16_t& time);  // NOLINT
+  static void oldToNew(uint16_t *date, uint16_t *time) {
+    uint16_t d;
+    uint16_t t;
+    oldDateTime_(d, t);
+    *date = d;
+    *time = t;
+  }
+#endif  // ALLOW_DEPRECATED_FUNCTIONS
+//------------------------------------------------------------------------------
+ private:
+  // Volume info
+  static uint8_t  volumeInitialized_;  // true if volume has been initialized
+  static uint8_t  fatCount_;           // number of FATs
+  static uint8_t  blocksPerCluster_;   // must be power of 2
+  static uint16_t rootDirEntryCount_;  // should be 512 for FAT16
+  static fat_t    blocksPerFat_;       // number of blocks in one FAT
+  static fat_t    clusterCount_;       // total clusters in volume
+  static uint32_t fatStartBlock_;      // start of first FAT
+  static uint32_t rootDirStartBlock_;  // start of root dir
+  static uint32_t dataStartBlock_;     // start of data clusters
+
+  // block cache
+  static uint8_t const CACHE_FOR_READ  = 0;    // cache a block for read
+  static uint8_t const CACHE_FOR_WRITE = 1;    // cache a block and set dirty
+  static SdCard *rawDev_;             // Device
+  static cache16_t cacheBuffer_;      // 512 byte cache for raw blocks
+  static uint32_t cacheBlockNumber_;  // Logical number of block in the cache
+  static uint8_t cacheDirty_;         // cacheFlush() will write block if true
+  static uint32_t cacheMirrorBlock_;  // mirror block for second FAT
+
+  // callback function for date/time
+  static void (*dateTime_)(uint16_t* date, uint16_t* time);
+
+  // define fields in flags_
+  static uint8_t const F_OFLAG = O_ACCMODE | O_APPEND | O_SYNC;
+  static uint8_t const F_FILE_DIR_DIRTY = 0X80;  // require sync directory entry
+
+  uint8_t flags_;          // see above for bit definitions
+  int16_t dirEntryIndex_;  // index of directory entry for open file
+  fat_t firstCluster_;     // first cluster of file
+  uint32_t fileSize_;      // fileSize
+  fat_t curCluster_;       // current cluster
+  uint32_t curPosition_;   // current byte offset
+
+  // private functions for cache
+  static uint8_t blockOfCluster(uint32_t position) {
+    // depends on blocks per cluster being power of two
+    return (position >> 9) & (blocksPerCluster_ - 1);
+  }
+  static uint16_t cacheDataOffset(uint32_t position) {return position & 0X1FF;}
+  static dir_t* cacheDirEntry(uint16_t index, uint8_t action = 0);
+  static uint8_t cacheRawBlock(uint32_t blockNumber, uint8_t action = 0);
+  static uint8_t cacheFlush(void);
+  static void cacheSetDirty(void) {cacheDirty_ |= CACHE_FOR_WRITE;}
+  static uint32_t dataBlockLba(fat_t cluster, uint8_t blockOfCluster) {
+    return dataStartBlock_ + (uint32_t)(cluster - 2) * blocksPerCluster_
+      + blockOfCluster;
+  }
+  static uint8_t fatGet(fat_t cluster, fat_t* value);
+  static uint8_t fatPut(fat_t cluster, fat_t value);
+  // end of chain test
+  static uint8_t isEOC(fat_t cluster) {return cluster >= 0XFFF8;}
+  // allocate a cluster to a file
+  uint8_t addCluster(void);
+  // free a cluster chain
+  uint8_t freeChain(fat_t cluster);
+};
+#endif  // Fat16_h
diff --git a/libraries/Robot_Control/Fat16Config.h b/libraries/Robot_Control/Fat16Config.h
new file mode 100644
index 0000000..d598b56
--- /dev/null
+++ b/libraries/Robot_Control/Fat16Config.h
@@ -0,0 +1,38 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+ /**
+  * \file
+  * Configuration file
+  */
+#ifndef Fat16Config_h
+#define Fat16Config_h
+/**
+ * Allow use of deprecated functions if non-zero
+ */
+#define ALLOW_DEPRECATED_FUNCTIONS 1
+/**
+ * SdCard::writeBlock will protect block zero if set non-zero
+ */
+#define SD_PROTECT_BLOCK_ZERO 1
+/**
+ * Set non-zero to allow access to Fat16 internals by cardInfo debug sketch
+ */
+#define FAT16_DEBUG_SUPPORT 1
+#endif  // Fat16Config_h
diff --git a/libraries/Robot_Control/Fat16mainpage.h b/libraries/Robot_Control/Fat16mainpage.h
new file mode 100644
index 0000000..2c4f773
--- /dev/null
+++ b/libraries/Robot_Control/Fat16mainpage.h
@@ -0,0 +1,208 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *  
+ * This file is part of the Arduino FAT16 Library
+ *  
+ * This Library is free software: you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+ 
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+\mainpage Arduino Fat16 Library
+<CENTER>Copyright &copy; 2008 by William Greiman
+</CENTER>
+
+\section Intro Introduction
+The Arduino Fat16 Library is a minimal implementation of the FAT16 file system
+on standard SD flash memory cards.  Fat16 supports read, write, file
+creation, deletion, and truncation. 
+
+The Fat16 class only supports access to files in the root directory and only
+supports short 8.3 names.  Directory time and date fields for creation
+and modification can be maintained by providing a date/time callback
+function \link Fat16::dateTimeCallback() dateTimeCallback()\endlink
+or calling \link Fat16::timestamp() timestamp()\endlink.
+
+Fat16 was designed to use the Arduino Print class which
+allows files to be written with \link Print::print() print() \endlink and 
+\link Print::println()  println()\endlink.
+
+\section comment Bugs and Comments
+
+If you wish to report bugs or have comments, send email to fat16lib@sbcglobal.net.
+
+
+\section SDcard SD Cards
+
+Arduinos access SD cards using the cards SPI protocol.  PCs, Macs, and
+most consumer devices use the 4-bit parallel SD protocol.  A card that
+functions well on A PC or Mac may not work well on the Arduino.
+
+Most cards have good SPI read performance but cards vary widely in SPI
+write performance.  Write performance is limited by how efficiently the
+card manages internal erase/remapping operations.  The Arduino cannot
+optimize writes to reduce erase operations because of its limit RAM.
+
+SanDisk cards generally have good write performance.  They seem to have
+more internal RAM buffering than other cards and therefore can limit
+the number of flash erase operations that the Arduino forces due to its
+limited RAM.
+
+Some Dane-Elec cards have a write speed that is only 20% as fast as
+a good SanDisk card.
+
+
+\section Hardware Hardware Configuration
+Fat16 was developed using an <A HREF = "http://www.adafruit.com/"> Adafruit Industries</A> 
+<A HREF = "http://ladyada.net/make/gpsshield/modules.html"> GPS Shield</A>.
+
+The hardware interface to the SD card should not use a resistor based level
+shifter.  SdCard::init() sets the SPI bus frequency to 8 MHz which results in
+signal rise times that are too slow for the edge detectors in many newer SD card
+controllers when resistor voltage dividers are used.
+
+The 5 to 3.3 V level shifter for 5 V arduinos should be IC based like the
+74HC4050N based circuit shown in the file SdLevel.png.  The Adafruit Wave Shield
+uses a 74AHC125N.  Gravitech sells SD and MicroSD Card Adapters based on the
+74LCX245.
+
+If you are using a resistor based level shifter and are having problems try
+setting the SPI bus frequency to 4 MHz.  This can be done by using
+card.init(true) to initialize the SD card.
+
+
+\section Fat16Class Fat16 Usage
+
+The class Fat16 is a minimal implementation of FAT16 on standard SD cards.
+High Capacity SD cards, SDHC, are not supported. It should work on all
+standard cards from 8MB to 2GB formatted with a FAT16 file system.
+
+\note 
+  The Arduino Print class uses character
+at a time writes so it was necessary to use a \link Fat16::sync() sync() \endlink
+function to control when data is written to the SD card.
+
+\par
+An application which writes to a file using \link Print::print() print()\endlink, 
+\link Print::println() println() \endlink 
+or \link Fat16::write write() \endlink must call \link Fat16::sync() sync() \endlink 
+at the appropriate time to force data and directory information to be written 
+to the SD Card.  Data and directory information are also written to the SD card
+when \link Fat16::close() close() \endlink is called.
+
+\par 
+Applications must use care calling \link Fat16::sync() sync() \endlink
+since 2048 bytes of I/O is required to update file and
+directory information.  This includes writing the current data block, reading
+the block that contains the directory entry for update, writing the directory
+block back and reading back the current data block.
+
+Fat16 only supports access to files in the root directory and only supports
+short 8.3 names.
+
+It is possible to open a file with two or more instances of Fat16.  A file may
+be corrupted if data is written to the file by more than one instance of Fat16.
+
+Short names are limited to 8 characters followed by an optional period (.)
+and extension of up to 3 characters.  The characters may be any combination
+of letters and digits.  The following special characters are also allowed:
+
+$ % ' - _ @ ~ ` ! ( ) { } ^ # &
+
+Short names are always converted to upper case and their original case
+value is lost.  
+
+Fat16 uses a slightly restricted form of short names.
+Only printable ASCII characters are supported. No characters with code point
+values greater than 127 are allowed.  Space is not allowed even though space 
+was allowed in the API of early versions of DOS.
+
+Fat16 has been optimized for The Arduino ATmega168.  Minimizing RAM use is the
+highest priority goal followed by flash use and finally performance.  
+Most SD cards only support 512 byte block write operations so a 512 byte
+cache buffer is used by Fat16.  This is the main use of RAM.  A small
+amount of RAM is used to store key volume and file information.
+Flash memory usage can be controlled by selecting options in Fat16Config.h.
+
+\section HowTo How to format SD Cards as FAT16 Volumes
+
+Microsoft operating systems support removable media formatted with a 
+Master Boot Record, MBR, or formatted as a super floppy with a FAT Boot Sector
+in block zero.
+
+Microsoft operating systems expect MBR formatted removable media 
+to have only one partition. The first partition should be used.
+
+Microsoft operating systems do not support partitioning SD flash cards.
+If you erase an SD card with a program like KillDisk, Most versions of
+Windows will format the card as a super floppy.
+
+The best way to restore an SD card's MBR is to use SDFormatter 
+which can be downloaded from:
+
+http://www.sdcard.org/consumers/formatter/
+
+SDFormatter does not have an option for FAT type so it may format
+small cards as FAT12.
+
+After the MBR is restored by SDFormatter you may need to reformat small
+cards that have been formatted FAT12 to force the volume type to be FAT16. 
+
+The FAT type, FAT12, FAT16, or FAT32, is determined by the count 
+of clusters on the volume and nothing else.
+
+Microsoft published the following code for determining FAT type:
+
+\code
+if (CountOfClusters < 4085) {
+  // Volume is FAT12
+}
+else if (CountOfClusters < 65525) {
+  // Volume is FAT16
+}
+else {
+  // Volume is FAT32
+}
+
+\endcode
+If you format a FAT volume with an OS utility , choose a cluster size that 
+will result in:
+
+4084 < CountOfClusters && CountOfClusters < 65525
+
+The volume will then be FAT16.
+
+If you are formatting an SD card on OS X or Linux, be sure to use the first 
+partition. Format this partition with a cluster count in above range.
+
+\section  References References
+
+The Arduino site:
+
+http://www.arduino.cc/
+
+For more information about FAT file systems see:
+
+http://www.microsoft.com/whdc/system/platform/firmware/fatgen.mspx
+
+For information about using SD cards as SPI devices see:
+
+http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+
+The ATmega328 datasheet:
+
+http://www.atmel.com/dyn/resources/prod_documents/doc8161.pdf
+ 
+
+ */  
\ No newline at end of file
diff --git a/libraries/Robot_Control/Fat16util.h b/libraries/Robot_Control/Fat16util.h
new file mode 100644
index 0000000..1fea068
--- /dev/null
+++ b/libraries/Robot_Control/Fat16util.h
@@ -0,0 +1,74 @@
+#ifndef Fat16util_h
+#define Fat16util_h
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * \file
+ * Useful utility functions.
+ */
+#if ARDUINO < 100
+#include <WProgram.h>
+#else  // ARDUINO
+#include <Arduino.h>
+#endif  // ARDUINO
+#include <avr/pgmspace.h>
+/** Store and print a string in flash memory.*/
+#define PgmPrint(x) SerialPrint_P(PSTR(x))
+/** Store and print a string in flash memory followed by a CR/LF.*/
+#define PgmPrintln(x) SerialPrintln_P(PSTR(x))
+/** Defined so doxygen works for function definitions. */
+#define NOINLINE __attribute__((noinline))
+//------------------------------------------------------------------------------
+/** Return the number of bytes currently free in RAM. */
+static int FreeRam(void) {
+  extern int  __bss_end;
+  extern int* __brkval;
+  int free_memory;
+  if (reinterpret_cast<int>(__brkval) == 0) {
+    // if no heap use from end of bss section
+    free_memory = reinterpret_cast<int>(&free_memory)
+                  - reinterpret_cast<int>(&__bss_end);
+  } else {
+    // use from top of stack to heap
+    free_memory = reinterpret_cast<int>(&free_memory)
+                  - reinterpret_cast<int>(__brkval);
+  }
+  return free_memory;
+}
+//------------------------------------------------------------------------------
+/**
+ * %Print a string in flash memory to the serial port.
+ *
+ * \param[in] str Pointer to string stored in flash memory.
+ */
+static NOINLINE void SerialPrint_P(PGM_P str) {
+  for (uint8_t c; (c = pgm_read_byte(str)); str++) Serial.write(c);
+}
+//------------------------------------------------------------------------------
+/**
+ * %Print a string in flash memory followed by a CR/LF.
+ *
+ * \param[in] str Pointer to string stored in flash memory.
+ */
+static NOINLINE void SerialPrintln_P(PGM_P str) {
+  SerialPrint_P(str);
+  Serial.println();
+}
+#endif  // #define Fat16util_h
diff --git a/libraries/Robot_Control/FatStructs.h b/libraries/Robot_Control/FatStructs.h
new file mode 100644
index 0000000..431bf30
--- /dev/null
+++ b/libraries/Robot_Control/FatStructs.h
@@ -0,0 +1,418 @@
+/* Arduino Fat16 Library
+ * Copyright (C) 2009 by William Greiman
+ *
+ * This file is part of the Arduino Fat16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#ifndef FatStructs_h
+#define FatStructs_h
+/**
+ * \file
+ * FAT file structures
+ */
+/*
+ * mostly from Microsoft document fatgen103.doc
+ * http://www.microsoft.com/whdc/system/platform/firmware/fatgen.mspx
+ */
+//------------------------------------------------------------------------------
+/** Value for byte 510 of boot block or MBR */
+uint8_t const BOOTSIG0 = 0X55;
+/** Value for byte 511 of boot block or MBR */
+uint8_t const BOOTSIG1 = 0XAA;
+//------------------------------------------------------------------------------
+/**
+ * \struct partitionTable
+ * \brief MBR partition table entry
+ *
+ * A partition table entry for a MBR formatted storage device.
+ * The MBR partition table has four entries.
+ */
+struct partitionTable {
+          /**
+           * Boot Indicator . Indicates whether the volume is the active
+           * partition.  Legal values include: 0X00. Do not use for booting.
+           * 0X80 Active partition.
+           */
+  uint8_t  boot;
+          /**
+            * Head part of Cylinder-head-sector address of the first block in
+            * the partition. Legal values are 0-255. Only used in old PC BIOS.
+            */
+  uint8_t  beginHead;
+          /**
+           * Sector part of Cylinder-head-sector address of the first block in
+           * the partition. Legal values are 1-63. Only used in old PC BIOS.
+           */
+  unsigned beginSector : 6;
+           /** High bits cylinder for first block in partition. */
+  unsigned beginCylinderHigh : 2;
+          /**
+           * Combine beginCylinderLow with beginCylinderHigh. Legal values
+           * are 0-1023.  Only used in old PC BIOS.
+           */
+  uint8_t  beginCylinderLow;
+          /**
+           * Partition type. See defines that begin with PART_TYPE_ for
+           * some Microsoft partition types.
+           */
+  uint8_t  type;
+          /**
+           * head part of cylinder-head-sector address of the last sector in the
+           * partition.  Legal values are 0-255. Only used in old PC BIOS.
+           */
+  uint8_t  endHead;
+          /**
+           * Sector part of cylinder-head-sector address of the last sector in
+           * the partition.  Legal values are 1-63. Only used in old PC BIOS.
+           */
+  unsigned endSector : 6;
+           /** High bits of end cylinder */
+  unsigned endCylinderHigh : 2;
+          /**
+           * Combine endCylinderLow with endCylinderHigh. Legal values
+           * are 0-1023.  Only used in old PC BIOS.
+           */
+  uint8_t  endCylinderLow;
+           /** Logical block address of the first block in the partition. */
+  uint32_t firstSector;
+           /** Length of the partition, in blocks. */
+  uint32_t totalSectors;
+};
+/** Type name for partitionTable */
+typedef struct partitionTable part_t;
+//------------------------------------------------------------------------------
+/**
+ * \struct masterBootRecord
+ *
+ * \brief Master Boot Record
+ *
+ * The first block of a storage device that is formatted with a MBR.
+ */
+struct masterBootRecord {
+           /** Code Area for master boot program. */
+  uint8_t  codeArea[440];
+           /** Optional WindowsNT disk signature. May contain more boot code. */
+  uint32_t diskSignature;
+           /** Usually zero but may be more boot code. */
+  uint16_t usuallyZero;
+           /** Partition tables. */
+  part_t   part[4];
+           /** First MBR signature byte. Must be 0X55 */
+  uint8_t  mbrSig0;
+           /** Second MBR signature byte. Must be 0XAA */
+  uint8_t  mbrSig1;
+};
+/** Type name for masterBootRecord */
+typedef struct masterBootRecord mbr_t;
+//------------------------------------------------------------------------------
+/** 
+ * \struct biosParmBlock
+ *
+ * \brief BIOS parameter block
+ * 
+ *  The BIOS parameter block describes the physical layout of a FAT volume.
+ */
+struct biosParmBlock {
+          /**
+           * Count of bytes per sector. This value may take on only the
+           * following values: 512, 1024, 2048 or 4096
+           */
+  uint16_t bytesPerSector;
+          /**
+           * Number of sectors per allocation unit. This value must be a
+           * power of 2 that is greater than 0. The legal values are
+           * 1, 2, 4, 8, 16, 32, 64, and 128.
+           */
+  uint8_t  sectorsPerCluster;
+          /**
+           * Number of sectors before the first FAT.
+           * This value must not be zero.
+           */
+  uint16_t reservedSectorCount;
+          /** The count of FAT data structures on the volume. This field should
+           *  always contain the value 2 for any FAT volume of any type.
+           */
+  uint8_t  fatCount;
+          /**
+          * For FAT12 and FAT16 volumes, this field contains the count of
+          * 32-byte directory entries in the root directory. For FAT32 volumes,
+          * this field must be set to 0. For FAT12 and FAT16 volumes, this
+          * value should always specify a count that when multiplied by 32
+          * results in a multiple of bytesPerSector.  FAT16 volumes should
+          * use the value 512.
+          */
+  uint16_t rootDirEntryCount;
+          /**
+           * This field is the old 16-bit total count of sectors on the volume.
+           * This count includes the count of all sectors in all four regions
+           * of the volume. This field can be 0; if it is 0, then totalSectors32
+           * must be non-zero.  For FAT32 volumes, this field must be 0. For
+           * FAT12 and FAT16 volumes, this field contains the sector count, and
+           * totalSectors32 is 0 if the total sector count fits
+           * (is less than 0x10000).
+           */
+  uint16_t totalSectors16;
+          /**
+           * This dates back to the old MS-DOS 1.x media determination and is
+           * no longer usually used for anything.  0xF8 is the standard value
+           * for fixed (non-removable) media. For removable media, 0xF0 is
+           * frequently used. Legal values are 0xF0 or 0xF8-0xFF.
+           */
+  uint8_t  mediaType;
+          /**
+           * Count of sectors occupied by one FAT on FAT12/FAT16 volumes.
+           * On FAT32 volumes this field must be 0, and sectorsPerFat32
+           * contains the FAT size count.
+           */
+  uint16_t sectorsPerFat16;
+           /** Sectors per track for interrupt 0x13. Not used otherwise. */
+  uint16_t sectorsPerTrtack;
+           /** Number of heads for interrupt 0x13.  Not used otherwise. */
+  uint16_t headCount;
+          /**
+           * Count of hidden sectors preceding the partition that contains this
+           * FAT volume. This field is generally only relevant for media
+           *  visible on interrupt 0x13.
+           */
+  uint32_t hidddenSectors;
+          /**
+           * This field is the new 32-bit total count of sectors on the volume.
+           * This count includes the count of all sectors in all four regions
+           * of the volume.  This field can be 0; if it is 0, then
+           * totalSectors16 must be non-zero.
+           */
+  uint32_t totalSectors32;
+          /**
+           * Count of sectors occupied by one FAT on FAT32 volumes.
+           */
+  uint32_t sectorsPerFat32;
+          /**
+           * This field is only defined for FAT32 media and does not exist on
+           * FAT12 and FAT16 media.
+           * Bits 0-3 -- Zero-based number of active FAT.
+           *             Only valid if mirroring is disabled.
+           * Bits 4-6 -- Reserved.
+           * Bit 7	-- 0 means the FAT is mirrored at runtime into all FATs.
+	         *        -- 1 means only one FAT is active; it is the one referenced in bits 0-3.
+           * Bits 8-15 	-- Reserved.
+           */
+  uint16_t fat32Flags;
+          /**
+           * FAT32 version. High byte is major revision number.
+           * Low byte is minor revision number. Only 0.0 define.
+           */
+  uint16_t fat32Version;
+          /**
+           * Cluster number of the first cluster of the root directory for FAT32.
+           * This usually 2 but not required to be 2.
+           */
+  uint32_t fat32RootCluster;
+          /**
+           * Sector number of FSINFO structure in the reserved area of the
+           * FAT32 volume. Usually 1.
+           */
+  uint16_t fat32FSInfo;
+          /**
+           * If non-zero, indicates the sector number in the reserved area
+           * of the volume of a copy of the boot record. Usually 6.
+           * No value other than 6 is recommended.
+           */
+  uint16_t fat32BackBootBlock;
+          /**
+           * Reserved for future expansion. Code that formats FAT32 volumes
+           * should always set all of the bytes of this field to 0.
+           */
+  uint8_t  fat32Reserved[12];
+};
+/** Type name for biosParmBlock */
+typedef struct biosParmBlock bpb_t;
+//------------------------------------------------------------------------------
+/**
+ * \struct fat32BootSector
+ *
+ * \brief Boot sector for a FAT16 or FAT32 volume.
+ * 
+ */  
+struct fat32BootSector {
+           /** X86 jmp to boot program */
+  uint8_t  jmpToBootCode[3];
+           /** informational only - don't depend on it */
+  char     oemName[8];
+           /** BIOS Parameter Block */
+  bpb_t    bpb;
+           /** for int0x13 use value 0X80 for hard drive */
+  uint8_t  driveNumber;
+           /** used by Windows NT - should be zero for FAT */
+  uint8_t  reserved1;
+           /** 0X29 if next three fields are valid */
+  uint8_t  bootSignature;
+           /** usually generated by combining date and time */
+  uint32_t volumeSerialNumber;
+           /** should match volume label in root dir */
+  char     volumeLabel[11];
+           /** informational only - don't depend on it */
+  char     fileSystemType[8];
+           /** X86 boot code */
+  uint8_t  bootCode[420];
+           /** must be 0X55 */
+  uint8_t  bootSectorSig0;
+           /** must be 0XAA */
+  uint8_t  bootSectorSig1;
+};
+//------------------------------------------------------------------------------
+// End Of Chain values for FAT entries
+/** FAT16 end of chain value used by Microsoft. */
+uint16_t const FAT16EOC = 0XFFFF;
+/** Minimum value for FAT16 EOC.  Use to test for EOC. */
+uint16_t const FAT16EOC_MIN = 0XFFF8;
+/** FAT32 end of chain value used by Microsoft. */
+uint32_t const FAT32EOC = 0X0FFFFFFF;
+/** Minimum value for FAT32 EOC.  Use to test for EOC. */
+uint32_t const FAT32EOC_MIN = 0X0FFFFFF8;
+/** Mask a for FAT32 entry. Entries are 28 bits. */
+uint32_t const FAT32MASK = 0X0FFFFFFF;
+
+/** Type name for fat32BootSector */
+typedef struct fat32BootSector fbs_t;
+//------------------------------------------------------------------------------
+/**
+ * \struct directoryEntry
+ * \brief FAT short directory entry
+ *
+ * Short means short 8.3 name, not the entry size.
+ *  
+ * Date Format. A FAT directory entry date stamp is a 16-bit field that is 
+ * basically a date relative to the MS-DOS epoch of 01/01/1980. Here is the
+ * format (bit 0 is the LSB of the 16-bit word, bit 15 is the MSB of the 
+ * 16-bit word):
+ *   
+ * Bits 9-15: Count of years from 1980, valid value range 0-127 
+ * inclusive (1980-2107).
+ *   
+ * Bits 5-8: Month of year, 1 = January, valid value range 1-12 inclusive.
+ *
+ * Bits 0-4: Day of month, valid value range 1-31 inclusive.
+ *
+ * Time Format. A FAT directory entry time stamp is a 16-bit field that has
+ * a granularity of 2 seconds. Here is the format (bit 0 is the LSB of the 
+ * 16-bit word, bit 15 is the MSB of the 16-bit word).
+ *   
+ * Bits 11-15: Hours, valid value range 0-23 inclusive.
+ * 
+ * Bits 5-10: Minutes, valid value range 0-59 inclusive.
+ *      
+ * Bits 0-4: 2-second count, valid value range 0-29 inclusive (0 - 58 seconds).
+ *   
+ * The valid time range is from Midnight 00:00:00 to 23:59:58.
+ */
+struct directoryEntry {
+           /**
+            * Short 8.3 name.
+            * The first eight bytes contain the file name with blank fill.
+            * The last three bytes contain the file extension with blank fill.
+            */
+  uint8_t  name[11];
+          /** Entry attributes.
+           *
+           * The upper two bits of the attribute byte are reserved and should
+           * always be set to 0 when a file is created and never modified or
+           * looked at after that.  See defines that begin with DIR_ATT_.
+           */
+  uint8_t  attributes;
+          /**
+           * Reserved for use by Windows NT. Set value to 0 when a file is
+           * created and never modify or look at it after that.
+           */
+  uint8_t  reservedNT;
+          /**
+           * The granularity of the seconds part of creationTime is 2 seconds
+           * so this field is a count of tenths of a second and its valid
+           * value range is 0-199 inclusive. (WHG note - seems to be hundredths)
+           */
+  uint8_t  creationTimeTenths;
+           /** Time file was created. */
+  uint16_t creationTime;
+           /** Date file was created. */
+  uint16_t creationDate;
+          /**
+           * Last access date. Note that there is no last access time, only
+           * a date.  This is the date of last read or write. In the case of
+           * a write, this should be set to the same date as lastWriteDate.
+           */
+  uint16_t lastAccessDate;
+          /**
+           * High word of this entry's first cluster number (always 0 for a
+           * FAT12 or FAT16 volume).
+           */
+  uint16_t firstClusterHigh;
+           /** Time of last write. File creation is considered a write. */
+  uint16_t lastWriteTime;
+           /** Date of last write. File creation is considered a write. */
+  uint16_t lastWriteDate;
+           /** Low word of this entry's first cluster number. */
+  uint16_t firstClusterLow;
+           /** 32-bit unsigned holding this file's size in bytes. */
+  uint32_t fileSize;
+};
+//------------------------------------------------------------------------------
+// Definitions for directory entries
+//
+/** Type name for directoryEntry */
+typedef struct directoryEntry dir_t;
+/** escape for name[0] = 0XE5 */
+uint8_t const DIR_NAME_0XE5 = 0X05;
+/** name[0] value for entry that is free after being "deleted" */
+uint8_t const DIR_NAME_DELETED = 0XE5;
+/** name[0] value for entry that is free and no allocated entries follow */
+uint8_t const DIR_NAME_FREE = 0X00;
+/** file is read-only */
+uint8_t const DIR_ATT_READ_ONLY = 0X01;
+/** File should hidden in directory listings */
+uint8_t const DIR_ATT_HIDDEN = 0X02;
+/** Entry is for a system file */
+uint8_t const DIR_ATT_SYSTEM = 0X04;
+/** Directory entry contains the volume label */
+uint8_t const DIR_ATT_VOLUME_ID = 0X08;
+/** Entry is for a directory */
+uint8_t const DIR_ATT_DIRECTORY = 0X10;
+/** Old DOS archive bit for backup support */
+uint8_t const DIR_ATT_ARCHIVE = 0X20;
+/** Test value for long name entry.  Test is
+  (d->attributes & DIR_ATT_LONG_NAME_MASK) == DIR_ATT_LONG_NAME. */
+uint8_t const DIR_ATT_LONG_NAME = 0X0F;
+/** Test mask for long name entry */
+uint8_t const DIR_ATT_LONG_NAME_MASK = 0X3F;
+/** defined attribute bits */
+uint8_t const DIR_ATT_DEFINED_BITS = 0X3F;
+/** Directory entry is part of a long name */
+static inline uint8_t DIR_IS_LONG_NAME(const dir_t* dir) {
+  return (dir->attributes & DIR_ATT_LONG_NAME_MASK) == DIR_ATT_LONG_NAME;
+}
+/** Mask for file/subdirectory tests */
+uint8_t const DIR_ATT_FILE_TYPE_MASK = (DIR_ATT_VOLUME_ID | DIR_ATT_DIRECTORY);
+/** Directory entry is for a file */
+static inline uint8_t DIR_IS_FILE(const dir_t* dir) {
+  return (dir->attributes & DIR_ATT_FILE_TYPE_MASK) == 0;
+}
+/** Directory entry is for a subdirectory */
+static inline uint8_t DIR_IS_SUBDIR(const dir_t* dir) {
+  return (dir->attributes & DIR_ATT_FILE_TYPE_MASK) == DIR_ATT_DIRECTORY;
+}
+/** Directory entry is for a file or subdirectory */
+static inline uint8_t DIR_IS_FILE_OR_SUBDIR(const dir_t* dir) {
+  return (dir->attributes & DIR_ATT_VOLUME_ID) == 0;
+}
+#endif  // FatStructs_h
diff --git a/libraries/Robot_Control/Melody.cpp b/libraries/Robot_Control/Melody.cpp
new file mode 100644
index 0000000..0341c55
--- /dev/null
+++ b/libraries/Robot_Control/Melody.cpp
@@ -0,0 +1,100 @@
+#include "ArduinoRobot.h"
+#include "SquawkSD.h"
+#include "Fat16.h"
+
+
+
+SQUAWK_CONSTRUCT_ISR(SQUAWK_PWM_PIN5);
+
+
+void RobotControl::beginSpeaker(uint16_t frequency){
+  SquawkSynth::begin(frequency);
+  SquawkSynth::play();
+  osc[2].vol = 0x7F;
+}
+
+void RobotControl::playNote(byte period, word length, char modifier) {
+  // Modifier . makes note length 2/3
+  if(modifier == '.') length = (length * 2) / 3;
+  // Set up the play frequency, 352800 is [sample_rate]=44100 * [tuning]=8.0
+  osc[2].freq = 352800 / period;
+  // Delay, silence, delay
+  delay(length);
+  osc[2].freq = 0;
+  delay(length);
+}
+
+void RobotControl::playMelody(char* script){
+  // Find length of play string
+  word length = strlen(script);
+  // Set the default note time
+  word time = 500;
+  // Loop through each character in the play string
+  for(int n = 0; n < length; n++) {
+    // Fetch the character AFTER the current one - it may contain a modifier
+    char modifier = script[n + 1];
+    // Fetch the current character and branch accordingly
+    switch(script[n]) {
+      // Notes
+      case 'c': playNote(214, time, modifier); break; // Play a  C
+      case 'C': playNote(202, time, modifier); break; // Play a  C#
+      case 'd': playNote(190, time, modifier); break; // Play a  D
+      case 'D': playNote(180, time, modifier); break; // Play a  D#
+      case 'e': playNote(170, time, modifier); break; // Play an F
+      case 'f': playNote(160, time, modifier); break; // Play an F
+      case 'F': playNote(151, time, modifier); break; // Play an F#
+      case 'g': playNote(143, time, modifier); break; // Play a  G
+      case 'G': playNote(135, time, modifier); break; // Play a  G#
+      case 'a': playNote(127, time, modifier); break; // Play an A
+      case 'A': playNote(120, time, modifier); break; // Play an A#
+      case 'b': playNote(113, time, modifier); break; // Play a  B
+      // Delay
+      case '-': playNote(0,   time, modifier); break; // Play a quiet note
+      // Note lengths
+      case '1': time = 1000; break; // Full note
+      case '2': time =  500; break; // Half note
+      case '4': time =  250; break; // Quarter note
+      case '8': time =   50; break; // Eigth note
+	  // Modifier '.' makes note length 2/3
+
+    }
+  }
+}
+
+void RobotControl::beep(int beep_length){
+  char scr1[]="8F";
+  char scr2[]="8Fe";
+  char scr3[]="1F";
+
+  switch (beep_length)
+  {
+  case BEEP_SIMPLE:
+  default:
+    playMelody(scr1);
+    break;
+
+  case BEEP_DOUBLE:
+	playMelody(scr2);
+    break;
+
+  case BEEP_LONG:
+    playMelody(scr3);
+  }
+
+}
+
+void RobotControl::tempoWrite(int tempo){
+	SquawkSynthSD::tempo(tempo);
+}
+void RobotControl::tuneWrite(float tune){
+	SquawkSynthSD::tune(tune);
+}
+
+void RobotControl::playFile(char* filename){
+  melody.open(filename,O_READ);
+  SquawkSynthSD::play(melody);
+}
+
+void RobotControl::stopPlayFile(){
+  melody.close();
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/Motors.cpp b/libraries/Robot_Control/Motors.cpp
new file mode 100644
index 0000000..9d5e8b0
--- /dev/null
+++ b/libraries/Robot_Control/Motors.cpp
@@ -0,0 +1 @@
+#include "ArduinoRobot.h"
#include "EasyTransfer2.h"


void RobotControl::motorsStop(){
	messageOut.writeByte(COMMAND_MOTORS_STOP);
	messageOut.sendData();
}
void RobotControl::motorsWrite(int speedLeft,int speedRight){
	messageOut.writeByte(COMMAND_RUN);
	messageOut.writeInt(speedLeft);
	messageOut.writeInt(speedRight);
	messageOut.sendData();
}
void RobotControl::motorsWritePct(int speedLeftPct, int speedRightPct){
	int16_t speedLeft=255*speedLeftPct;
	int16_t speedRight=255*speedRightPct;
	
	motorsWrite(speedLeft,speedRight);
}
void RobotControl::pointTo(int angle){
	int target=angle;
	uint8_t speed=80;
	target=target%360;
	if(target<0){
		target+=360;
	}
	int direction=angle;
	while(1){
		if(direction>0){
			motorsWrite(speed,-speed);//right
			delay(10);
		}else{
			motorsWrite(-speed,speed);//left
			delay(10);
		}
		int currentAngle=compassRead();
		int diff=target-currentAngle;
		if(diff<-180) 
			diff += 360;
		else if(diff> 180) 
			diff -= 360;
		direction=-diff;
		
		if(abs(diff)<5){
			motorsWrite(0,0);
			return;
		}
	}
}
void RobotControl::turn(int angle){
	int originalAngle=compassRead();
	int target=originalAngle+angle;
	pointTo(target);
	/*uint8_t speed=80;
	target=target%360;
	if(target<0){
		target+=360;
	}
	int direction=angle;
	while(1){
		if(direction>0){
			motorsWrite(speed,speed);//right
			delay(10);
		}else{
			motorsWrite(-speed,-speed);//left
			delay(10);
		}
		int currentAngle=compassRead();
		int diff=target-currentAngle;
		if(diff<-180) 
			diff += 360;
		else if(diff> 180) 
			diff -= 360;
		direction=-diff;
		
		if(abs(diff)<5){
			motorsWrite(0,0);
			return;
		}
	}*/
}

void RobotControl::moveForward(int speed){
	motorsWrite(speed,speed);
}
void RobotControl::moveBackward(int speed){
	motorsWrite(speed,speed);
}
void RobotControl::turnLeft(int speed){
	motorsWrite(speed,255);
}
void RobotControl::turnRight(int speed){
	motorsWrite(255,speed);
}



/*
int RobotControl::getIRrecvResult(){
	messageOut.writeByte(COMMAND_GET_IRRECV);
	messageOut.sendData();
	//delay(10);
	while(!messageIn.receiveData());
	
	if(messageIn.readByte()==COMMAND_GET_IRRECV_RE){
		return messageIn.readInt();
	}


	return -1;
}
*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/Multiplexer.cpp b/libraries/Robot_Control/Multiplexer.cpp
new file mode 100644
index 0000000..8f7d30e
--- /dev/null
+++ b/libraries/Robot_Control/Multiplexer.cpp
@@ -0,0 +1,37 @@
+#include "Multiplexer.h"
+
+void Multiplexer::begin(uint8_t* selectors, uint8_t Z, uint8_t length){
+	for(uint8_t i=0;i<length;i++){
+		this->selectors[i]=selectors[i];
+		pinMode(selectors[i],OUTPUT);
+	}
+	this->length=length;
+	this->pin_Z=Z;
+	pinMode(pin_Z,INPUT);
+}
+
+void Multiplexer::selectPin(uint8_t num){
+	for(uint8_t i=0;i<length;i++){
+		//Serial.print(bitRead(num,i));
+		digitalWrite(selectors[i],bitRead(num,i));
+	}
+	//Serial.println("");
+}
+
+int Multiplexer::getAnalogValue(){
+	return analogRead(pin_Z);
+}
+
+bool Multiplexer::getDigitalValue(){
+	return digitalRead(pin_Z);
+}
+
+int Multiplexer::getAnalogValueAt(uint8_t num){
+	selectPin(num);
+	return getAnalogValue();
+}
+
+bool Multiplexer::getDigitalValueAt(uint8_t num){
+	selectPin(num);
+	return getDigitalValue();
+}
diff --git a/libraries/Robot_Control/Multiplexer.h b/libraries/Robot_Control/Multiplexer.h
new file mode 100644
index 0000000..a0c4c5c
--- /dev/null
+++ b/libraries/Robot_Control/Multiplexer.h
@@ -0,0 +1,24 @@
+#ifndef Multiplexer_h
+#define Multiplexer_h
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+class Multiplexer{
+	public:
+		void begin(uint8_t* selectors, uint8_t Z, uint8_t length);
+		void selectPin(uint8_t num);
+		int getAnalogValue();
+		int getAnalogValueAt(uint8_t num);
+		bool getDigitalValue();
+		bool getDigitalValueAt(uint8_t num);
+	private:
+		uint8_t selectors[4];
+		uint8_t pin_Z;
+		uint8_t length;
+};
+
+#endif
diff --git a/libraries/Robot_Control/RobotSdCard.cpp b/libraries/Robot_Control/RobotSdCard.cpp
new file mode 100644
index 0000000..df833d2
--- /dev/null
+++ b/libraries/Robot_Control/RobotSdCard.cpp
@@ -0,0 +1,22 @@
+#include <ArduinoRobot.h>
+
+void RobotControl::beginSD(){
+  card.init();
+  file.init(&card);
+  melody.init(&card);
+}
+
+void RobotControl::_enableSD(){
+  DDRB = DDRB & 0xDF; //pinMode(CS_LCD,INPUT);
+  DDRB = DDRB | 0x10; //pinMode(CS_SD,OUTPUT);
+}
+
+/*
+void RobotControl::sdTest(){
+  file.open("Infor.txt",O_READ);
+  uint8_t buf[7];
+  char n;
+  while ((n = file.read(buf, sizeof(buf))) > 0) {
+    for (uint8_t i = 0; i < n; i++) Serial.write(buf[i]);
+  }
+}*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/SPI.cpp b/libraries/Robot_Control/SPI.cpp
new file mode 100644
index 0000000..5e48073
--- /dev/null
+++ b/libraries/Robot_Control/SPI.cpp
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
+ * SPI Master library for arduino.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#include "pins_arduino.h"
+#include "SPI.h"
+
+SPIClass SPI;
+
+void SPIClass::begin() {
+
+  // Set SS to high so a connected chip will be "deselected" by default
+  digitalWrite(SS, HIGH);
+
+  // When the SS pin is set as OUTPUT, it can be used as
+  // a general purpose output port (it doesn't influence
+  // SPI operations).
+  pinMode(SS, OUTPUT);
+
+  // Warning: if the SS pin ever becomes a LOW INPUT then SPI
+  // automatically switches to Slave, so the data direction of
+  // the SS pin MUST be kept as OUTPUT.
+  SPCR |= _BV(MSTR);
+  SPCR |= _BV(SPE);
+
+  // Set direction register for SCK and MOSI pin.
+  // MISO pin automatically overrides to INPUT.
+  // By doing this AFTER enabling SPI, we avoid accidentally
+  // clocking in a single bit since the lines go directly
+  // from "input" to SPI control.  
+  // http://code.google.com/p/arduino/issues/detail?id=888
+  pinMode(SCK, OUTPUT);
+  pinMode(MOSI, OUTPUT);
+}
+
+
+void SPIClass::end() {
+  SPCR &= ~_BV(SPE);
+}
+
+void SPIClass::setBitOrder(uint8_t bitOrder)
+{
+  if(bitOrder == LSBFIRST) {
+    SPCR |= _BV(DORD);
+  } else {
+    SPCR &= ~(_BV(DORD));
+  }
+}
+
+void SPIClass::setDataMode(uint8_t mode)
+{
+  SPCR = (SPCR & ~SPI_MODE_MASK) | mode;
+}
+
+void SPIClass::setClockDivider(uint8_t rate)
+{
+  SPCR = (SPCR & ~SPI_CLOCK_MASK) | (rate & SPI_CLOCK_MASK);
+  SPSR = (SPSR & ~SPI_2XCLOCK_MASK) | ((rate >> 2) & SPI_2XCLOCK_MASK);
+}
+
diff --git a/libraries/Robot_Control/SPI.h b/libraries/Robot_Control/SPI.h
new file mode 100644
index 0000000..f647d5c
--- /dev/null
+++ b/libraries/Robot_Control/SPI.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
+ * SPI Master library for arduino.
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _SPI_H_INCLUDED
+#define _SPI_H_INCLUDED
+
+#include <stdio.h>
+#include <Arduino.h>
+#include <avr/pgmspace.h>
+
+#define SPI_CLOCK_DIV4 0x00
+#define SPI_CLOCK_DIV16 0x01
+#define SPI_CLOCK_DIV64 0x02
+#define SPI_CLOCK_DIV128 0x03
+#define SPI_CLOCK_DIV2 0x04
+#define SPI_CLOCK_DIV8 0x05
+#define SPI_CLOCK_DIV32 0x06
+//#define SPI_CLOCK_DIV64 0x07
+
+#define SPI_MODE0 0x00
+#define SPI_MODE1 0x04
+#define SPI_MODE2 0x08
+#define SPI_MODE3 0x0C
+
+#define SPI_MODE_MASK 0x0C  // CPOL = bit 3, CPHA = bit 2 on SPCR
+#define SPI_CLOCK_MASK 0x03  // SPR1 = bit 1, SPR0 = bit 0 on SPCR
+#define SPI_2XCLOCK_MASK 0x01  // SPI2X = bit 0 on SPSR
+
+class SPIClass {
+public:
+  inline static byte transfer(byte _data);
+
+  // SPI Configuration methods
+
+  inline static void attachInterrupt();
+  inline static void detachInterrupt(); // Default
+
+  static void begin(); // Default
+  static void end();
+
+  static void setBitOrder(uint8_t);
+  static void setDataMode(uint8_t);
+  static void setClockDivider(uint8_t);
+};
+
+extern SPIClass SPI;
+
+byte SPIClass::transfer(byte _data) {
+  SPDR = _data;
+  while (!(SPSR & _BV(SPIF)))
+    ;
+  return SPDR;
+}
+
+void SPIClass::attachInterrupt() {
+  SPCR |= _BV(SPIE);
+}
+
+void SPIClass::detachInterrupt() {
+  SPCR &= ~_BV(SPIE);
+}
+
+#endif
diff --git a/libraries/Robot_Control/SdCard.cpp b/libraries/Robot_Control/SdCard.cpp
new file mode 100644
index 0000000..fbbd8bc
--- /dev/null
+++ b/libraries/Robot_Control/SdCard.cpp
@@ -0,0 +1,279 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#include <avr/pgmspace.h>
+#if ARDUINO < 100
+#include <WProgram.h>
+#else  // ARDUINO
+#include <Arduino.h>
+#endif  // ARDUINO
+#include <Fat16Config.h>
+#include <SdCard.h>
+//------------------------------------------------------------------------------
+// r1 status values
+uint8_t const R1_READY_STATE = 0;
+uint8_t const R1_IDLE_STATE  = 1;
+// start data token for read or write
+uint8_t const DATA_START_BLOCK = 0XFE;
+// data response tokens for write block
+uint8_t const DATA_RES_MASK        = 0X1F;
+uint8_t const DATA_RES_ACCEPTED    = 0X05;
+uint8_t const DATA_RES_CRC_ERROR   = 0X0B;
+uint8_t const DATA_RES_WRITE_ERROR = 0X0D;
+//
+// stop compiler from inlining where speed optimization is not required
+#define STATIC_NOINLINE static __attribute__((noinline))
+//------------------------------------------------------------------------------
+// SPI static functions
+//
+// clock byte in
+STATIC_NOINLINE uint8_t spiRec(void) {
+  SPDR = 0xff;
+  while (!(SPSR & (1 << SPIF)));
+  return SPDR;
+}
+// clock byte out
+STATIC_NOINLINE void spiSend(uint8_t b) {
+  SPDR = b;
+  while (!(SPSR & (1 << SPIF)));
+}
+//------------------------------------------------------------------------------
+// wait for card to go not busy
+// return false if timeout
+static uint8_t waitForToken(uint8_t token, uint16_t timeoutMillis) {
+  uint16_t t0 = millis();
+  while (spiRec() != token) {
+    if (((uint16_t)millis() - t0) > timeoutMillis) return false;
+  }
+  return true;
+}
+//------------------------------------------------------------------------------
+uint8_t SdCard::cardCommand(uint8_t cmd, uint32_t arg) {
+  uint8_t r1;
+
+  // select card
+  chipSelectLow();
+
+  // wait if busy
+  waitForToken(0XFF, SD_COMMAND_TIMEOUT);
+
+  // send command
+  spiSend(cmd | 0x40);
+
+  // send argument
+  for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
+
+  // send CRC - must send valid CRC for CMD0
+  spiSend(cmd == CMD0 ? 0x95 : 0XFF);
+
+  // wait for not busy
+  for (uint8_t retry = 0; (0X80 & (r1 = spiRec())) && retry != 0XFF; retry++);
+  return r1;
+}
+//------------------------------------------------------------------------------
+uint8_t SdCard::cardAcmd(uint8_t cmd, uint32_t arg) {
+  cardCommand(CMD55, 0);
+  return cardCommand(cmd, arg);
+}
+//==============================================================================
+// SdCard member functions
+//------------------------------------------------------------------------------
+/**
+ * Determine the size of a standard SD flash memory card
+ * \return The number of 512 byte data blocks in the card
+ */
+uint32_t SdCard::cardSize(void) {
+  uint16_t c_size;
+  csd_t csd;
+  if (!readReg(CMD9, &csd)) return 0;
+  uint8_t read_bl_len = csd.read_bl_len;
+  c_size = (csd.c_size_high << 10) | (csd.c_size_mid << 2) | csd.c_size_low;
+  uint8_t c_size_mult = (csd.c_size_mult_high << 1) | csd.c_size_mult_low;
+  return (uint32_t)(c_size+1) << (c_size_mult + read_bl_len - 7);
+}
+//------------------------------------------------------------------------------
+void SdCard::chipSelectHigh(void) {
+  digitalWrite(chipSelectPin_, HIGH);
+  // make sure MISO goes high impedance
+  spiSend(0XFF);
+}
+//------------------------------------------------------------------------------
+void SdCard::chipSelectLow(void) {
+  // Enable SPI, Master, clock rate F_CPU/4
+  SPCR = (1 << SPE) | (1 << MSTR);
+
+  // Doubled Clock Frequency to F_CPU/2 unless speed_ is nonzero
+  if (!speed_) SPSR |= (1 << SPI2X);
+
+  digitalWrite(chipSelectPin_, LOW);
+}
+//------------------------------------------------------------------------------
+void SdCard::error(uint8_t code, uint8_t data) {
+  errorData = data;
+  error(code);
+}
+//------------------------------------------------------------------------------
+void SdCard::error(uint8_t code) {
+  errorCode = code;
+  chipSelectHigh();
+}
+//------------------------------------------------------------------------------
+/**
+ * Initialize a SD flash memory card.
+ *
+ * \param[in] speed Set SPI Frequency to F_CPU/2 if speed = 0 or F_CPU/4
+ *  if speed = 1.
+ * \param[in] chipSelectPin SD chip select pin number.
+ *
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ *
+ */
+uint8_t SdCard::init(uint8_t speed, uint8_t chipSelectPin) {
+  if (speed > 1) {
+    error(SD_ERROR_SPI_SPEED);
+    return false;
+  }
+  speed_ = speed;
+  chipSelectPin_ = chipSelectPin;
+  errorCode = 0;
+  uint8_t r;
+  // 16-bit init start time allows over a minute
+  uint16_t t0 = (uint16_t)millis();
+
+  pinMode(chipSelectPin_, OUTPUT);
+  digitalWrite(chipSelectPin_, HIGH);
+  pinMode(SPI_MISO_PIN, INPUT);
+  pinMode(SPI_SS_PIN, OUTPUT);
+  pinMode(SPI_MOSI_PIN, OUTPUT);
+  pinMode(SPI_SCK_PIN, OUTPUT);
+
+  // Enable SPI, Master, clock rate F_CPU/128
+  SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (1 << SPR0);
+
+  // must supply min of 74 clock cycles with CS high.
+  for (uint8_t i = 0; i < 10; i++) spiSend(0XFF);
+  digitalWrite(chipSelectPin_, LOW);
+
+  // command to go idle in SPI mode
+  while ((r = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
+    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
+      error(SD_ERROR_CMD0, r);
+      return false;
+    }
+  }
+  // start initialization and wait for completed initialization
+  while ((r = cardAcmd(ACMD41, 0)) != R1_READY_STATE) {
+    if (((uint16_t)millis() - t0) > SD_INIT_TIMEOUT) {
+      error(SD_ERROR_ACMD41, r);
+      return false;
+    }
+  }
+  chipSelectHigh();
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ * Reads a 512 byte block from a storage device.
+ *
+ * \param[in] blockNumber Logical block to be read.
+ * \param[out] dst Pointer to the location that will receive the data.
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t SdCard::readBlock(uint32_t blockNumber, uint8_t* dst) {
+  if (cardCommand(CMD17, blockNumber << 9)) {
+    error(SD_ERROR_CMD17);
+    return false;
+  }
+  return readTransfer(dst, 512);
+}
+//------------------------------------------------------------------------------
+uint8_t SdCard::readReg(uint8_t cmd, void* buf) {
+  uint8_t* dst = reinterpret_cast<uint8_t*>(buf);
+  if (cardCommand(cmd, 0)) {
+    chipSelectHigh();
+    return false;
+  }
+  return readTransfer(dst, 16);
+}
+//------------------------------------------------------------------------------
+uint8_t SdCard::readTransfer(uint8_t* dst, uint16_t count) {
+  // wait for start of data
+  if (!waitForToken(DATA_START_BLOCK, SD_READ_TIMEOUT)) {
+    error(SD_ERROR_READ_TIMEOUT);
+  }
+  // start first spi transfer
+  SPDR = 0XFF;
+  for (uint16_t i = 0; i < count; i++) {
+    while (!(SPSR & (1 << SPIF)));
+    dst[i] = SPDR;
+    SPDR = 0XFF;
+  }
+  // wait for first CRC byte
+  while (!(SPSR & (1 << SPIF)));
+  spiRec();  // second CRC byte
+  chipSelectHigh();
+  return true;
+}
+//------------------------------------------------------------------------------
+/**
+ * Writes a 512 byte block to a storage device.
+ *
+ * \param[in] blockNumber Logical block to be written.
+ * \param[in] src Pointer to the location of the data to be written.
+ * \return The value one, true, is returned for success and
+ * the value zero, false, is returned for failure.
+ */
+uint8_t SdCard::writeBlock(uint32_t blockNumber, const uint8_t* src) {
+  uint32_t address = blockNumber << 9;
+#if SD_PROTECT_BLOCK_ZERO
+  // don't allow write to first block
+  if (address == 0) {
+    error(SD_ERROR_BLOCK_ZERO_WRITE);
+    return false;
+  }
+#endif  // SD_PROTECT_BLOCK_ZERO
+  if (cardCommand(CMD24, address)) {
+    error(SD_ERROR_CMD24);
+    return false;
+  }
+  // optimize write loop
+  SPDR = DATA_START_BLOCK;
+  for (uint16_t i = 0; i < 512; i++) {
+    while (!(SPSR & (1 << SPIF)));
+    SPDR = src[i];
+  }
+  while (!(SPSR & (1 << SPIF)));  // wait for last data byte
+  spiSend(0xFF);  // dummy crc
+  spiSend(0xFF);  // dummy crc
+
+  // get write response
+  uint8_t r1 = spiRec();
+  if ((r1 & DATA_RES_MASK) != DATA_RES_ACCEPTED) {
+    error(SD_ERROR_WRITE_RESPONSE, r1);
+    return false;
+  }
+  // wait for card to complete write programming
+  if (!waitForToken(0XFF, SD_WRITE_TIMEOUT)) {
+      error(SD_ERROR_WRITE_TIMEOUT);
+  }
+  chipSelectHigh();
+  return true;
+}
diff --git a/libraries/Robot_Control/SdCard.h b/libraries/Robot_Control/SdCard.h
new file mode 100644
index 0000000..c03e6ab
--- /dev/null
+++ b/libraries/Robot_Control/SdCard.h
@@ -0,0 +1,192 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#ifndef SdCard_h
+#define SdCard_h
+ /**
+  * \file
+  * SdCard class
+  */
+#include <SdInfo.h>
+//------------------------------------------------------------------------------
+// Warning only SD_CHIP_SELECT_PIN, the SD card select pin, may be redefined.
+// define hardware SPI pins
+#if defined(__AVR_ATmega168__)\
+||defined(__AVR_ATmega168P__)\
+||defined(__AVR_ATmega328P__)
+// 168 and 328 Arduinos
+/** Slave Select pin */
+uint8_t const SPI_SS_PIN   = 10;
+/** Master Out Slave In pin */
+uint8_t const SPI_MOSI_PIN = 11;
+/** Master In Slave Out pin */
+uint8_t const SPI_MISO_PIN = 12;
+/** Serial Clock */
+uint8_t const SPI_SCK_PIN  = 13;
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega1280__)\
+|| defined(__AVR_ATmega2560__)
+// pins for Arduino Mega
+uint8_t const SPI_SS_PIN   = 53;
+uint8_t const SPI_MOSI_PIN = 51;
+uint8_t const SPI_MISO_PIN = 50;
+uint8_t const SPI_SCK_PIN  = 52;
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega644P__)\
+|| defined(__AVR_ATmega644__)\
+|| defined(__AVR_ATmega1284P__)
+// pins for Sanguino
+uint8_t const SPI_SS_PIN   = 4;
+uint8_t const SPI_MOSI_PIN = 5;
+uint8_t const SPI_MISO_PIN = 6;
+uint8_t const SPI_SCK_PIN  = 7;
+//------------------------------------------------------------------------------
+#elif defined(__AVR_ATmega32U4__)
+// pins for Teensy 2.0
+uint8_t const SPI_SS_PIN   = 8;
+uint8_t const SPI_MOSI_PIN = 16;
+uint8_t const SPI_MISO_PIN = 14;
+uint8_t const SPI_SCK_PIN  = 15;
+//------------------------------------------------------------------------------
+#elif defined(__AVR_AT90USB646__)\
+|| defined(__AVR_AT90USB1286__)
+// pins for Teensy++ 1.0 & 2.0
+uint8_t const SPI_SS_PIN   = 20;
+uint8_t const SPI_MOSI_PIN = 22;
+uint8_t const SPI_MISO_PIN = 23;
+uint8_t const SPI_SCK_PIN  = 21;
+//------------------------------------------------------------------------------
+#else  // SPI pins
+#error unknown CPU
+#endif  // SPI pins
+//------------------------------------------------------------------------------
+/**
+ * SD Chip Select pin
+ *
+ * Warning if this pin is redefined the hardware SS pin will be enabled
+ * as an output by init().  An avr processor will not function as an SPI
+ * master unless SS is set to output mode.
+ *
+ * For example to set SD_CHIP_SELECT_PIN to 8 for the SparkFun microSD shield:
+ * uint8_t const SD_CHIP_SELECT_PIN = 8;
+ *
+ * The default chip select pin for the SD card is SS.
+ */
+uint8_t const SD_CHIP_SELECT_PIN = SPI_SS_PIN;
+//------------------------------------------------------------------------------
+/** command timeout ms */
+uint16_t const SD_COMMAND_TIMEOUT = 300;
+/** init timeout ms */
+uint16_t const SD_INIT_TIMEOUT    = 2000;
+/** read timeout ms */
+uint16_t const SD_READ_TIMEOUT    = 300;
+/** write timeout ms */
+uint16_t const SD_WRITE_TIMEOUT   = 600;
+//------------------------------------------------------------------------------
+// error codes
+/** Card did not go into SPI mode */
+uint8_t const SD_ERROR_CMD0              = 1;
+/** Card did not go ready */
+uint8_t const SD_ERROR_ACMD41            = 2;
+/** Write command not accepted */
+uint8_t const SD_ERROR_CMD24             = 3;
+/** Read command not accepted */
+uint8_t const SD_ERROR_CMD17             = 4;
+/** timeout waiting for read data */
+uint8_t const SD_ERROR_READ_TIMEOUT      = 5;
+/** write error occurred */
+uint8_t const SD_ERROR_WRITE_RESPONSE    = 6;
+/** timeout waiting for write status */
+uint8_t const SD_ERROR_WRITE_TIMEOUT     = 7;
+/** attempt to write block zero */
+uint8_t const SD_ERROR_BLOCK_ZERO_WRITE  = 8;
+/** card returned an error to a CMD13 status check after a write */
+uint8_t const SD_ERROR_WRITE_PROGRAMMING = 9;
+/** invalid SPI speed in init() call */
+uint8_t const SD_ERROR_SPI_SPEED         = 10;
+//------------------------------------------------------------------------------
+// SD command codes
+/** SEND OPERATING CONDITIONS */
+uint8_t const ACMD41   = 0X29;
+/** GO_IDLE_STATE - init card in spi mode if CS low */
+uint8_t const CMD0     = 0X00;
+/** SEND_CSD - Card Specific Data */
+uint8_t const CMD9     = 0X09;
+/** SEND_CID - Card IDentification */
+uint8_t const CMD10    = 0X0A;
+/** SEND_STATUS - read the card status register */
+uint8_t const CMD13    = 0X0D;
+/** READ_BLOCK */
+uint8_t const CMD17    = 0X11;
+/** WRITE_BLOCK */
+uint8_t const CMD24    = 0X18;
+/** APP_CMD - escape for application specific command */
+uint8_t const CMD55    = 0X37;
+//------------------------------------------------------------------------------
+/**
+ * \class SdCard
+ * \brief Hardware access class for SD flash cards
+ *
+ * Supports raw access to a standard SD flash memory card.
+ *
+ */
+class SdCard  {
+ public:
+  /** Code for a SD error. See SdCard.h for definitions. */
+  uint8_t errorCode;
+  /** Data that may be helpful in determining the cause of an error */
+  uint8_t errorData;
+  uint32_t cardSize(void);
+  /**
+   * Initialize an SD flash memory card with default clock rate and chip
+   * select pin.  See SdCard::init(uint8_t sckRateID, uint8_t chipSelectPin).
+   */
+  uint8_t init(void) {
+    return init(0, SD_CHIP_SELECT_PIN);
+  }
+  /**
+   * Initialize an SD flash memory card with the selected SPI clock rate
+   * and the default SD chip select pin.
+   * See SdCard::init(uint8_t slow, uint8_t chipSelectPin).
+   */
+  uint8_t init(uint8_t speed) {
+    return init(speed, SD_CHIP_SELECT_PIN);
+  }
+  uint8_t init(uint8_t speed, uint8_t chipselectPin);
+  uint8_t readBlock(uint32_t block, uint8_t* dst);
+  /** Read the CID register which contains info about the card.
+   *  This includes Manufacturer ID, OEM ID, product name, version,
+   *  serial number, and manufacturing date. */
+  uint8_t readCID(cid_t* cid) {
+    return readReg(CMD10, cid);
+  }
+  uint8_t writeBlock(uint32_t block, const uint8_t* src);
+ private:
+  uint8_t cardAcmd(uint8_t cmd, uint32_t arg);
+  uint8_t cardCommand(uint8_t cmd, uint32_t arg);
+  uint8_t chipSelectPin_;
+  uint8_t speed_;
+  void chipSelectHigh(void);
+  void chipSelectLow(void);
+  void error(uint8_t code, uint8_t data);
+  void error(uint8_t code);
+  uint8_t readReg(uint8_t cmd, void* buf);
+  uint8_t readTransfer(uint8_t* dst, uint16_t count);
+};
+#endif  // SdCard_h
diff --git a/libraries/Robot_Control/SdInfo.h b/libraries/Robot_Control/SdInfo.h
new file mode 100644
index 0000000..4c82e0b
--- /dev/null
+++ b/libraries/Robot_Control/SdInfo.h
@@ -0,0 +1,117 @@
+/* Arduino FAT16 Library
+ * Copyright (C) 2008 by William Greiman
+ *
+ * This file is part of the Arduino FAT16 Library
+ *
+ * This Library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 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 General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with the Arduino Fat16 Library.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+#ifndef SdInfo_h
+#define SdInfo_h
+#include <stdint.h>
+// Based on the document:
+//
+// SD Specifications
+// Part 1
+// Physical Layer
+// Simplified Specification
+// Version 2.00
+// September 25, 2006
+//
+// www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+//
+// Card IDentification (CID) register
+typedef struct CID {
+  // byte 0
+  uint8_t mid;  // Manufacturer ID
+  // byte 1-2
+  char oid[2];  // OEM/Application ID
+  // byte 3-7
+  char pnm[5];  // Product name
+  // byte 8
+  unsigned prv_m : 4;  // Product revision n.m
+  unsigned prv_n : 4;
+  // byte 9-12
+  uint32_t psn;  // Product serial number
+  // byte 13
+  unsigned mdt_year_high : 4;  // Manufacturing date
+  unsigned reserved : 4;
+  // byte 14
+  unsigned mdt_month : 4;
+  unsigned mdt_year_low :4;
+  // byte 15
+  unsigned always1 : 1;
+  unsigned crc : 7;
+}cid_t;
+// Card-Specific Data register
+typedef struct CSD {
+  // byte 0
+  unsigned reserved1 : 6;
+  unsigned csd_ver : 2;
+  // byte 1
+  uint8_t taac;
+  // byte 2
+  uint8_t nsac;
+  // byte 3
+  uint8_t tran_speed;
+  // byte 4
+  uint8_t ccc_high;
+  // byte 5
+  unsigned read_bl_len : 4;
+  unsigned ccc_low : 4;
+  // byte 6
+  unsigned c_size_high : 2;
+  unsigned reserved2 : 2;
+  unsigned dsr_imp : 1;
+  unsigned read_blk_misalign :1;
+  unsigned write_blk_misalign : 1;
+  unsigned read_bl_partial : 1;
+  // byte 7
+  uint8_t c_size_mid;
+  // byte 8
+  unsigned vdd_r_curr_max : 3;
+  unsigned vdd_r_curr_min : 3;
+  unsigned c_size_low :2;
+  // byte 9
+  unsigned c_size_mult_high : 2;
+  unsigned vdd_w_cur_max : 3;
+  unsigned vdd_w_curr_min : 3;
+  // byte 10
+  unsigned sector_size_high : 6;
+  unsigned erase_blk_en : 1;
+  unsigned c_size_mult_low : 1;
+  // byte 11
+  unsigned wp_grp_size : 7;
+  unsigned sector_size_low : 1;
+  // byte 12
+  unsigned write_bl_len_high : 2;
+  unsigned r2w_factor : 3;
+  unsigned reserved3 : 2;
+  unsigned wp_grp_enable : 1;
+  // byte 13
+  unsigned reserved4 : 5;
+  unsigned write_partial : 1;
+  unsigned write_bl_len_low : 2;
+  // byte 14
+  unsigned reserved5: 2;
+  unsigned file_format : 2;
+  unsigned tmp_write_protect : 1;
+  unsigned perm_write_protect : 1;
+  unsigned copy : 1;
+  unsigned file_format_grp : 1;
+  // byte 15
+  unsigned always1 : 1;
+  unsigned crc : 7;
+}csd_t;
+#endif  // SdInfo_h
diff --git a/libraries/Robot_Control/Sensors.cpp b/libraries/Robot_Control/Sensors.cpp
new file mode 100644
index 0000000..b651c28
--- /dev/null
+++ b/libraries/Robot_Control/Sensors.cpp
@@ -0,0 +1,274 @@
+#include "ArduinoRobot.h"
+#include "Multiplexer.h"
+#include "Wire.h"
+bool RobotControl::digitalRead(uint8_t port){
+	uint8_t type=_getTypeCode(port);
+	switch(type){
+		case TYPE_TOP_TK:
+			return _digitalReadTopMux(port);
+			break;
+		case TYPE_TOP_TKD:
+			return _digitalReadTopPin(port);
+			break;
+		case TYPE_BOTTOM_TK:
+			return _requestDigitalRead(port);
+			break;
+	}
+}
+int RobotControl::analogRead(uint8_t port){
+	uint8_t type=_getTypeCode(port);
+	switch(type){
+		case TYPE_TOP_TK:
+			return _analogReadTopMux(port);
+			break;
+		case TYPE_TOP_TKD:
+			return _analogReadTopPin(port);
+			break;
+		case TYPE_BOTTOM_TK:
+			return _requestAnalogRead(port);
+			break;
+	}
+}
+void RobotControl::digitalWrite(uint8_t port, bool value){
+	uint8_t type=_getTypeCode(port);
+	switch(type){
+		case TYPE_TOP_TK:
+			//Top TKs can't use digitalWrite?
+			break;
+		case TYPE_TOP_TKD:
+			_digitalWriteTopPin(port, value);
+			break;
+		case TYPE_BOTTOM_TK:
+			_requestDigitalWrite(port, value);
+			break;
+	}
+}
+void RobotControl::analogWrite(uint8_t port, uint8_t value){
+	if(port==TKD4)
+		::analogWrite(port,value);
+}
+
+uint8_t RobotControl::_getTypeCode(uint8_t port){
+	switch(port){
+		case TK0:
+		case TK1:
+		case TK2:
+		case TK3:
+		case TK4:
+		case TK5:
+		case TK6:
+		case TK7:
+			return TYPE_TOP_TK;
+			break;
+			
+		case TKD0:
+		case TKD1:
+		case TKD2:
+		case TKD3:
+		case TKD4:
+		case TKD5:
+		case LED1:
+			return TYPE_TOP_TKD;
+			break;
+			
+		case B_TK1:
+		case B_TK2:
+		case B_TK3:
+		case B_TK4:
+			return TYPE_BOTTOM_TK;
+			break;
+	}
+}
+uint8_t RobotControl::_portToTopMux(uint8_t port){
+	switch(port){
+		case TK0:
+			return 0;
+		case TK1:
+			return 1;
+		case TK2:
+			return 2;
+		case TK3:
+			return 3;
+		case TK4:
+			return 4;
+		case TK5:
+			return 5;
+		case TK6:
+			return 6;
+		case TK7:
+			return 7;
+	}
+}
+uint8_t RobotControl::_topDPortToAPort(uint8_t port){
+	switch(port){
+		case TKD0:
+		  return A1;
+		case TKD1:
+		  return A2;
+		case TKD2:
+		  return A3;
+		case TKD3:
+		  return A4;
+		case TKD4:
+		  return A7;
+		case TKD5:
+		  return A11;
+	}
+}
+int* RobotControl::parseMBDPort(uint8_t port){
+	//Serial.println(port);
+	switch(port){
+		case B_TK1:
+			return &motorBoardData._B_TK1;
+		case B_TK2:
+			return &motorBoardData._B_TK2;
+		case B_TK3:
+			return &motorBoardData._B_TK3;
+		case B_TK4:
+			return &motorBoardData._B_TK4;
+		
+		/*		
+		case B_IR0:
+			return &motorBoardData._B_IR0;
+		case B_IR1:
+			return &motorBoardData._B_IR1;
+		case B_IR2:
+			return &motorBoardData._B_IR2;
+		case B_IR3:
+			return &motorBoardData._B_IR3;
+		case B_IR4:
+			return &motorBoardData._B_IR4;*/
+	}
+}
+int RobotControl::get_motorBoardData(uint8_t port){
+	return *parseMBDPort(port);
+}
+void RobotControl::set_motorBoardData(uint8_t port, int data){
+	*parseMBDPort(port)=data;
+}
+
+bool RobotControl::_digitalReadTopMux(uint8_t port){
+	uint8_t num=_portToTopMux(port);
+	return Multiplexer::getDigitalValueAt(num);
+}
+
+int RobotControl::_analogReadTopMux(uint8_t port){
+	uint8_t num=_portToTopMux(port);
+	return Multiplexer::getAnalogValueAt(num);
+}
+
+bool RobotControl::_digitalReadTopPin(uint8_t port){
+	return ::digitalRead(port);
+}
+int RobotControl::_analogReadTopPin(uint8_t port){
+	uint8_t aPin=_topDPortToAPort(port);
+	return ::analogRead(aPin);
+}
+void RobotControl::_digitalWriteTopPin(uint8_t port, bool value){
+	::digitalWrite(port, value);
+}
+
+bool RobotControl::_requestDigitalRead(uint8_t port){
+	messageOut.writeByte(COMMAND_DIGITAL_READ);
+	messageOut.writeByte(port);//B_TK1 - B_TK4
+	messageOut.sendData();
+	delay(10);
+	if(messageIn.receiveData()){
+		//Serial.println("*************");
+		uint8_t cmd=messageIn.readByte();
+		//Serial.print("cmd: ");
+		//Serial.println(cmd);
+		if(!(cmd==COMMAND_DIGITAL_READ_RE))
+			return false;
+			
+		uint8_t pt=messageIn.readByte(); //Bottom TK port codename
+		//Serial.print("pt: ");
+		//Serial.println(pt);
+		set_motorBoardData(pt,messageIn.readByte());
+		return get_motorBoardData(port);
+	}
+}
+int RobotControl::_requestAnalogRead(uint8_t port){
+	messageOut.writeByte(COMMAND_ANALOG_READ);
+	messageOut.writeByte(port);//B_TK1 - B_TK4
+	messageOut.sendData();
+	delay(10);
+	if(messageIn.receiveData()){
+		uint8_t cmd=messageIn.readByte();
+		//Serial.println("*************");
+		//Serial.print("cmd: ");
+		//Serial.println(cmd);
+		if(!(cmd==COMMAND_ANALOG_READ_RE))
+			return false;
+			
+		uint8_t pt=messageIn.readByte();
+		//Serial.print("pt: ");
+		//Serial.println(pt);
+		set_motorBoardData(pt,messageIn.readInt());
+		return get_motorBoardData(port);
+	}
+}
+void RobotControl::_requestDigitalWrite(uint8_t selector, uint8_t value){
+	messageOut.writeByte(COMMAND_DIGITAL_WRITE);
+	messageOut.writeByte(selector);//B_TK1 - B_TK4
+	messageOut.writeByte(value);
+	messageOut.sendData();
+}
+
+
+
+
+
+void RobotControl::updateIR(){
+	messageOut.writeByte(COMMAND_READ_IR);
+	messageOut.sendData();
+	delay(10);
+	if(messageIn.receiveData()){
+		if(messageIn.readByte()==COMMAND_READ_IR_RE){
+			for(int i=0;i<5;i++){
+				IRarray[i]=messageIn.readInt();
+			}
+		}
+	}
+}
+
+int RobotControl::knobRead(){
+	return ::analogRead(POT);
+}
+
+int RobotControl::trimRead(){
+	messageOut.writeByte(COMMAND_READ_TRIM);
+	messageOut.sendData();
+	delay(10);
+	if(messageIn.receiveData()){
+		uint8_t cmd=messageIn.readByte();
+		if(!(cmd==COMMAND_READ_TRIM_RE))
+			return false;
+			
+		uint16_t pt=messageIn.readInt();
+		return pt;
+	}
+}
+
+uint16_t RobotControl::compassRead(){
+	return Compass::getReading();
+}
+
+/*
+void RobotControl::beginUR(uint8_t pinTrigger, uint8_t pinEcho){
+	pinTrigger_UR=pinTrigger;
+	pinEcho_UR=pinEcho;
+	
+	pinMode(pinEcho_UR, INPUT);
+	pinMode(pinTrigger_UR, OUTPUT);
+}
+uint16_t RobotControl::getDistance(){
+	digitalWrite(pinTrigger_UR, LOW);                   // Set the trigger pin to low for 2uS
+	delayMicroseconds(2);
+	digitalWrite(pinTrigger_UR, HIGH);                  // Send a 10uS high to trigger ranging
+	delayMicroseconds(10);
+	digitalWrite(pinTrigger_UR, LOW);                   // Send pin low again
+	uint16_t distance = pulseIn(pinEcho_UR, HIGH);        // Read in times pulse
+	distance= distance/58;                        // Calculate distance from time of pulse
+	return distance;
+}*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/Squawk.cpp b/libraries/Robot_Control/Squawk.cpp
new file mode 100644
index 0000000..5b39ebe
--- /dev/null
+++ b/libraries/Robot_Control/Squawk.cpp
@@ -0,0 +1,601 @@
+// Squawk Soft-Synthesizer Library for Arduino
+//
+// Davey Taylor 2013
+// d.taylor@arduino.cc
+
+#include "Squawk.h"
+
+// Period range, used for clamping
+#define PERIOD_MIN 28
+#define PERIOD_MAX 3424
+
+// Convenience macros
+#define LO4(V)    ((V) & 0x0F)
+#define HI4(V)    (((V) & 0xF0) >> 4)
+#define MIN(A, B) ((A) < (B) ? (A) : (B))
+#define MAX(A, B) ((A) > (B) ? (A) : (B))
+#define FREQ(PERIOD) (tuning_long / (PERIOD))
+
+// SquawkStream class for PROGMEM data
+class StreamROM : public SquawkStream {
+  private:
+    uint8_t *p_start;
+    uint8_t *p_cursor;
+  public:
+    StreamROM(const uint8_t *p_rom = NULL) { p_start = p_cursor = (uint8_t*)p_rom; }
+    uint8_t read() { return pgm_read_byte(p_cursor++); }
+    void seek(size_t offset) { p_cursor = p_start + offset; }
+};
+
+// Oscillator memory
+typedef struct {
+  uint8_t fxp;
+  uint8_t offset;
+  uint8_t mode;
+} pto_t;
+
+// Deconstructed cell
+typedef struct {
+  uint8_t fxc, fxp, ixp;
+} cel_t;
+
+// Effect memory
+typedef struct {
+  int8_t    volume;
+  uint8_t   port_speed;
+  uint16_t  port_target;
+  bool      glissando;
+  pto_t     vibr;
+  pto_t     trem;
+  uint16_t  period;
+  uint8_t   param;
+} fxm_t;
+
+// Locals
+static uint8_t  order_count;
+static uint8_t  order[64];
+static uint8_t  speed;
+static uint8_t  tick;
+static uint8_t  ix_row;
+static uint8_t  ix_order;
+static uint8_t  ix_nextrow;
+static uint8_t  ix_nextorder;
+static uint8_t  row_delay;
+static fxm_t    fxm[4];
+static cel_t    cel[4];
+static uint32_t tuning_long;
+static uint16_t sample_rate;
+static float    tuning = 1.0;
+static uint16_t tick_rate = 50;
+
+static SquawkStream *stream;
+static uint16_t stream_base;
+static StreamROM rom;
+
+// Imports
+extern intptr_t squawk_register;
+extern uint16_t cia;
+
+// Exports
+osc_t osc[4];
+uint8_t pcm = 128;
+
+// ProTracker period tables
+uint16_t period_tbl[84] PROGMEM = {
+  3424, 3232, 3048, 2880, 2712, 2560, 2416, 2280, 2152, 2032, 1920, 1814,
+  1712, 1616, 1524, 1440, 1356, 1280, 1208, 1140, 1076, 1016,  960,  907,
+   856,  808,  762,  720,  678,  640,  604,  570,  538,  508,  480,  453,
+   428,  404,  381,  360,  339,  320,  302,  285,  269,  254,  240,  226,
+   214,  202,  190,  180,  170,  160,  151,  143,  135,  127,  120,  113,
+   107,  101,   95,   90,   85,   80,   75,   71,   67,   63,   60,   56,
+    53,   50,   47,   45,   42,   40,   37,   35,   33,   31,   30,   28,
+};
+
+// ProTracker sine table
+int8_t sine_tbl[32] PROGMEM = {
+  0x00, 0x0C, 0x18, 0x25, 0x30, 0x3C, 0x47, 0x51, 0x5A, 0x62, 0x6A, 0x70, 0x76, 0x7A, 0x7D, 0x7F,
+  0x7F, 0x7F, 0x7D, 0x7A, 0x76, 0x70, 0x6A, 0x62, 0x5A, 0x51, 0x47, 0x3C, 0x30, 0x25, 0x18, 0x0C,
+};
+
+// Squawk object
+SquawkSynth Squawk;
+
+// Look up or generate waveform for ProTracker vibrato/tremolo oscillator
+static int8_t do_osc(pto_t *p_osc) {
+  int8_t sample = 0;
+  int16_t mul;
+  switch(p_osc->mode & 0x03) {
+    case 0: // Sine
+      sample = pgm_read_byte(&sine_tbl[(p_osc->offset) & 0x1F]);
+      if(p_osc->offset & 0x20) sample = -sample;
+      break;
+    case 1: // Square
+      sample = (p_osc->offset & 0x20) ? 127 : -128;
+      break;
+    case 2: // Saw
+      sample = -(p_osc->offset << 2);
+      break;
+    case 3: // Noise (random)
+      sample = rand();
+      break;
+  }
+  mul = sample * LO4(p_osc->fxp);
+  p_osc->offset = (p_osc->offset + HI4(p_osc->fxp));
+  return mul >> 6;
+}
+
+// Calculates and returns arpeggio period
+// Essentially finds period of current note + halftones
+static inline uint16_t arpeggio(uint8_t ch, uint8_t halftones) {
+  uint8_t n;
+  for(n = 0; n != 47; n++) {
+    if(fxm[ch].period >= pgm_read_word(&period_tbl[n])) break;
+  }
+  return pgm_read_word(&period_tbl[MIN(n + halftones, 47)]);
+}
+
+// Calculates and returns glissando period
+// Essentially snaps a sliding frequency to the closest note
+static inline uint16_t glissando(uint8_t ch) {
+  uint8_t n;
+  uint16_t period_h, period_l;
+  for(n = 0; n != 47; n++) {
+    period_l = pgm_read_word(&period_tbl[n]);
+    period_h = pgm_read_word(&period_tbl[n + 1]);
+    if(fxm[ch].period < period_l && fxm[ch].period >= period_h) {
+      if(period_l - fxm[ch].period <= fxm[ch].period - period_h) {
+        period_h = period_l;
+      }
+      break;
+    }
+  }
+  return period_h;
+}
+
+// Tunes Squawk to a different frequency
+void SquawkSynth::tune(float new_tuning) {
+  tuning = new_tuning;
+  tuning_long = (long)(((double)3669213184.0 / (double)sample_rate) * (double)tuning);
+
+}
+
+// Sets tempo
+void SquawkSynth::tempo(uint16_t new_tempo) {
+  tick_rate = new_tempo;
+  cia = sample_rate / tick_rate; // not atomic?
+}
+
+// Initializes Squawk
+// Sets up the selected port, and the sample grinding ISR
+void SquawkSynth::begin(uint16_t hz) {
+  word isr_rr;
+
+  sample_rate = hz;
+  tuning_long = (long)(((double)3669213184.0 / (double)sample_rate) * (double)tuning);
+  cia = sample_rate / tick_rate;
+
+  if(squawk_register == (intptr_t)&OCR0A) {
+    // Squawk uses PWM on OCR0A/PD5(ATMega328/168)/PB7(ATMega32U4)
+#ifdef  __AVR_ATmega32U4__
+    DDRB  |= 0b10000000; // TODO: FAIL on 32U4
+#else
+    DDRD  |= 0b01000000;
+#endif
+    TCCR0A = 0b10000011; // Fast-PWM 8-bit
+    TCCR0B = 0b00000001; // 62500Hz
+    OCR0A  = 0x7F;
+  } else if(squawk_register == (intptr_t)&OCR0B) {
+    // Squawk uses PWM on OCR0B/PC5(ATMega328/168)/PD0(ATMega32U4)
+#ifdef  __AVR_ATmega32U4__
+    DDRD  |= 0b00000001;
+#else
+    DDRD  |= 0b00100000;
+#endif                   // Set timer mode to
+    TCCR0A = 0b00100011; // Fast-PWM 8-bit
+    TCCR0B = 0b00000001; // 62500Hz
+    OCR0B  = 0x7F;
+#ifdef OCR2A
+  } else if(squawk_register == (intptr_t)&OCR2A) {
+    // Squawk uses PWM on OCR2A/PB3
+    DDRB  |= 0b00001000; // Set timer mode to
+    TCCR2A = 0b10000011; // Fast-PWM 8-bit
+    TCCR2B = 0b00000001; // 62500Hz
+    OCR2A  = 0x7F;
+#endif
+#ifdef OCR2B
+  } else if(squawk_register == (intptr_t)&OCR2B) {
+    // Squawk uses PWM on OCR2B/PD3
+    DDRD  |= 0b00001000; // Set timer mode to
+    TCCR2A = 0b00100011; // Fast-PWM 8-bit
+    TCCR2B = 0b00000001; // 62500Hz
+    OCR2B  = 0x7F;
+#endif
+#ifdef OCR3AL
+  } else if(squawk_register == (intptr_t)&OCR3AL) {
+    // Squawk uses PWM on OCR3AL/PC6
+    DDRC  |= 0b01000000; // Set timer mode to
+    TCCR3A = 0b10000001; // Fast-PWM 8-bit
+    TCCR3B = 0b00001001; // 62500Hz
+    OCR3AH = 0x00;
+    OCR3AL = 0x7F;
+#endif
+  } else if(squawk_register == (intptr_t)&SPDR) {
+    // NOT YET SUPPORTED
+    // Squawk uses external DAC via SPI
+    // TODO: Configure SPI
+    // TODO: Needs SS toggle in sample grinder
+  } else if(squawk_register == (intptr_t)&PORTB) {
+    // NOT YET SUPPORTED
+    // Squawk uses resistor ladder on PORTB
+    // TODO: Needs shift right in sample grinder
+    DDRB   = 0b11111111;
+  } else if(squawk_register == (intptr_t)&PORTC) {
+    // NOT YET SUPPORTED
+    // Squawk uses resistor ladder on PORTC
+    // TODO: Needs shift right in sample grinder
+    DDRC   = 0b11111111;
+  }
+
+  // Seed LFSR (needed for noise)
+  osc[3].freq = 0x2000;
+
+  // Set up ISR to run at sample_rate (may not be exact)
+  isr_rr = F_CPU / sample_rate;
+  TCCR1A = 0b00000000;     // Set timer mode
+  TCCR1B = 0b00001001;
+  OCR1AH = isr_rr >> 8;    // Set freq
+  OCR1AL = isr_rr & 0xFF;
+}
+
+// Decrunches a 9 byte row into a useful data
+static void decrunch_row() {
+  uint8_t data;
+
+  // Initial decrunch
+  stream->seek(stream_base + ((order[ix_order] << 6) + ix_row) * 9);
+  data = stream->read(); cel[0].fxc  =  data << 0x04;
+                         cel[1].fxc  =  data &  0xF0;
+  data = stream->read(); cel[0].fxp  =  data;
+  data = stream->read(); cel[1].fxp  =  data;
+  data = stream->read(); cel[2].fxc  =  data << 0x04;
+                         cel[3].fxc  =  data >> 0x04;
+  data = stream->read(); cel[2].fxp  =  data;
+  data = stream->read(); cel[3].fxp  =  data;
+  data = stream->read(); cel[0].ixp  =  data;
+  data = stream->read(); cel[1].ixp  =  data;
+  data = stream->read(); cel[2].ixp  =  data;
+
+  // Decrunch extended effects
+  if(cel[0].fxc == 0xE0) { cel[0].fxc |= cel[0].fxp >> 4; cel[0].fxp &= 0x0F; }
+  if(cel[1].fxc == 0xE0) { cel[1].fxc |= cel[1].fxp >> 4; cel[1].fxp &= 0x0F; }
+  if(cel[2].fxc == 0xE0) { cel[2].fxc |= cel[2].fxp >> 4; cel[2].fxp &= 0x0F; }
+
+  // Decrunch cell 3 ghetto-style
+  cel[3].ixp = ((cel[3].fxp & 0x80) ? 0x00 : 0x7F) | ((cel[3].fxp & 0x40) ? 0x80 : 0x00);
+  cel[3].fxp &= 0x3F;
+  switch(cel[3].fxc) {
+    case 0x02:
+    case 0x03: if(cel[3].fxc & 0x01) cel[3].fxp |= 0x40; cel[3].fxp = (cel[3].fxp >> 4) | (cel[3].fxp << 4); cel[3].fxc = 0x70; break;
+    case 0x01: if(cel[3].fxp & 0x08) cel[3].fxp = (cel[3].fxp & 0x07) << 4; cel[3].fxc = 0xA0; break;
+    case 0x04: cel[3].fxc = 0xC0; break;
+    case 0x05: cel[3].fxc = 0xB0; break;
+    case 0x06: cel[3].fxc = 0xD0; break;
+    case 0x07: cel[3].fxc = 0xF0; break;
+    case 0x08: cel[3].fxc = 0xE7; break;
+    case 0x09: cel[3].fxc = 0xE9; break;
+    case 0x0A: cel[3].fxc = (cel[3].fxp & 0x08) ? 0xEA : 0xEB; cel[3].fxp &= 0x07; break;
+    case 0x0B: cel[3].fxc = (cel[3].fxp & 0x10) ? 0xED : 0xEC; cel[3].fxp &= 0x0F; break;
+    case 0x0C: cel[3].fxc = 0xEE; break;
+  }
+
+  // Apply generic effect parameter memory
+  uint8_t ch;
+  cel_t *p_cel = cel;
+  fxm_t *p_fxm = fxm;
+  for(ch = 0; ch != 4; ch++) {
+    uint8_t fx = p_cel->fxc;
+    if(fx == 0x10 || fx == 0x20 || fx == 0xE1 || fx == 0xE2 || fx == 0x50 || fx == 0x60 || fx == 0xA0) {
+      if(p_cel->fxp) {
+        p_fxm->param = p_cel->fxp;
+      } else {
+        p_cel->fxp = p_fxm->param;
+      }
+    }
+    p_cel++; p_fxm++;
+  }
+}
+
+// Resets playback
+static void playroutine_reset() {
+  memset(fxm, 0, sizeof(fxm));
+  tick         = 0;
+  ix_row       = 0;
+  ix_order     = 0;
+  ix_nextrow   = 0xFF;
+  ix_nextorder = 0xFF;
+  row_delay    = 0;
+  speed        = 6;
+  decrunch_row();
+}
+
+// Start grinding samples
+void SquawkSynth::play() {
+  TIMSK1 = 1 << OCIE1A; // Enable interrupt
+}
+
+// Load a melody stream and start grinding samples
+void SquawkSynth::play(SquawkStream *melody) {
+  uint8_t n;
+  pause();
+  stream = melody;
+  stream->seek(0);
+  n = stream->read();
+  if(n == 'S') {
+    // Squawk SD file
+    stream->seek(4);
+    stream_base = stream->read() << 8;
+    stream_base |= stream->read();
+    stream_base += 6;
+  } else {
+    // Squawk ROM array
+    stream_base = 1;
+  }
+  stream->seek(stream_base);
+  order_count = stream->read();
+  if(order_count <= 64) {
+    stream_base += order_count + 1;
+    for(n = 0; n < order_count; n++) order[n] = stream->read();
+    playroutine_reset();
+    play();
+  } else {
+    order_count = 0;
+  }
+}
+
+// Load a melody in PROGMEM and start grinding samples
+void SquawkSynth::play(const uint8_t *melody) {
+  pause();
+  rom = StreamROM(melody);
+  play(&rom);
+}
+
+// Pause playback
+void SquawkSynth::pause() {
+  TIMSK1 = 0; // Disable interrupt
+}
+
+// Stop playing, unload melody
+void SquawkSynth::stop() {
+  pause();
+  order_count = 0; // Unload melody
+}
+
+// Progress module by one tick
+void squawk_playroutine() {
+  static bool lockout = false;
+
+  if(!order_count) return;
+
+  // Protect from re-entry via ISR
+  cli();
+  if(lockout) {
+    sei();
+    return;
+  }
+  lockout = true;
+  sei();
+
+  // Handle row delay
+  if(row_delay) {
+    if(tick == 0) row_delay--;
+    // Advance tick
+    if(++tick == speed) tick = 0;
+  } else {
+
+    // Quick pointer access
+    fxm_t *p_fxm = fxm;
+    osc_t *p_osc = osc;
+    cel_t *p_cel = cel;
+
+    // Temps
+    uint8_t ch, fx, fxp;
+    bool    pattern_jump = false;
+    uint8_t ix_period;
+
+    for(ch = 0; ch != 4; ch++) {
+      uint8_t       temp;
+
+      // Local register copy
+      fx        = p_cel->fxc;
+      fxp       = p_cel->fxp;
+      ix_period = p_cel->ixp;
+
+      // If first tick
+      if(tick == (fx == 0xED ? fxp : 0)) {
+
+        // Reset volume
+        if(ix_period & 0x80) p_osc->vol = p_fxm->volume = 0x20;
+
+        if((ix_period & 0x7F) != 0x7F) {
+
+          // Reset oscillators (unless continous flag set)
+          if((p_fxm->vibr.mode & 0x4) == 0x0) p_fxm->vibr.offset = 0;
+          if((p_fxm->trem.mode & 0x4) == 0x0) p_fxm->trem.offset = 0;
+
+          // Cell has note
+          if(fx == 0x30 || fx == 0x50) {
+
+            // Tone-portamento effect setup
+            p_fxm->port_target = pgm_read_word(&period_tbl[ix_period & 0x7F]);
+          } else {
+
+            // Set required effect memory parameters
+            p_fxm->period = pgm_read_word(&period_tbl[ix_period & 0x7F]);
+
+            // Start note
+            if(ch != 3) p_osc->freq = FREQ(p_fxm->period);
+
+          }
+        }
+
+        // Effects processed when tick = 0
+        switch(fx) {
+          case 0x30: // Portamento
+            if(fxp) p_fxm->port_speed = fxp;
+            break;
+          case 0xB0: // Jump to pattern
+            ix_nextorder = (fxp >= order_count ? 0x00 : fxp);
+            ix_nextrow = 0;
+            pattern_jump = true;
+            break;
+          case 0xC0: // Set volume
+            p_osc->vol = p_fxm->volume = MIN(fxp, 0x20);
+            break;
+          case 0xD0: // Jump to row
+            if(!pattern_jump) ix_nextorder = ((ix_order + 1) >= order_count ? 0x00 : ix_order + 1);
+            pattern_jump = true;
+            ix_nextrow = (fxp > 63 ? 0 : fxp);
+            break;
+          case 0xF0: // Set speed, BPM(CIA) not supported
+            if(fxp <= 0x20) speed = fxp;
+            break;
+          case 0x40: // Vibrato
+            if(fxp) p_fxm->vibr.fxp = fxp;
+            break;
+          case 0x70: // Tremolo
+            if(fxp) p_fxm->trem.fxp = fxp;
+            break;
+          case 0xE1: // Fine slide up
+            if(ch != 3) {
+              p_fxm->period = MAX(p_fxm->period - fxp, PERIOD_MIN);
+              p_osc->freq = FREQ(p_fxm->period);
+            }
+            break;
+          case 0xE2: // Fine slide down
+            if(ch != 3) {
+              p_fxm->period = MIN(p_fxm->period + fxp, PERIOD_MAX);
+              p_osc->freq = FREQ(p_fxm->period);
+            }
+            break;
+          case 0xE3: // Glissando control
+            p_fxm->glissando = (fxp != 0);
+            break;
+          case 0xE4: // Set vibrato waveform
+            p_fxm->vibr.mode = fxp;
+            break;
+          case 0xE7: // Set tremolo waveform
+            p_fxm->trem.mode = fxp;
+            break;
+          case 0xEA: // Fine volume slide up
+            p_osc->vol = p_fxm->volume = MIN(p_fxm->volume + fxp, 0x20);
+            break;
+          case 0xEB: // Fine volume slide down
+            p_osc->vol = p_fxm->volume = MAX(p_fxm->volume - fxp, 0);
+            break;
+          case 0xEE: // Delay
+            row_delay = fxp;
+            break;
+        }
+      } else {
+
+        // Effects processed when tick > 0
+        switch(fx) {
+          case 0x10: // Slide up
+            if(ch != 3) {
+              p_fxm->period = MAX(p_fxm->period - fxp, PERIOD_MIN);
+              p_osc->freq = FREQ(p_fxm->period);
+            }
+            break;
+          case 0x20: // Slide down
+            if(ch != 3) {
+              p_fxm->period = MIN(p_fxm->period + fxp, PERIOD_MAX);
+              p_osc->freq = FREQ(p_fxm->period);
+            }
+            break;
+/*
+          // Just feels... ugly
+          case 0xE9: // Retrigger note
+            temp = tick; while(temp >= fxp) temp -= fxp;
+            if(!temp) {
+              if(ch == 3) {
+                p_osc->freq = p_osc->phase = 0x2000;
+              } else {
+                p_osc->phase = 0;
+              }
+            }
+            break;
+*/            
+          case 0xEC: // Note cut
+            if(fxp == tick) p_osc->vol = 0x00;
+            break;
+          default:   // Multi-effect processing
+
+            // Portamento
+            if(ch != 3 && (fx == 0x30 || fx == 0x50)) {
+              if(p_fxm->period < p_fxm->port_target) p_fxm->period = MIN(p_fxm->period + p_fxm->port_speed,  p_fxm->port_target);
+              else                                   p_fxm->period = MAX(p_fxm->period - p_fxm->port_speed,  p_fxm->port_target);
+              if(p_fxm->glissando) p_osc->freq = FREQ(glissando(ch));
+              else                 p_osc->freq = FREQ(p_fxm->period);
+            }
+
+            // Volume slide
+            if(fx == 0x50 || fx == 0x60 || fx == 0xA0) {
+              if((fxp & 0xF0) == 0) p_fxm->volume -= (LO4(fxp));
+              if((fxp & 0x0F) == 0) p_fxm->volume += (HI4(fxp));
+              p_osc->vol = p_fxm->volume = MAX(MIN(p_fxm->volume, 0x20), 0);
+            }
+        }
+      }
+
+      // Normal play and arpeggio
+      if(fx == 0x00) {
+        if(ch != 3) {
+          temp = tick; while(temp > 2) temp -= 2;
+          if(temp == 0) {
+
+            // Reset
+            p_osc->freq = FREQ(p_fxm->period);
+          } else if(fxp) {
+
+            // Arpeggio
+            p_osc->freq = FREQ(arpeggio(ch, (temp == 1 ? HI4(fxp) : LO4(fxp))));
+          }
+        }
+      } else if(fx == 0x40 || fx == 0x60) {
+
+        // Vibrato
+        if(ch != 3) p_osc->freq = FREQ((p_fxm->period + do_osc(&p_fxm->vibr)));
+      } else if(fx == 0x70) {
+        int8_t trem = p_fxm->volume + do_osc(&p_fxm->trem);
+        p_osc->vol = MAX(MIN(trem, 0x20), 0);
+      }
+
+      // Next channel
+      p_fxm++; p_cel++; p_osc++;
+    }
+
+    // Advance tick
+    if(++tick == speed) tick = 0;
+
+    // Advance playback
+    if(tick == 0) {
+      if(++ix_row == 64) {
+        ix_row = 0;
+        if(++ix_order >= order_count) ix_order = 0;
+      }
+	    // Forced order/row
+	    if( ix_nextorder != 0xFF ) {
+	      ix_order = ix_nextorder;
+	      ix_nextorder = 0xFF;
+	    }
+	    if( ix_nextrow != 0xFF ) {
+	      ix_row = ix_nextrow;
+	      ix_nextrow = 0xFF;
+	    }
+			decrunch_row();
+    }
+
+  }
+
+  lockout = false;
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/Squawk.h b/libraries/Robot_Control/Squawk.h
new file mode 100644
index 0000000..3481acf
--- /dev/null
+++ b/libraries/Robot_Control/Squawk.h
@@ -0,0 +1,265 @@
+// Squawk Soft-Synthesizer Library for Arduino
+//
+// Davey Taylor 2013
+// d.taylor@arduino.cc
+
+#ifndef _SQUAWK_H_
+#define _SQUAWK_H_
+#include <stddef.h>
+#include <inttypes.h>
+#include "Arduino.h"
+
+#define Melody const uint8_t PROGMEM
+
+class SquawkStream {
+	public:
+	  virtual ~SquawkStream() = 0;
+    virtual uint8_t read() = 0;
+    virtual void seek(size_t offset) = 0;
+};
+inline SquawkStream::~SquawkStream() { }
+
+class SquawkSynth {
+
+protected:
+  // Load and play specified melody
+  void play(SquawkStream *melody);
+
+public:
+  SquawkSynth() {};
+
+  // Initialize Squawk to generate samples at sample_rate Hz
+  void begin(uint16_t sample_rate);
+
+  // Load and play specified melody
+  // melody needs to point to PROGMEM data
+  void play(const uint8_t *melody);
+  
+  // Resume currently loaded melody (or enable direct osc manipulation by sketch)
+  void play();
+    
+  // Pause playback
+  void pause();
+  
+  // Stop playback (unloads song)
+  void stop();
+  
+  // Tune Squawk to a different frequency - default is 1.0
+  void tune(float tuning);
+
+  // Change the tempo - default is 50
+	void tempo(uint16_t tempo);
+};
+
+extern SquawkSynth Squawk;
+
+// oscillator structure
+typedef struct {
+  uint8_t  vol;
+  uint16_t freq;
+  uint16_t phase;
+} osc_t;
+
+typedef osc_t Oscillator;
+
+// oscillator memory
+extern osc_t osc[4];
+extern uint8_t pcm;
+// channel 0 is pulse wave @ 25% duty
+// channel 1 is square wave
+// channel 2 is triangle wave
+// channel 3 is noise
+
+// For channel 3, freq is used as part of its LFSR and should not be changed.
+// LFSR: Linear feedback shift register, a method of producing a
+// pseudo-random bit sequence, used to generate nasty noise.
+
+#ifdef __AVR_ATmega32U4__
+// Supported configurations for ATmega32U4
+#define SQUAWK_PWM_PIN5  OCR3AL
+#define SQUAWK_PWM_PIN11 OCR0A
+#define SQUAWK_PWM_PIN3  OCR0B
+/*
+// NOT SUPPORTED YET
+#define SQUAWK_PWM_PIN6  OCR4D
+#define SQUAWK_PWM_PIN9  OCR4B
+#define SQUAWK_PWM_PIN10 OCR4B
+*/
+#endif
+
+#ifdef __AVR_ATmega168__
+// Supported configurations for ATmega168
+#define SQUAWK_PWM_PIN6  OCR0A
+#define SQUAWK_PWM_PIN5  OCR0B
+#define SQUAWK_PWM_PIN11 OCR2A
+#define SQUAWK_PWM_PIN3  OCR2B
+#endif
+
+#ifdef __AVR_ATmega328P__
+// Supported configurations for ATmega328P
+#define SQUAWK_PWM_PIN6  OCR0A
+#define SQUAWK_PWM_PIN5  OCR0B
+#define SQUAWK_PWM_PIN11 OCR2A
+#define SQUAWK_PWM_PIN3  OCR2B
+#endif
+
+/*
+// NOT SUPPORTED YET
+#define SQUAWK_SPI SPDR
+#define SQUAWK_RLD_PORTB PORTB
+#define SQUAWK_RLD_PORTC PORTC
+*/
+
+extern void squawk_playroutine() asm("squawk_playroutine");
+
+// SAMPLE GRINDER
+// generates samples and updates oscillators
+// uses 132 cycles (not counting playroutine)
+//     ~1/3 CPU @ 44kHz on 16MHz
+#define SQUAWK_CONSTRUCT_ISR(TARGET_REGISTER) \
+uint16_t cia, cia_count; \
+intptr_t squawk_register = (intptr_t)&TARGET_REGISTER; \
+ISR(TIMER1_COMPA_vect, ISR_NAKED) { \
+  asm volatile( \
+    "push r2                                          " "\n\t" \
+    "in   r2,                    __SREG__             " "\n\t" \
+    "push r18                                         " "\n\t" \
+    "push r27                                         " "\n\t" \
+    "push r26                                         " "\n\t" \
+    "push r0                                          " "\n\t" \
+    "push r1                                          " "\n\t" \
+\
+    "lds  r18,                   osc+2*%[mul]+%[fre]  " "\n\t" \
+    "lds  r0,                    osc+2*%[mul]+%[pha]  " "\n\t" \
+    "add  r0,                    r18                  " "\n\t" \
+    "sts  osc+2*%[mul]+%[pha],   r0                   " "\n\t" \
+    "lds  r18,                   osc+2*%[mul]+%[fre]+1" "\n\t" \
+    "lds  r1,                    osc+2*%[mul]+%[pha]+1" "\n\t" \
+    "adc  r1,                    r18                  " "\n\t" \
+    "sts  osc+2*%[mul]+%[pha]+1, r1                   " "\n\t" \
+\
+    "mov  r27,                   r1                   " "\n\t" \
+    "sbrc r27,                   7                    " "\n\t" \
+    "com  r27                                         " "\n\t" \
+    "lsl  r27                                         " "\n\t" \
+    "lds  r26,                   osc+2*%[mul]+%[vol]  " "\n\t" \
+    "subi r27,                   128                  " "\n\t" \
+    "muls r27,                   r26                  " "\n\t" \
+    "lsl  r1                                          " "\n\t" \
+    "mov  r26,                   r1                   " "\n\t" \
+\
+    "lds  r18,                   osc+0*%[mul]+%[fre]  " "\n\t" \
+    "lds  r0,                    osc+0*%[mul]+%[pha]  " "\n\t" \
+    "add  r0,                    r18                  " "\n\t" \
+    "sts  osc+0*%[mul]+%[pha],   r0                   " "\n\t" \
+    "lds  r18,                   osc+0*%[mul]+%[fre]+1" "\n\t" \
+    "lds  r1,                    osc+0*%[mul]+%[pha]+1" "\n\t" \
+    "adc  r1,                    r18                  " "\n\t" \
+    "sts  osc+0*%[mul]+%[pha]+1, r1                   " "\n\t" \
+\
+    "mov  r18,                   r1                   " "\n\t" \
+    "lsl  r18                                         " "\n\t" \
+    "and  r18,                   r1                   " "\n\t" \
+    "lds  r27,                   osc+0*%[mul]+%[vol]  " "\n\t" \
+    "sbrc r18,                   7                    " "\n\t" \
+    "neg  r27                                         " "\n\t" \
+    "add  r26,                   r27                  " "\n\t" \
+\
+    "lds  r18,                   osc+1*%[mul]+%[fre]  " "\n\t" \
+    "lds  r0,                    osc+1*%[mul]+%[pha]  " "\n\t" \
+    "add  r0,                    r18                  " "\n\t" \
+    "sts  osc+1*%[mul]+%[pha],   r0                   " "\n\t" \
+    "lds  r18,                   osc+1*%[mul]+%[fre]+1" "\n\t" \
+    "lds  r1,                    osc+1*%[mul]+%[pha]+1" "\n\t" \
+    "adc  r1,                    r18                  " "\n\t" \
+    "sts  osc+1*%[mul]+%[pha]+1, r1                   " "\n\t" \
+\
+    "lds  r27,                   osc+1*%[mul]+%[vol]  " "\n\t" \
+    "sbrc r1,                    7                    " "\n\t" \
+    "neg  r27                                         " "\n\t" \
+    "add  r26,                   r27                  " "\n\t" \
+\
+    "ldi  r27,                   1                    " "\n\t" \
+    "lds  r0,                    osc+3*%[mul]+%[fre]  " "\n\t" \
+    "lds  r1,                    osc+3*%[mul]+%[fre]+1" "\n\t" \
+    "add  r0,                    r0                   " "\n\t" \
+    "adc  r1,                    r1                   " "\n\t" \
+    "sbrc r1,                    7                    " "\n\t" \
+    "eor  r0,                    r27                  " "\n\t" \
+    "sbrc r1,                    6                    " "\n\t" \
+    "eor  r0,                    r27                  " "\n\t" \
+    "sts  osc+3*%[mul]+%[fre],   r0                   " "\n\t" \
+    "sts  osc+3*%[mul]+%[fre]+1, r1                   " "\n\t" \
+\
+    "lds  r27,                   osc+3*%[mul]+%[vol]  " "\n\t" \
+    "sbrc r1,                    7                    " "\n\t" \
+    "neg  r27                                         " "\n\t" \
+    "add  r26,                   r27                  " "\n\t" \
+\
+    "lds  r27,                   pcm                  " "\n\t" \
+    "add  r26,                   r27                  " "\n\t" \
+    "sts  %[reg],                r26                  " "\n\t" \
+\
+	  "lds  r27,                   cia_count+1          " "\n\t" \
+	  "lds  r26,                   cia_count            " "\n\t" \
+	  "sbiw r26,                   1                    " "\n\t" \
+	  "breq call_playroutine                            " "\n\t" \
+	  "sts  cia_count+1,           r27                  " "\n\t" \
+	  "sts  cia_count,             r26                  " "\n\t" \
+    "pop  r1                                          " "\n\t" \
+    "pop  r0                                          " "\n\t" \
+    "pop  r26                                         " "\n\t" \
+    "pop  r27                                         " "\n\t" \
+    "pop  r18                                         " "\n\t" \
+    "out  __SREG__,              r2                   " "\n\t" \
+    "pop  r2                                          " "\n\t" \
+	  "reti                                             " "\n\t" \
+    "call_playroutine:                                " "\n\t" \
+\
+	  "lds  r27, cia+1                                  " "\n\t" \
+	  "lds  r26, cia                                    " "\n\t" \
+	  "sts  cia_count+1,           r27                  " "\n\t" \
+	  "sts  cia_count,             r26                  " "\n\t" \
+\
+    "sei                                              " "\n\t" \
+	  "push r19                                         " "\n\t" \
+	  "push r20                                         " "\n\t" \
+	  "push r21                                         " "\n\t" \
+	  "push r22                                         " "\n\t" \
+	  "push r23                                         " "\n\t" \
+	  "push r24                                         " "\n\t" \
+	  "push r25                                         " "\n\t" \
+	  "push r30                                         " "\n\t" \
+	  "push r31                                         " "\n\t" \
+\
+    "clr  r1                                          " "\n\t" \
+    "call squawk_playroutine                          " "\n\t" \
+\
+	  "pop  r31                                         " "\n\t" \
+	  "pop  r30                                         " "\n\t" \
+	  "pop  r25                                         " "\n\t" \
+	  "pop  r24                                         " "\n\t" \
+	  "pop  r23                                         " "\n\t" \
+	  "pop  r22                                         " "\n\t" \
+	  "pop  r21                                         " "\n\t" \
+	  "pop  r20                                         " "\n\t" \
+	  "pop  r19                                         " "\n\t" \
+\
+    "pop  r1                                          " "\n\t" \
+    "pop  r0                                          " "\n\t" \
+    "pop  r26                                         " "\n\t" \
+    "pop  r27                                         " "\n\t" \
+    "pop  r18                                         " "\n\t" \
+    "out  __SREG__,              r2                   " "\n\t" \
+    "pop  r2                                          " "\n\t" \
+	  "reti                                             " "\n\t" \
+    : \
+    : [reg] "M" _SFR_MEM_ADDR(TARGET_REGISTER), \
+      [mul] "M" (sizeof(Oscillator)), \
+      [pha] "M" (offsetof(Oscillator, phase)), \
+      [fre] "M" (offsetof(Oscillator, freq)), \
+      [vol] "M" (offsetof(Oscillator, vol)) \
+  ); \
+}
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/SquawkSD.cpp b/libraries/Robot_Control/SquawkSD.cpp
new file mode 100644
index 0000000..3c97ef4
--- /dev/null
+++ b/libraries/Robot_Control/SquawkSD.cpp
@@ -0,0 +1,182 @@
+#include <SquawkSD.h>
+
+SquawkSynthSD SquawkSD;
+
+class StreamFile : public SquawkStream {
+  private:
+    Fat16 f;
+	public:
+		StreamFile(Fat16 file = Fat16()) { f = file; }
+    uint8_t read() { return f.read(); }
+    void seek(size_t offset) { f.seekSet(offset); }
+};
+
+static StreamFile file;
+
+extern uint16_t period_tbl[84] PROGMEM;
+
+void SquawkSynthSD::play(Fat16 melody) {
+	SquawkSynth::pause();
+	file = StreamFile(melody);
+	SquawkSynth::play(&file);
+}
+
+/*
+void SquawkSynthSD::convert(Fat16 in, Fat16 out) {
+  unsigned int n;
+  uint8_t patterns = 0, order_count;
+  unsigned int ptn, row, chn;
+  uint8_t temp;
+
+  uint8_t fxc[4], fxp[4], note[4], sample[4];
+  uint16_t period;
+
+  out.write('S'); // ID
+  out.write('Q');
+  out.write('M');
+  out.write('1');
+  out.write((uint8_t)0); // No meta data
+  out.write((uint8_t)0);
+  
+  // Write order list, count patterns
+  in.seek(0x3B6);
+  order_count = in.read();
+  out.write(order_count);
+  in.seek(0x3B8);
+  for(n = 0; n < order_count; n++) {
+    temp = in.read();
+    if(temp >= patterns) patterns = temp + 1;
+    out.write(temp);
+  }
+  
+  // Write patterns
+  in.seek(0x43C);
+  for(ptn = 0; ptn < patterns; ptn++) {
+    for(row = 0; row < 64; row++) {
+      for(chn = 0; chn < 4; chn++) {
+        
+        // Basic extraction
+        temp         = in.read();          // sample.msb and period.msb
+        period       = (temp & 0x0F) << 8;
+        sample[chn]  = temp & 0xF0;
+        period      |= in.read();          // period.lsb
+        temp         = in.read();          // sample.lsb and effect
+        sample[chn] |= temp >> 4;
+        fxc[chn]     = (temp & 0x0F) << 4;
+        fxp[chn]     = in.read();          // parameters
+        if(fxc[chn] == 0xE0) {
+          fxc[chn]    |= fxp[chn] >> 4;    // extended parameters
+          fxp[chn]    &= 0x0F;
+        }
+        
+        #define DIF(A, B) ((A) > (B) ? ((int32_t)(A) - (int32_t)(B)) : ((int32_t)(B) - (int32_t)(A)))
+        // Find closest matching period
+        if(period == 0) {
+          note[chn] = 0x7F;
+        } else {
+          int16_t best = DIF(period, pgm_read_word(&period_tbl[0]));
+          note[chn] = 0;
+          for(n = 0; n < sizeof(period_tbl) / sizeof(uint16_t); n++) {
+            if(DIF(period, pgm_read_word(&period_tbl[n])) < best) {
+              note[chn] = n;
+              best = DIF(period, pgm_read_word(&period_tbl[n]));
+            }
+          }
+        }
+        
+        // Crunch volume/decimal commands
+        if(fxc[chn] == 0x50 || fxc[chn] == 0x60 || fxc[chn] == 0xA0) {
+          fxp[chn] = (fxp[chn] >> 1) & 0x77;
+        } else if(fxc[chn] == 0x70) {
+          fxp[chn] = (fxp[chn] & 0xF0) | ((fxp[chn] & 0x0F) >> 1);
+        } else if(fxc[chn] == 0xC0 || fxc[chn] == 0xEA || fxc[chn] == 0xEB) {
+          fxp[chn] >>= 1;
+        } else if(fxc[chn] == 0xD0) {
+          fxp[chn] = ((fxp[chn] >> 4) * 10) | (fxp[chn] & 0x0F);
+        }
+
+        // Re-nibblify - it's a word!
+        if(chn != 3) {
+          if((fxc[chn] & 0xF0) == 0xE0) fxp[chn] |= fxc[chn] << 4;
+          fxc[chn] >>= 4;       
+        }
+
+      }
+
+      // Ghetto crunch the last channel to save a byte
+      switch(fxc[3]) {
+        case 0x50: case 0x60: case 0xA0:
+          fxc[3] = 0x1;
+          if((fxp[3] >> 4) >= (fxp[3] & 0x0F)) {
+            fxp[3] = 0x80 + ((fxp[3] >> 4) - (fxp[3] & 0x0F));
+          } else {
+            fxp[3] = ((fxp[3] & 0x0F) - (fxp[3] >> 4));
+          }
+          break;
+        case 0x70:
+          fxc[3] = (fxp[3] & 0x4) ? 0x3 : 0x2;
+          fxp[3] = (fxp[3] >> 4) | ((fxp[3] & 0x03) << 4);
+          break;
+        case 0xC0:
+          fxc[3] = 0x4;
+          fxp[3] &= 0x1F;
+          break;
+        case 0xB0:
+          fxc[3] = 0x5;
+          fxp[3] &= 0x1F;
+          break;
+        case 0xD0:
+          fxc[3] = 0x6;
+          if(fxp[3] > 63) fxp[3] = 0;
+          break;
+        case 0xF0:
+          if(fxp[3] > 0x20) {
+            fxc[3] = 0x0;
+            fxp[3] = 0x00;
+          } else {
+            fxc[3] = 0x7;             
+          }
+          break;
+        case 0xE7:
+          fxc[3] = 0x8;
+          break;
+        case 0xE9:
+          fxc[3] = 0x9;
+          break;
+        case 0xEA:
+          fxc[3] = 0xA;
+          fxp[3] |= 0x08;
+          break;
+        case 0xEB:
+          fxc[3] = 0xA;
+          break;
+        case 0xEC:
+          fxc[3] = 0xB;
+          break;
+        case 0xED:
+          fxc[3] = 0xB;
+          fxp[3] |= 0x10;
+          break;
+        case 0xEE:
+          fxc[3] = 0xC;
+          break;
+        default:
+          fxc[3] = 0;
+          fxp[3] = 0;
+      }
+      if(note[3] != 0x7F) fxp[3] |= 0x80;
+      if(sample[3]) fxp[3] |= 0x40;
+
+      // Write out
+      out.write((fxc[0]) | fxc[1] << 4);
+      out.write(fxp[0]);
+      out.write(fxp[1]);
+      out.write((fxc[2]) | fxc[3] << 4);
+      out.write(fxp[2]);
+      out.write(fxp[3]);
+      out.write(note[0] | (sample[0] == 0 ? 0x00 : 0x80));
+      out.write(note[1] | (sample[1] == 0 ? 0x00 : 0x80));
+      out.write(note[2] | (sample[2] == 0 ? 0x00 : 0x80));
+    }
+  }
+}*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/SquawkSD.h b/libraries/Robot_Control/SquawkSD.h
new file mode 100644
index 0000000..89d46a5
--- /dev/null
+++ b/libraries/Robot_Control/SquawkSD.h
@@ -0,0 +1,17 @@
+#ifndef _SQUAWKSD_H_
+#define _SQUAWKSD_H_
+#include <Squawk.h>
+#include "Fat16.h"
+
+class SquawkSynthSD : public SquawkSynth {
+  private:
+  	Fat16 f;
+	public:
+	  inline void play() { Squawk.play(); };
+		void play(Fat16 file);
+		//void convert(Fat16 in, Fat16 out);
+};
+
+extern SquawkSynthSD SquawkSD;
+
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/Wire.cpp b/libraries/Robot_Control/Wire.cpp
new file mode 100644
index 0000000..4e7a17c
--- /dev/null
+++ b/libraries/Robot_Control/Wire.cpp
@@ -0,0 +1,298 @@
+/*
+  TwoWire.cpp - TWI/I2C library for Wiring & Arduino
+  Copyright (c) 2006 Nicholas Zambetti.  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
+ 
+  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+*/
+
+extern "C" {
+  #include <stdlib.h>
+  #include <string.h>
+  #include <inttypes.h>
+  #include "twi.h"
+}
+
+#include "Wire.h"
+
+// Initialize Class Variables //////////////////////////////////////////////////
+
+uint8_t TwoWire::rxBuffer[BUFFER_LENGTH];
+uint8_t TwoWire::rxBufferIndex = 0;
+uint8_t TwoWire::rxBufferLength = 0;
+
+uint8_t TwoWire::txAddress = 0;
+uint8_t TwoWire::txBuffer[BUFFER_LENGTH];
+uint8_t TwoWire::txBufferIndex = 0;
+uint8_t TwoWire::txBufferLength = 0;
+
+uint8_t TwoWire::transmitting = 0;
+void (*TwoWire::user_onRequest)(void);
+void (*TwoWire::user_onReceive)(int);
+
+// Constructors ////////////////////////////////////////////////////////////////
+
+TwoWire::TwoWire()
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+void TwoWire::begin(void)
+{
+  rxBufferIndex = 0;
+  rxBufferLength = 0;
+
+  txBufferIndex = 0;
+  txBufferLength = 0;
+
+  twi_init();
+}
+
+void TwoWire::begin(uint8_t address)
+{
+  twi_setAddress(address);
+  twi_attachSlaveTxEvent(onRequestService);
+  twi_attachSlaveRxEvent(onReceiveService);
+  begin();
+}
+
+void TwoWire::begin(int address)
+{
+  begin((uint8_t)address);
+}
+
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop)
+{
+  // clamp to buffer length
+  if(quantity > BUFFER_LENGTH){
+    quantity = BUFFER_LENGTH;
+  }
+  // perform blocking read into buffer
+  uint8_t read = twi_readFrom(address, rxBuffer, quantity, sendStop);
+  // set rx buffer iterator vars
+  rxBufferIndex = 0;
+  rxBufferLength = read;
+
+  return read;
+}
+
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity)
+{
+  return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true);
+}
+
+uint8_t TwoWire::requestFrom(int address, int quantity)
+{
+  return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true);
+}
+
+uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop)
+{
+  return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)sendStop);
+}
+
+void TwoWire::beginTransmission(uint8_t address)
+{
+  // indicate that we are transmitting
+  transmitting = 1;
+  // set address of targeted slave
+  txAddress = address;
+  // reset tx buffer iterator vars
+  txBufferIndex = 0;
+  txBufferLength = 0;
+}
+
+void TwoWire::beginTransmission(int address)
+{
+  beginTransmission((uint8_t)address);
+}
+
+//
+//	Originally, 'endTransmission' was an f(void) function.
+//	It has been modified to take one parameter indicating
+//	whether or not a STOP should be performed on the bus.
+//	Calling endTransmission(false) allows a sketch to 
+//	perform a repeated start. 
+//
+//	WARNING: Nothing in the library keeps track of whether
+//	the bus tenure has been properly ended with a STOP. It
+//	is very possible to leave the bus in a hung state if
+//	no call to endTransmission(true) is made. Some I2C
+//	devices will behave oddly if they do not see a STOP.
+//
+uint8_t TwoWire::endTransmission(uint8_t sendStop)
+{
+  // transmit buffer (blocking)
+  int8_t ret = twi_writeTo(txAddress, txBuffer, txBufferLength, 1, sendStop);
+  // reset tx buffer iterator vars
+  txBufferIndex = 0;
+  txBufferLength = 0;
+  // indicate that we are done transmitting
+  transmitting = 0;
+  return ret;
+}
+
+//	This provides backwards compatibility with the original
+//	definition, and expected behaviour, of endTransmission
+//
+uint8_t TwoWire::endTransmission(void)
+{
+  return endTransmission(true);
+}
+
+// must be called in:
+// slave tx event callback
+// or after beginTransmission(address)
+size_t TwoWire::write(uint8_t data)
+{
+  if(transmitting){
+  // in master transmitter mode
+    // don't bother if buffer is full
+    if(txBufferLength >= BUFFER_LENGTH){
+      setWriteError();
+      return 0;
+    }
+    // put byte in tx buffer
+    txBuffer[txBufferIndex] = data;
+    ++txBufferIndex;
+    // update amount in buffer   
+    txBufferLength = txBufferIndex;
+  }else{
+  // in slave send mode
+    // reply to master
+    twi_transmit(&data, 1);
+  }
+  return 1;
+}
+
+// must be called in:
+// slave tx event callback
+// or after beginTransmission(address)
+size_t TwoWire::write(const uint8_t *data, size_t quantity)
+{
+  if(transmitting){
+  // in master transmitter mode
+    for(size_t i = 0; i < quantity; ++i){
+      write(data[i]);
+    }
+  }else{
+  // in slave send mode
+    // reply to master
+    twi_transmit(data, quantity);
+  }
+  return quantity;
+}
+
+// must be called in:
+// slave rx event callback
+// or after requestFrom(address, numBytes)
+int TwoWire::available(void)
+{
+  return rxBufferLength - rxBufferIndex;
+}
+
+// must be called in:
+// slave rx event callback
+// or after requestFrom(address, numBytes)
+int TwoWire::read(void)
+{
+  int value = -1;
+  
+  // get each successive byte on each call
+  if(rxBufferIndex < rxBufferLength){
+    value = rxBuffer[rxBufferIndex];
+    ++rxBufferIndex;
+  }
+
+  return value;
+}
+
+// must be called in:
+// slave rx event callback
+// or after requestFrom(address, numBytes)
+int TwoWire::peek(void)
+{
+  int value = -1;
+  
+  if(rxBufferIndex < rxBufferLength){
+    value = rxBuffer[rxBufferIndex];
+  }
+
+  return value;
+}
+
+void TwoWire::flush(void)
+{
+  // XXX: to be implemented.
+}
+
+// behind the scenes function that is called when data is received
+void TwoWire::onReceiveService(uint8_t* inBytes, int numBytes)
+{
+  // don't bother if user hasn't registered a callback
+  if(!user_onReceive){
+    return;
+  }
+  // don't bother if rx buffer is in use by a master requestFrom() op
+  // i know this drops data, but it allows for slight stupidity
+  // meaning, they may not have read all the master requestFrom() data yet
+  if(rxBufferIndex < rxBufferLength){
+    return;
+  }
+  // copy twi rx buffer into local read buffer
+  // this enables new reads to happen in parallel
+  for(uint8_t i = 0; i < numBytes; ++i){
+    rxBuffer[i] = inBytes[i];    
+  }
+  // set rx iterator vars
+  rxBufferIndex = 0;
+  rxBufferLength = numBytes;
+  // alert user program
+  user_onReceive(numBytes);
+}
+
+// behind the scenes function that is called when data is requested
+void TwoWire::onRequestService(void)
+{
+  // don't bother if user hasn't registered a callback
+  if(!user_onRequest){
+    return;
+  }
+  // reset tx buffer iterator vars
+  // !!! this will kill any pending pre-master sendTo() activity
+  txBufferIndex = 0;
+  txBufferLength = 0;
+  // alert user program
+  user_onRequest();
+}
+
+// sets function called on slave write
+void TwoWire::onReceive( void (*function)(int) )
+{
+  user_onReceive = function;
+}
+
+// sets function called on slave read
+void TwoWire::onRequest( void (*function)(void) )
+{
+  user_onRequest = function;
+}
+
+// Preinstantiate Objects //////////////////////////////////////////////////////
+
+TwoWire Wire = TwoWire();
+
diff --git a/libraries/Robot_Control/Wire.h b/libraries/Robot_Control/Wire.h
new file mode 100644
index 0000000..a93d0f5
--- /dev/null
+++ b/libraries/Robot_Control/Wire.h
@@ -0,0 +1,79 @@
+/*
+  TwoWire.h - TWI/I2C library for Arduino & Wiring
+  Copyright (c) 2006 Nicholas Zambetti.  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
+
+  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+*/
+
+#ifndef TwoWire_h
+#define TwoWire_h
+
+#include <inttypes.h>
+#include "Stream.h"
+
+#define BUFFER_LENGTH 32
+
+class TwoWire : public Stream
+{
+  private:
+    static uint8_t rxBuffer[];
+    static uint8_t rxBufferIndex;
+    static uint8_t rxBufferLength;
+
+    static uint8_t txAddress;
+    static uint8_t txBuffer[];
+    static uint8_t txBufferIndex;
+    static uint8_t txBufferLength;
+
+    static uint8_t transmitting;
+    static void (*user_onRequest)(void);
+    static void (*user_onReceive)(int);
+    static void onRequestService(void);
+    static void onReceiveService(uint8_t*, int);
+  public:
+    TwoWire();
+    void begin();
+    void begin(uint8_t);
+    void begin(int);
+    void beginTransmission(uint8_t);
+    void beginTransmission(int);
+    uint8_t endTransmission(void);
+    uint8_t endTransmission(uint8_t);
+    uint8_t requestFrom(uint8_t, uint8_t);
+    uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
+    uint8_t requestFrom(int, int);
+    uint8_t requestFrom(int, int, int);
+    virtual size_t write(uint8_t);
+    virtual size_t write(const uint8_t *, size_t);
+    virtual int available(void);
+    virtual int read(void);
+    virtual int peek(void);
+	virtual void flush(void);
+    void onReceive( void (*)(int) );
+    void onRequest( void (*)(void) );
+  
+    inline size_t write(unsigned long n) { return write((uint8_t)n); }
+    inline size_t write(long n) { return write((uint8_t)n); }
+    inline size_t write(unsigned int n) { return write((uint8_t)n); }
+    inline size_t write(int n) { return write((uint8_t)n); }
+    using Print::write;
+};
+
+extern TwoWire Wire;
+
+#endif
+
diff --git a/libraries/Robot_Control/communication.cpp b/libraries/Robot_Control/communication.cpp
new file mode 100644
index 0000000..eaf5346
--- /dev/null
+++ b/libraries/Robot_Control/communication.cpp
@@ -0,0 +1 @@
+#include <ArduinoRobot.h>

bool RobotControl::isActionDone(){
	if(messageIn.receiveData()){
		if(messageIn.readByte()==COMMAND_ACTION_DONE){
			return true;
		}
	}
	return false;
}

void RobotControl::pauseMode(uint8_t onOff){
	messageOut.writeByte(COMMAND_PAUSE_MODE);
	if(onOff){
		messageOut.writeByte(true);
	}else{
		messageOut.writeByte(false);
	}
	messageOut.sendData();
}

void RobotControl::lineFollowConfig(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
	messageOut.writeByte(COMMAND_LINE_FOLLOW_CONFIG);
	messageOut.writeByte(KP);
	messageOut.writeByte(KD);
	messageOut.writeByte(robotSpeed);
	messageOut.writeByte(intergrationTime);
	messageOut.sendData();
}
\ No newline at end of file
diff --git a/libraries/Robot_Control/examples/explore/R01_Logo/R01_Logo.ino b/libraries/Robot_Control/examples/explore/R01_Logo/R01_Logo.ino
new file mode 100644
index 0000000..794479e
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R01_Logo/R01_Logo.ino
@@ -0,0 +1,134 @@
+/* Robot Logo
+
+ This sketch demonstrates basic movement of the Robot. 
+ When the sketch starts, press the on-board buttons to tell 
+ the robot how to move. Pressing the middle button will 
+ save the pattern, and the robot will follow accordingly. 
+ You can record up to 20 commands. The robot will move for 
+ one second per command.
+ 
+ This example uses images on an SD card. It looks for
+ files named "lg0.bmp" and "lg1.bmp" and draws them on the
+ screen.
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+
+int commands[20];  //  array for storing commands
+
+void setup() {
+  // initialize the Robot, SD card, and display 
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+
+  // draw "lg0.bmp" and "lg1.bmp" on the screen
+  Robot.displayLogos();
+}
+
+void loop() {
+  
+  Robot.drawBMP("intro.bmp", 0, 0);  //display background image
+  
+  iniCommands(); // remove commands from the array
+  addCommands(); // add commands to the array
+  
+  delay(1000); // wait for a second
+  
+  executeCommands(); // follow orders
+  
+  Robot.stroke(0,0,0);
+  Robot.text("Done!", 5, 103); // write some text to the display
+  delay(1500); // wait for a moment
+}
+
+// empty the commands array
+void iniCommands() {
+  for(int i=0; i<20; i++)
+    commands[i]=-1;
+}
+
+// add commands to the array
+void addCommands() {
+  Robot.stroke(0,0,0);
+  // display text on the screen
+  Robot.text("1. Press buttons to\n add commands.\n\n 2. Middle to finish.", 5, 5);
+  
+  // read the buttons' state
+  for(int i=0; i<20;) {  //max 20 commands
+    int key = Robot.keyboardRead();
+    if(key == BUTTON_MIDDLE) {  //finish input
+      break;
+    }else if(key == BUTTON_NONE) {  //if no button is pressed 
+      continue;
+    }
+    commands[i] = key; // save the button to the array
+    PrintCommandI(i, 46); // print the command on the screen
+    delay(100);
+    i++;
+  }
+}
+
+// run through the array and move the robot
+void executeCommands() {
+  // print status to the screen
+  Robot.text("Excuting...",5,70);
+  
+  // read through the array and move accordingly
+  for(int i=0; i<20; i++) {
+    switch(commands[i]) {
+      case BUTTON_LEFT:
+        Robot.turn(-90);
+        break;
+      case BUTTON_RIGHT:
+        Robot.turn(90);
+        break;
+      case BUTTON_UP:
+        Robot.motorsWrite(255, 255);
+        break;
+      case BUTTON_DOWN:
+        Robot.motorsWrite(-255, -255);
+        break;
+      case BUTTON_NONE:
+        return;
+    }
+    // print the current command to the screen
+    Robot.stroke(255,0,0);
+    PrintCommandI(i, 86);
+    delay(1000);
+    
+    // stop moving for a second
+    Robot.motorsStop();
+    delay(1000);
+  }
+}
+
+// convert the button press to a single character
+char keyToChar(int key) {
+  switch(key) {
+    case BUTTON_LEFT:
+      return '<';
+    case BUTTON_RIGHT:
+      return '>';
+    case BUTTON_UP:
+      return '^';
+    case BUTTON_DOWN:
+      return 'v';
+  }
+}
+
+// display a command
+void PrintCommandI(int i, int originY) {
+  Robot.text(keyToChar(commands[i]), i%14*8+5, i/14*10+originY);
+}
+
diff --git a/libraries/Robot_Control/examples/explore/R02_Line_Follow/R02_Line_Follow.ino b/libraries/Robot_Control/examples/explore/R02_Line_Follow/R02_Line_Follow.ino
new file mode 100644
index 0000000..58de253
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R02_Line_Follow/R02_Line_Follow.ino
@@ -0,0 +1,73 @@
+/* Robot Line Follow
+
+ This sketch demonstrates the line following capabilities
+ of the Arduino Robot. On the floor, place some black 
+ electrical tape along the path you wish the robot to follow.
+ To indicate a stopping point, place another piece of tape
+ perpendicular to the path.
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+
+long timerOrigin; // used for counting elapsed time
+
+void setup() {
+  // initialize the Robot, SD card, display, and speaker 
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+  Robot.beginSpeaker();
+
+  // show the logots on the TFT screen
+  Robot.displayLogos();
+  
+  Robot.drawBMP("lf.bmp", 0, 0); // display background image
+
+  Robot.playFile("chase.sqm");  // play a song from the SD card
+  
+  // add the instructions
+  Robot.text("Line Following\n\n place the robot on\n the track and \n see it run", 5, 5);
+  Robot.text("Press the middle\n button to start...", 5, 61);
+  Robot.waitContinue();
+
+  // These are some general values that work for line following 
+  // uncomment one or the other to see the different behaviors of the robot
+  // Robot.lineFollowConfig(11, 5, 50, 10);
+  Robot.lineFollowConfig(14, 9, 50, 10);
+  
+  //set the motor board into line-follow mode
+  Robot.setMode(MODE_LINE_FOLLOW);  
+  
+  // start
+  Robot.fill(255, 255, 255);
+  Robot.stroke(255, 255, 255);
+  Robot.rect(0, 0, 128, 80); // erase the previous text
+  Robot.stroke(0, 0, 0);
+  Robot.text("Start", 5, 5);
+  
+  Robot.stroke(0, 0, 0); // choose color for the text
+  Robot.text("Time passed:", 5, 21); // write some text to the screen
+  
+  timerOrigin=millis(); // keep track of the elapsed time
+  
+  while(!Robot.isActionDone()) {  //wait for the finish signal
+    Robot.debugPrint(millis()-timerOrigin, 5, 29);  // show how much time has passed
+  }
+  
+  Robot.stroke(0, 0, 0);  
+  Robot.text("Done!", 5, 45);
+}
+void loop() {
+  //nothing here, the program only runs once. Reset the robot 
+  //to do it again!
+}
diff --git a/libraries/Robot_Control/examples/explore/R03_Disco_Bot/R03_Disco_Bot.ino b/libraries/Robot_Control/examples/explore/R03_Disco_Bot/R03_Disco_Bot.ino
new file mode 100644
index 0000000..3574b01
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R03_Disco_Bot/R03_Disco_Bot.ino
@@ -0,0 +1,179 @@
+/* Disco Bot
+
+ This sketch shows you how to use the melody playing 
+ feature of the robot, with some really cool 8-bit music. 
+ Music will play when the robot is turned on, and it 
+ will show you some dance moves.
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+
+/* Dancing steps:
+  S: stop
+  L: turn left
+  R: turn right
+  F: go forward
+  B: go backwards
+
+  The number after each command determines how long 
+  each step lasts. Each number is 1/2 second long.
+
+  The "\0" indicates end of string
+*/
+char danceScript[] = "S4L1R1S2F1B1S1\0"; 
+
+int currentScript = 0; // what step are we at
+
+int currentSong = 0; // keep track of the current song
+static const int SONGS_COUNT = 3; // number of songs
+
+// an array to hold the songs
+char musics[][11] = {
+  "melody.sqm",
+  "menu.sqm",
+  "chase.sqm",
+};
+
+// variables for non-blocking delay
+long waitFrom;
+long waitTime = 0;
+
+void setup() {
+  // initialize the Robot, SD card, display, and speaker 
+  Robot.begin();
+  Robot.beginSpeaker();
+  Robot.beginSD();
+  Robot.beginTFT();
+ 
+  // draw "lg0.bmp" and "lg1.bmp" on the screen
+  Robot.displayLogos();
+  
+  // Print instructions to the screen
+  Robot.text("1. Use left and\n right key to switch\n song", 5, 5);
+  Robot.text("2. Put robot on the\n ground to dance", 5, 33);
+
+  // wait for a few soconds
+  delay(3000);
+  
+  setInterface(); // display the current song
+  play(0);  //play the first song in the array
+  
+  resetWait();  //Initialize non-blocking delay
+}
+
+void loop() {
+  // read the butttons on the robot
+  int key = Robot.keyboardRead();
+  
+  // Right/left buttons play next/previous song
+  switch(key) {
+    case BUTTON_UP:
+    case BUTTON_LEFT:
+      play(-1);  //play previous song
+      break;
+    case BUTTON_DOWN:
+    case BUTTON_RIGHT:
+      play(1);  //play next song
+      break;
+  }
+  
+  // dance!
+  runScript();
+}
+
+// Dancing function
+void runScript() { 
+ if(!waiting()) { // if the previous instructions have finished
+   // get the next 2 commands (direction and duration)
+    parseCommand(danceScript[currentScript], danceScript[currentScript+1]);
+    currentScript += 2;
+    if(danceScript[currentScript] == '\0') // at the end of the array
+      currentScript = 0; // start again at the beginning
+ }
+}
+
+// instead of delay, use this timer
+boolean waiting() {
+  if(millis()-waitFrom >= waitTime)
+    return false;
+  else
+    return true;
+}
+
+// how long to wait
+void wait(long t) {
+  resetWait();
+  waitTime = t;
+}
+
+// reset the timer
+void resetWait() {
+  waitFrom = millis();
+}
+
+// read the direction and dirstion of the steps
+void parseCommand(char dir, char duration) { 
+  //convert the scripts to action
+  switch(dir) { 
+    case 'L':
+      Robot.motorsWrite(-255, 255);
+      break;
+    case 'R':
+      Robot.motorsWrite(255, -255);
+      break;
+    case 'F':
+      Robot.motorsWrite(255, 255);
+      break;
+    case 'B':
+      Robot.motorsWrite(-255, -255);
+      break;
+    case 'S':
+      Robot.motorsStop();
+      break;
+  }
+  //You can change "500" to change the pace of dancing
+  wait(500*(duration-'0'));
+}
+
+// display the song
+void setInterface() {
+  Robot.clearScreen();
+  Robot.stroke(0, 0, 0);
+  Robot.text(musics[0], 0, 0);
+}
+
+// display the next song
+void select(int seq, boolean onOff) {
+  if(onOff){//select
+    Robot.stroke(0, 0, 0);
+    Robot.text(musics[seq], 0, 0);
+  }else{//deselect
+    Robot.stroke(255, 255, 255);
+    Robot.text(musics[seq], 0, 0);
+  }
+}
+
+// play the slected song
+void play(int seq) {
+  select(currentSong, false);
+  if(currentSong <= 0 && seq == -1) {  //previous of 1st song?
+    currentSong = SONGS_COUNT-1;  //go to last song
+  } else if(currentSong >= SONGS_COUNT-1 && seq == 1) {  //next of last?
+    currentSong = 0;  //go to 1st song
+  } else {
+    currentSong += seq;  //next song
+  }
+  Robot.stopPlayFile();
+  Robot.playFile(musics[currentSong]);
+  select(currentSong, true);  //display the current song
+}
diff --git a/libraries/Robot_Control/examples/explore/R04_Compass/R04_Compass.ino b/libraries/Robot_Control/examples/explore/R04_Compass/R04_Compass.ino
new file mode 100644
index 0000000..a7a7315
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R04_Compass/R04_Compass.ino
@@ -0,0 +1,70 @@
+/* Robot Compass
+
+ The robot has an on-board compass module, with
+ which it can tell the direction the robot is 
+ facing. This sketch will make sure the robot 
+ goes towards a certain direction. 
+ 
+ Beware, magnets will interfere with the compass
+ readings.
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+// include the robot library
+#include <ArduinoRobot.h>
+
+int speedLeft;
+int speedRight;
+int compassValue;
+int direc = 180;  //Direction the robot is heading
+
+void setup() {
+  // initialize the modules
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+  Robot.displayLogos();
+}
+
+void loop() { 
+  // read the compass orientation
+  compassValue = Robot.compassRead();
+  
+  // how many degrees are we off
+  int diff = compassValue-direc;
+  
+  // modify degress 
+  if(diff > 180)
+    diff = -360+diff;
+  else if(diff < -180)
+    diff = 360+diff;
+    
+  // Make the robot turn to its proper orientation
+  diff = map(diff, -180, 180, -255, 255);
+  
+  if(diff > 0) {
+    // keep the right wheel spinning, 
+    // change the speed of the left wheel 
+    speedLeft = 255-diff;
+    speedRight = 255;
+  } else {
+    // keep the right left spinning,
+    // change the speed of the left wheel   
+    speedLeft = 255;
+    speedRight = 255+diff;
+  }
+  // write out to the motors
+  Robot.motorsWrite(speedLeft, speedRight);
+
+  // draw the orientation on the screen
+  Robot.drawCompass(compassValue);
+}
diff --git a/libraries/Robot_Control/examples/explore/R05_Inputs/R05_Inputs.ino b/libraries/Robot_Control/examples/explore/R05_Inputs/R05_Inputs.ino
new file mode 100644
index 0000000..1359f8d
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R05_Inputs/R05_Inputs.ino
@@ -0,0 +1,166 @@
+/* Robot Inputs
+
+ This sketch shows you how to use the on-board 
+ potentiometer and buttons as inputs. 
+
+ Turning the potentiometer draws a clock-shaped 
+ circle. The up and down buttons change the pitch, 
+ while the left and right buttons change the tempo. 
+ The middle button resets tempo and pitch.
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+// default tempo and pitch of the music
+int tempo = 60;
+int pitch = 1000;
+
+void setup() {
+  // initialize the Robot, SD card, speaker, and display 
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSpeaker();
+  Robot.beginSD();
+
+  // draw "lg0.bmp" and "lg1.bmp" on the screen  
+  Robot.displayLogos();
+
+  // play a sound file  
+  Robot.playFile("Melody.sqm");
+}
+
+void loop() {
+  // check the value of the buttons
+  keyDown(Robot.keyboardRead());
+
+  // check the value of the pot
+  drawKnob(Robot.knobRead());
+}
+
+// Draw the basic interface
+void renderUI() {
+  //fill the buttons blank
+  Robot.fill(255, 255, 255);      
+  Robot.rect(53, 58, 13, 13); // left
+  Robot.rect(93, 58, 13, 13); // right
+  Robot.rect(73, 38, 13, 13); // up
+  Robot.circle(79, 64, 6); // middle
+  Robot.rect(73, 78, 13, 13); // down
+  Robot.circle(26, 116, 18); // knob
+
+  //draw the vertical bargraph
+  int fullPart=map(pitch, 200, 2000, 0, 58);  //length of filled bargraph
+  Robot.fill(255, 255, 255);        
+  Robot.rect(21, 30, 13, 58-fullPart);
+  Robot.fill(0, 0, 255);        
+  Robot.rect(21, 88-fullPart, 13, fullPart); //58-fullPart+30
+
+  //draw the horizontal bargraph
+  fullPart = map(tempo, 20, 100, 0, 58);  // length of filled bargraph
+  Robot.fill(255, 190, 0);  
+  Robot.rect(53, 110, fullPart, 13);
+  Robot.fill(255, 255, 255);        
+  Robot.rect(53+fullPart, 110, 58-fullPart, 13);
+}
+
+void keyDown(int keyCode) {
+  // use a static int so it is persistent over time
+  static int oldKey;
+  switch(keyCode) {
+    case BUTTON_LEFT:
+      //left button pressed, reduces tempo
+      tempo -= 5;
+      if(tempo < 20) tempo = 20; //lowest tempo 20
+      Robot.fill(255,190,0);
+
+      Robot.rect(53, 58, 13, 13);
+      break;
+    case BUTTON_RIGHT:
+      //right button pressed, increases tempo
+      tempo += 5;
+      if(tempo > 100) tempo = 100; //highest tempo 100
+      Robot.fill(255,190,0);
+      Robot.rect(93, 58, 13, 13);
+      break;
+    case BUTTON_UP:
+      //up button pressed, increases pitch
+      pitch += 120;
+      if(pitch > 2000) pitch = 2000;
+      Robot.fill(0, 0, 255);
+
+      Robot.rect(73, 38, 13, 13);
+      break;
+    case BUTTON_DOWN:
+      //down button pressed, reduces pitch
+      pitch -= 120;
+      if(pitch < 200){
+        pitch = 200;  
+      }
+      Robot.fill(0, 0, 255);
+
+      Robot.rect(73, 78, 13, 13);
+      break;
+    case BUTTON_MIDDLE:
+      //middle button pressed, resets tempo and pitch
+      tempo = 60;
+      pitch = 1000;
+      Robot.fill(160,160,160);
+
+      Robot.circle(79, 64, 6);
+      break;
+    case BUTTON_NONE:
+      //Only when the keys are released(thus BUTTON_NONE is
+      //encountered the first time), the interface will be 
+      //re-drawn.
+      if(oldKey != BUTTON_NONE){  
+        renderUI();
+      }
+      break;
+  }
+  if(oldKey != keyCode) {
+    // change the song's tempo
+    Robot.tempoWrite(tempo);
+    // change the song's pitch
+    Robot.tuneWrite(float(pitch/1000.0));
+  }
+  oldKey = keyCode;
+}
+
+void drawKnob(int val) {
+  static int x = 0, y = 0, val_old = 0;
+  // radian number, -3.14 to 3.14
+  float ang = map(val, 0, 1023, -PI*1000, PI*1000) / 1000.0;
+
+  // erase the old line  
+  if (val_old != val) {
+    Robot.stroke(255, 255, 255);
+    Robot.line(26, 116, x, y);
+  }
+  
+  // the following lines avoid a glitch in the TFT library
+  // that seems to appear when drawing a vertical line 
+  if (val < 1011 && val > 265 || val < 253) { 
+    //a bit math for drawing the hand inside the clock
+    x = 16*sin(ang)+26;
+    y = 16*cos(ang)+116;
+  } 
+  if (val > 265 && val < 253) {
+    x = 10; y = 116; 
+  }    
+  if (val >= 1011) {
+    x = 27; y = 100; 
+  }    
+  Robot.stroke(0, 0, 0);
+  Robot.line(26, 116, x, y); 
+  val_old = val;
+}
diff --git a/libraries/Robot_Control/examples/explore/R06_Wheel_Calibration/R06_Wheel_Calibration.ino b/libraries/Robot_Control/examples/explore/R06_Wheel_Calibration/R06_Wheel_Calibration.ino
new file mode 100644
index 0000000..c571b3a
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R06_Wheel_Calibration/R06_Wheel_Calibration.ino
@@ -0,0 +1,103 @@
+/* 6 Wheel Calibration
+
+ Use this sketch to calibrate the wheels in your robot. 
+ Your robot should drive as straight as possible when 
+ putting both motors at the same speed.
+
+ Run the software and follow the on-screen instructions. 
+ Use the trimmer on the motor board to make sure the 
+ robot is working at its best!
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // inport the robot librsry
+// import the utility library
+// a description of its funtionality is below
+#include <utility/RobotTextManager.h> 
+
+// arrays to hold the text for instructions
+char script1[] ="Wheel Calibration";
+char script2[] ="1. Put Robot on a\n flat surface";
+char script3[] ="2. Adjust speed with the knob on top";
+char script4[] ="3. If robot goes\n straight, it's done";
+char script5[] ="4. Use screwdriver\n on the bottom trim";
+char script6[] ="- Robot turns left,\n screw it clockwise;";
+char script7[] ="- Turns right, screw it ct-colockwise;";
+char script8[] ="5. Repeat 4 until\n going straight";
+
+int speedRobot;  //robot speed
+int calibrationValue; //value for calibrate difference between wheels
+
+void setup(){
+  //necessary initialization sequence
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+  
+  // left and top margin for displaying text
+  // see below for a description of this
+  textManager.setMargin(5,5);
+  // write all instructions at once
+  writeAllscript();
+  
+}
+void loop(){
+  //Control the robot's speed with knob on top
+  int speedRobot=map(Robot.knobRead(),0,1023,-255,255);
+  Robot.motorsWrite(speedRobot,speedRobot);
+
+  //read value of the pot on motor baord,to clibrate the wheels
+  int calibrationValue=map(Robot.trimRead(),0,1023,-30,30);
+  // print the values to the screen
+  Robot.debugPrint(calibrationValue,110,145);
+  delay(40);
+
+}
+
+void writeAllscript(){
+  //prints 8 scripts one after another
+  textManager.writeText(0,0,script1);
+  textManager.writeText(1,0,script2);
+  textManager.writeText(3,0,script3);
+  textManager.writeText(5,0,script4);
+  textManager.writeText(7,0,script5);
+  textManager.writeText(9,0,script6);
+  textManager.writeText(11,0,script7);
+  textManager.writeText(13,0,script8);
+}
+
+/**
+textManager mostly contains helper functions for 
+R06_Wheel_Calibration and R01_Hello_User.
+
+  textManager.setMargin(margin_left, margin_top):
+    Configure the left and top margin for text
+    display. The margins will be used by 
+    textManager.writeText().
+    Parameters:
+      margin_left, margin_top: int, the margin values
+      from the top and left side of the screen.
+    Returns:
+      none
+    
+  textManager.writeText(line,column,text):
+    Display text on the specific line and column. 
+    It's different from Robot.text() which
+    uses pixels for positioning the text.
+    Parameters:
+      line:int, which line is the text displayed. Each line
+        is 10px high.
+      column:int,  which column is the text displayed. Each
+        column is 8px wide.
+      text:a char array(string) of the text to be displayed.
+    Returns:
+      none
+*/
diff --git a/libraries/Robot_Control/examples/explore/R07_Runaway_Robot/R07_Runaway_Robot.ino b/libraries/Robot_Control/examples/explore/R07_Runaway_Robot/R07_Runaway_Robot.ino
new file mode 100644
index 0000000..9832d29
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R07_Runaway_Robot/R07_Runaway_Robot.ino
@@ -0,0 +1,78 @@
+/* Runaway Robot
+
+ Play tag with your robot! With an ultrasonic 
+ distance sensor, it's capable of detecting and avoiding
+ obstacles, never bumping into walls again!
+  
+ You'll need to attach an untrasonic range finder to TK1.
+
+ Circuit:
+ * Arduino Robot
+ * US range finder like Maxbotix EZ10, with analog output
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+// include the robot library
+#include <ArduinoRobot.h>
+
+int sensorPin = TK1;  // pin is used by the sensor
+
+void setup() {
+  // initialize the Robot, SD card, and display
+  Serial.begin(9600);
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+  Robot.displayLogos();
+  
+  // draw a face on the LCD screen
+  setFace(true);
+}
+
+void loop() {
+  // If the robot is blocked, turn until free
+  while(getDistance() < 40) {  // If an obstacle is less than 20cm away
+    setFace(false); //shows an unhappy face
+    Robot.motorsStop(); // stop the motors
+    delay(1000); // wait for a moment
+    Robot.turn(90); // turn to the right and try again
+    setFace(true); // happy face
+  }
+  // if there are no objects in the way, keep moving
+  Robot.motorsWrite(255, 255); 
+  delay(100);
+}
+
+// return the distance in cm
+float getDistance() {
+  // read the value from the sensor
+  int sensorValue = Robot.analogRead(sensorPin);
+  //Convert the sensor input to cm.
+  float distance_cm = sensorValue*1.27;
+  return distance_cm;  
+}
+
+// make a happy or sad face
+void setFace(boolean onOff) {
+  if(onOff) {
+    // if true show a happy face
+    Robot.background(0, 0, 255);
+    Robot.setCursor(44, 60);
+    Robot.stroke(0, 255, 0);
+    Robot.setTextSize(4);
+    Robot.print(":)");
+  }else{
+    // if false show an upset face
+    Robot.background(255, 0, 0);
+    Robot.setCursor(44, 60);
+    Robot.stroke(0, 255, 0);
+    Robot.setTextSize(4);
+    Robot.print("X(");
+  }
+}
diff --git a/libraries/Robot_Control/examples/explore/R08_Remote_Control/R08_Remote_Control.ino b/libraries/Robot_Control/examples/explore/R08_Remote_Control/R08_Remote_Control.ino
new file mode 100644
index 0000000..fda21cb
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R08_Remote_Control/R08_Remote_Control.ino
@@ -0,0 +1,123 @@
+/* 08 Remote Control
+
+ *******************
+ ***
+ ***This example code is in an experimental state.
+ ***You are welcome to try this with your robot,
+ ***and no harm will come to it. We will provide a
+ ***detailed description of an updated version of this
+ ***in a future update
+ ***
+ *** For this example to work you need:
+ ***
+ *** - download and install the IR-Remote library by Ken Shirriff
+ ***   to be found at https://github.com/shirriff/Arduino-IRremote
+ *** - get a Sony remote control
+ ***
+ *** This example will be updated soon, come back to the Robot
+ *** page on the Arduino server for updates!!
+ ***
+ *******************
+ 
+ If you connect a IR receiver to the robot, 
+ you can control it like you control a TV set. 
+ Using a Sony compatiable remote control, 
+ map some buttons to different actions. 
+ You can make the robot move around without 
+ even touching it!
+
+ Circuit: 
+ * Arduino Robot
+ * Connect the IRreceiver to TDK2
+ * Sony compatible remote control
+
+ based on the IRremote library
+ by Ken Shirriff
+ http://arcfn.com
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+// include the necessary libraries
+#include <IRremote.h>
+#include <ArduinoRobot.h>
+
+// Define a few commands from your remote control
+#define IR_CODE_FORWARD 0x2C9B
+#define IR_CODE_BACKWARDS 0x6C9B
+#define IR_CODE_TURN_LEFT 0xD4B8F
+#define IR_CODE_TURN_RIGHT 0x34B8F
+
+int RECV_PIN = TKD2; // the pin the IR receiver is connected to
+IRrecv irrecv(RECV_PIN); // an instance of the IR receiver object
+decode_results results; // container for received IR codes
+
+void setup() {
+  // initialize the Robot, SD card, display, and speaker 
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+
+  // print some text to the screen
+  Robot.stroke(0, 0, 0);
+  Robot.text("Remote Control code:", 5, 5);
+  Robot.text("Command:", 5, 26);
+  irrecv.enableIRIn(); // Start the receiver
+}
+
+void loop() {
+  // if there is an IR command, process it
+  if (irrecv.decode(&results)) {
+    processResult();
+    irrecv.resume(); // resume receiver
+  }
+}
+
+void processResult() {
+  unsigned long res = results.value;
+  // print the value to the screen
+  Robot.debugPrint(res, 5, 15);
+
+  if(res == IR_CODE_FORWARD || res == IR_CODE_BACKWARDS || res == IR_CODE_TURN_LEFT || res == IR_CODE_TURN_RIGHT) {
+    Robot.fill(255, 255, 255);
+    Robot.stroke(255, 255, 255);
+
+    Robot.rect(5, 36, 55, 10);
+  }
+  switch(results.value){
+    case IR_CODE_FORWARD:
+      Robot.stroke(0, 0, 0);
+      Robot.text("Forward", 5, 36);
+      Robot.motorsWrite(255, 255);
+      delay(300);
+      Robot.motorsStop();
+      break;
+    case IR_CODE_BACKWARDS:
+      Robot.stroke(0, 0, 0);
+      Robot.text("Backwards", 5, 36);
+      Robot.motorsWrite(-255, -255);
+      delay(300);
+      Robot.motorsStop();
+      break;
+    case IR_CODE_TURN_LEFT:
+      Robot.stroke(0, 0, 0);
+      Robot.text("Left", 5, 36);
+      Robot.motorsWrite(-255, 255); 
+      delay(100);
+      Robot.motorsStop();
+      break;
+    case IR_CODE_TURN_RIGHT:
+      Robot.stroke(0, 0, 0);
+      Robot.text("Right", 5, 36);
+      Robot.motorsWrite(255, -255); 
+      delay(100);
+      Robot.motorsStop();
+      break;
+  }
+}
+
diff --git a/libraries/Robot_Control/examples/explore/R09_Picture_Browser/R09_Picture_Browser.ino b/libraries/Robot_Control/examples/explore/R09_Picture_Browser/R09_Picture_Browser.ino
new file mode 100644
index 0000000..a43348c
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R09_Picture_Browser/R09_Picture_Browser.ino
@@ -0,0 +1,159 @@
+/* Picture Browser
+
+ You can make your own gallery/picture show with the 
+ Robot. Put some pictures on the SD card, start the 
+ sketch, they will diplay on the screen. 
+ 
+ Use the left/right buttons to navigate through the 
+ previous and next images. 
+ 
+ Press up or down to enter a mode where you change 
+ the pictures by rotating the robot.
+ 
+ You can add your own pictures onto the SD card, and 
+ view them in the Robot's gallery!
+
+ Pictures must be uncompressed BMP, 24-bit color depth, 
+ 160 pixels wide, and 128 pixels tall.
+ 
+ They should be named as "picN.bmp". Replace 'N' with a 
+ number between 0 and 9.
+
+ The current code only supports 10 pictures. How would you 
+ improve it to handle more?
+
+ Circuit:
+ * Arduino Robot
+
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+
+const int NUM_PICS = 4;  //Total number of pictures in Gallery
+
+// name the modes
+const int CONTROL_MODE_KEY = 0;
+const int CONTROL_MODE_COMPASS = 1;
+
+char buffer[] = "pic1.bmp";  // current file name
+int i = 1;  // Current gallery sequence counter
+int mode = 0;  // Current mode
+
+// text to display on screen
+char modeNames[][9] = { "keyboard", "tilt    " };
+
+void setup() {
+  // initialize the Robot, SD card, display, and speaker 
+  Robot.beginSD();
+  Robot.beginTFT();
+  Robot.begin();
+  
+  // draw "lg0.bmp" and "lg1.bmp" on the screen  
+  Robot.displayLogos();
+  
+  // draw init3.bmp from the SD card on the screen
+  Robot.drawBMP("init3.bmp", 0, 0);
+
+  // display instructions
+  Robot.stroke(0, 0, 0);
+  Robot.text("The gallery\n\n has 2 modes, in\n keyboard mode, L/R\n key for switching\n pictures, U/D key\n for changing modes", 5, 5);
+  delay(6000);
+  Robot.clearScreen();
+  Robot.drawBMP("pb.bmp", 0, 0);
+  Robot.text("In tilt mode,\n quickly tilt the\n robot to switch\n pictures", 5, 5);
+  delay(4000);
+}
+
+void loop() {
+  buffer[3] = '0'+i;// change filename of the img to be displayed
+  Robot.drawBMP(buffer, 0, 0); // draw the file on the screen
+  // change control modes
+  switch(mode) { 
+    case CONTROL_MODE_COMPASS:
+      compassControl(3);
+      break;
+    case CONTROL_MODE_KEY:
+      keyboardControl();
+      break;
+  }
+  delay(200);
+}
+
+void keyboardControl() {
+  //Use buttons to control the gallery
+  while(true) {
+    int keyPressed = Robot.keyboardRead(); // read the button values
+    switch(keyPressed) {
+      case BUTTON_LEFT:  // display previous picture
+        if(--i < 1) i = NUM_PICS;
+        return;
+      case BUTTON_MIDDLE:  // do nothing
+      case BUTTON_RIGHT:  // display next picture
+        if(++i > NUM_PICS) i = 1;
+        return;
+      case BUTTON_UP:  // change mode
+        changeMode(-1);
+        return;
+      case BUTTON_DOWN:  // change mode
+        changeMode(1);
+        return;
+    }
+  }
+}
+
+// if controlling by the compass
+void compassControl(int change) {
+  // Rotate the robot to change the pictures
+  while(true) {
+    // read the value of the compass
+    int oldV = Robot.compassRead();
+    
+    //get the change of angle
+    int diff = Robot.compassRead()-oldV;
+    if(diff > 180) diff -= 360;
+    else if(diff < -180) diff += 360;
+    
+    if(abs(diff) > change) {
+      if(++i > NUM_PICS) i = 1;
+      return;
+    }
+    
+    // chage modes, if buttons are pressed
+    int keyPressed = Robot.keyboardRead();
+    switch(keyPressed) {
+      case BUTTON_UP:
+        changeMode(-1);
+        return;
+      case BUTTON_DOWN:
+        changeMode(1);
+        return;
+    }
+    delay(10);
+  }
+}
+
+// Change the control mode and display it on the LCD
+void changeMode(int changeDir) {
+  // alternate modes
+  mode += changeDir;
+  if(mode < 0) { 
+    mode = 1;
+  } else if(mode > 1)
+    mode=0;
+    
+  // display  the mode on screen  
+  Robot.fill(255, 255, 255);  
+  Robot.stroke(255, 255, 255);
+  Robot.rect(0, 0, 128, 12);
+  Robot.stroke(0, 0, 0);
+  Robot.text("Control:", 2, 2);
+  Robot.text(modeNames[mode], 52, 2);
+  delay(1000);
+}
+
diff --git a/libraries/Robot_Control/examples/explore/R10_Rescue/R10_Rescue.ino b/libraries/Robot_Control/examples/explore/R10_Rescue/R10_Rescue.ino
new file mode 100644
index 0000000..48044db
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R10_Rescue/R10_Rescue.ino
@@ -0,0 +1,124 @@
+/* Robot Rescue
+
+ In this example, the robot enters the line following mode and
+ plays some music until it reaches its target. Once it finds the 
+ target, it pushes it out of the track. It then returns to the 
+ track and looks for a second target.
+
+ You can make the robot push as many objects as you want to, just
+ add more to calls to the rescue function or even move that code
+ into the loop.
+
+ Circuit:
+ * Arduino Robot
+ * some objects for the robot to push
+ * a line-following circuit
+
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+
+void setup(){
+  // initialize the Robot, SD card, display, and speaker 
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSD();
+  Robot.beginSpeaker();
+ 
+  // draw "lg0.bmp" and "lg1.bmp" on the screen 
+  Robot.displayLogos();
+
+  // display the line following instructional image from the SD card
+  Robot.drawBMP("lf.bmp", 0, 0);
+
+  // play the chase music file
+  Robot.playFile("chase.sqm");
+
+  // add the instructions
+  Robot.text("Rescue\n\n place the robot on\n the rescue track\n pushing the\n obstacles away", 5, 5);
+  Robot.text("Press the middle\n button to start...", 5, 61);
+  Robot.waitContinue();
+
+  // start
+  Robot.fill(255, 255, 255);
+  Robot.stroke(255, 255, 255);
+  Robot.rect(0, 0, 128, 80); // erase the previous text
+  Robot.stroke(0, 0, 0);
+  Robot.text("Start", 5, 5);
+  
+  // use this to calibrate the line following algorithm
+  // uncomment one or the other to see the different behaviors of the robot
+  // Robot.lineFollowConfig(11, 5, 50, 10);
+  Robot.lineFollowConfig(14, 9, 50, 10);
+  
+  // run the rescue sequence  
+  rescueSequence();
+  Robot.text("Found obstacle", 5, 12);
+  // find the track again
+  goToNext();
+  Robot.text("Found track", 5, 19);
+  // run the rescue sequence a second time
+  rescueSequence();
+  Robot.text("Found obstacle", 5, 26);
+  
+  // here you could go on ...
+ 
+  // write status on the screen
+  Robot.stroke(0, 0, 0);
+  Robot.text("Done!", 5, 25);
+}
+
+void loop(){
+  //nothing here, the program only runs once.
+}
+
+// run the sequence
+void rescueSequence(){
+  //set the motor board into line-follow mode
+  Robot.setMode(MODE_LINE_FOLLOW);  
+  
+  while(!Robot.isActionDone()){  // wait until it is no longer following the line
+  }
+  delay(1000);
+
+  // do the rescue operation
+  doRescue();
+  delay(1000);
+}
+
+void doRescue(){
+  // Reached the endline, engage the target
+  Robot.motorsWrite(200,200);
+  delay(250);
+  Robot.motorsStop();
+  delay(1000);
+
+  // Turn the robot 
+  Robot.turn(90);
+  Robot.motorsStop();
+  delay(1000);
+  
+  // Move forward
+  Robot.motorsWrite(200,200);
+  delay(500);
+  Robot.motorsStop();
+  delay(1000);
+  
+  // move backwards, leave the target
+  Robot.motorsWrite(-200,-200);
+  delay(500);
+  Robot.motorsStop();
+}
+
+void goToNext(){
+  // Turn the robot 
+  Robot.turn(-90);
+  Robot.motorsStop();
+  delay(1000);
+}
diff --git a/libraries/Robot_Control/examples/explore/R11_Hello_User/R11_Hello_User.ino b/libraries/Robot_Control/examples/explore/R11_Hello_User/R11_Hello_User.ino
new file mode 100644
index 0000000..90fbfff
--- /dev/null
+++ b/libraries/Robot_Control/examples/explore/R11_Hello_User/R11_Hello_User.ino
@@ -0,0 +1,181 @@
+/* Hello User
+
+ Hello User! This sketch is the first thing you see 
+ when starting this robot. It gives you a warm welcome, 
+ showing you some of the really amazing abilities of 
+ the robot, and make itself really personal to you. 
+
+ Circuit:
+ * Arduino Robot
+
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h> // include the robot library
+// include the utility function for ths sketch
+// see the details below
+#include <utility/RobotTextManager.h>  
+
+char buffer[20];//for storing user name
+
+void setup(){
+  //necessary initialization sequence
+  Robot.begin();
+  Robot.beginTFT();
+  Robot.beginSpeaker(32000);
+  Robot.beginSD();
+
+  // show the logos from the SD card
+  Robot.displayLogos();
+  
+  // play the music file
+  Robot.playFile("menu.sqm");
+
+  // clear the screen
+  Robot.clearScreen();
+  
+  // From now on, display different slides of 
+  // text/pictures in sequence. The so-called
+  // scripts are strings of text stored in the
+  // robot's memory
+  
+  // these functions are explained below
+  
+  //Script 6
+  textManager.writeScript(5, 4, 0); 
+  textManager.writeScript(9, 10, 0);
+  Robot.waitContinue();
+  delay(500);
+  Robot.clearScreen();
+
+  //Script 7
+  textManager.writeScript(6, 4, 0);
+  textManager.writeScript(9, 10, 0);
+  Robot.waitContinue();
+  delay(500);
+  Robot.clearScreen();
+
+  //Script 8
+  // this function enables sound and images at once
+  textManager.showPicture("init2.bmp", 0, 0);
+  
+  textManager.writeScript(7, 2, 0);
+  textManager.writeScript(9, 7, 0);
+  Robot.waitContinue();
+  delay(500);
+  Robot.clearScreen();
+
+  //Script 9
+  textManager.showPicture("init3.bmp", 0, 0);
+  textManager.writeScript(8, 2, 0);
+  textManager.writeScript(9, 7, 0);
+  Robot.waitContinue();
+  delay(500);
+  Robot.clearScreen();
+
+  //Script 11
+  textManager.writeScript(10, 4, 0);
+  textManager.writeScript(9, 10, 0);
+  Robot.waitContinue();
+  delay(500);
+  Robot.clearScreen();
+
+  //Input screen
+  textManager.writeScript(0, 1, 1);
+  textManager.input(3, 1, USERNAME);
+  
+  textManager.writeScript(1, 5, 1);
+  textManager.input(7, 1, ROBOTNAME);
+
+  delay(1000);
+  Robot.clearScreen();
+  
+  //last screen
+  textManager.showPicture("init4.bmp", 0, 0);
+  textManager.writeText(1, 2, "Hello");
+  Robot.userNameRead(buffer);
+  textManager.writeText(3, 2, buffer);
+
+  textManager.writeScript(4,10,0);
+
+  Robot.waitContinue(BUTTON_LEFT);
+  Robot.waitContinue(BUTTON_RIGHT);
+  textManager.showPicture("kt1.bmp", 0, 0);
+}
+
+void loop(){
+  // do nothing here
+}
+
+
+/**
+textManager mostly contains helper functions for 
+R06_Wheel_Calibration and R01_Hello_User.
+
+The ones used in this example:
+  textManager.setMargin(margin_left, margin_top):
+    Configure the left and top margin for text
+    display. The margins will be used for 
+    textManager.writeText().
+    Parameters:
+      margin_left, margin_top: the margin values
+      from the top and left side of the screen.
+    Returns:
+      none
+    
+  textManager.writeScript(script_number,line,column):
+    Display a script of Hello User example. 
+    Parameters:
+      script_number: an int value representing the 
+        script to be displayed.
+      line, column: in which line,column is the script
+        displayed. Same as writeText().
+    Returns:
+      none
+  
+  textManager.input(line,column,codename):
+    Print an input indicator(">") in the line and column,
+    dispaly and receive input from a virtual keyboard,
+    and save the value into EEPROM represented by codename
+    Parameters:
+      line,column: int values represents where the input
+        starts. Same as wirteText().
+      codename: either USERNAME,ROBOTNAME,CITYNAME or
+        COUNTRYNAME. You can call Robot.userNameRead(),
+        robotNameRead(),cityNameRead() or countryNameRead()
+        to access the values later.
+    Returns:
+      none;
+  
+  textManager.writeText(line,column,text):
+    Display text on the specific line and column. 
+    It's different from Robot.text() as the later
+    uses pixels for positioning the text.
+    Parameters:
+      line:in which line is the text displayed. Each line
+        is 10px high.
+      column:in which column is the text displayed. Each
+        column is 8px wide.
+      text:a char array(string) of the text to be displayed.
+    Returns:
+      none
+      
+    textManager.showPicture(filename, x, y):
+      It has the same functionality as Robot.drawPicture(),
+      while fixing the conflict between drawPicture() and 
+      sound playing. Using Robot.drawPicture(), it'll have
+      glitches when playing sound at the same time. Using
+      showPicture(), it'll stop sound when displaying 
+      picture, so preventing the problem.
+      Parameters:
+        filename:string, name of the bmp file in sd
+        x,y: int values, position of the picture
+      Returns:
+        none  
+    
+*/
diff --git a/libraries/Robot_Control/examples/learn/AllIOPorts/AllIOPorts.ino b/libraries/Robot_Control/examples/learn/AllIOPorts/AllIOPorts.ino
new file mode 100644
index 0000000..3520214
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/AllIOPorts/AllIOPorts.ino
@@ -0,0 +1,149 @@
+/*
+ All IO Ports
+ 
+ This example goes through all the IO ports on your robot and
+ reads/writes from/to them. Uncomment the different lines inside 
+ the loop to test the different possibilities.
+
+ The TK inputs on the Control Board are multiplexed and therefore 
+ it is not recommended to use them as outputs. The TKD pins on the
+ Control Board as well as the TK pins on the Motor Board go directly
+ to the microcontroller and therefore can be used both as inputs 
+ and outputs.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+// use arrays to store the names of the pins to be read
+uint8_t arr[]  = { TK0, TK1, TK2, TK3, TK4, TK5, TK6, TK7 };
+uint8_t arr2[] = { TKD0, TKD1, TKD2, TKD3, TKD4, TKD5 };
+uint8_t arr3[] = { B_TK1, B_TK2, B_TK3, B_TK4 };
+
+void setup(){
+  // initialize the robot
+  Robot.begin();
+
+  // open the serial port to send the information of what you are reading
+  Serial.begin(9600);
+}
+
+void loop(){
+  // read all the TK inputs at the Motor Board as analog 
+  analogReadB_TKs();
+
+  // read all the TK inputs at the Motor Board as digital
+  //digitalReadB_TKs();
+
+  // read all the TK inputs at the Control Board as analog
+  //analogReadTKs();
+
+  // read all the TK inputs at the Control Board as digital
+  //digitalReadTKs();
+
+  // read all the TKD inputs at the Control Board as analog
+  //analogReadTKDs();
+
+  // read all the TKD inputs at the Control Board as digital
+  //digitalReadTKDs();
+
+  // write all the TK outputs at the Motor Board as digital
+  //digitalWriteB_TKs();
+
+  // write all the TKD outputs at the Control Board as digital
+  //digitalWriteTKDs();
+  delay(5);  
+}
+
+// read all TK inputs on the Control Board as analog inputs
+void analogReadTKs() {
+  for(int i=0;i<8;i++) {
+    Serial.print(Robot.analogRead(arr[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
+
+// read all TK inputs on the Control Board as digital inputs
+void digitalReadTKs() {
+  for(int i=0;i<8;i++) {
+    Serial.print(Robot.digitalRead(arr[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
+
+// read all TKD inputs on the Control Board as analog inputs
+void analogReadTKDs() {
+  for(int i=0; i<6; i++) {
+    Serial.print(Robot.analogRead(arr2[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
+
+// read all TKD inputs on the Control Board as digital inputs
+void digitalReadTKDs() {
+  for(int i=0; i<6; i++) {
+    Serial.print(Robot.digitalRead(arr2[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
+
+// write all TKD outputs on the Control Board as digital outputs
+void digitalWriteTKDs() {
+  // turn all the pins on
+  for(int i=0; i<6; i++) {
+    Robot.digitalWrite(arr2[i], HIGH);
+  }
+  delay(500);
+
+  // turn all the pins off
+  for(int i=0; i<6; i++){
+    Robot.digitalWrite(arr2[i], LOW);
+  }
+  delay(500);
+}
+
+// write all TK outputs on the Motor Board as digital outputs
+void digitalWriteB_TKs() {
+  // turn all the pins on
+  for(int i=0; i<4; i++) {
+    Robot.digitalWrite(arr3[i], HIGH);
+  }
+  delay(500);
+
+  // turn all the pins off
+  for(int i=0; i<4; i++) {
+    Robot.digitalWrite(arr3[i], LOW);
+  }
+  delay(500);
+}
+
+// read all TK inputs on the Motor Board as analog inputs
+void analogReadB_TKs() {
+  for(int i=0; i<4; i++) {
+    Serial.print(Robot.analogRead(arr3[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
+
+// read all TKD inputs on the Motor Board as digital inputs
+void digitalReadB_TKs() {
+  for(int i=0; i<4; i++) {
+    Serial.print(Robot.digitalRead(arr3[i]));
+    Serial.print(",");
+  }
+  Serial.println("");
+}
diff --git a/libraries/Robot_Control/examples/learn/Beep/Beep.ino b/libraries/Robot_Control/examples/learn/Beep/Beep.ino
new file mode 100644
index 0000000..1a78673
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/Beep/Beep.ino
@@ -0,0 +1,39 @@
+/*
+ Beep
+ 
+ Test different pre-configured beeps on
+ the robot's speaker.
+
+ Possible beeps are:
+ - BEEP_SIMPLE
+ - BEEP_DOUBLE
+ - BEEP_LONG
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the sound speaker
+  Robot.beginSpeaker();
+}
+void loop() {
+  Robot.beep(BEEP_SIMPLE);
+  delay(1000);
+  Robot.beep(BEEP_DOUBLE);
+  delay(1000);
+  Robot.beep(BEEP_LONG);
+  delay(1000);
+}
diff --git a/libraries/Robot_Control/examples/learn/CleanEEPROM/CleanEEPROM.ino b/libraries/Robot_Control/examples/learn/CleanEEPROM/CleanEEPROM.ino
new file mode 100644
index 0000000..ae14bdd
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/CleanEEPROM/CleanEEPROM.ino
@@ -0,0 +1,41 @@
+/*
+ Clean EEPROM
+ 
+ This example erases the user information stored on the
+ external EEPROM memory chip on your robot.
+
+ BEWARE, this will erase the following information:
+ - your name
+ - your robots name given by you
+ - your city and country if you configured them via software
+
+ EEPROMs shouldn't be rewritten too often, therefore the
+ code runs only during setup and not inside loop.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup(){
+  // initialize the robot
+  Robot.begin();
+
+  // write empty strings for the different fields
+  Robot.userNameWrite("");
+  Robot.robotNameWrite("");
+  Robot.cityNameWrite("");
+  Robot.countryNameWrite(""); 
+}
+
+void loop(){
+  // do nothing  
+}
diff --git a/libraries/Robot_Control/examples/learn/Compass/Compass.ino b/libraries/Robot_Control/examples/learn/Compass/Compass.ino
new file mode 100644
index 0000000..4170ab7
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/Compass/Compass.ino
@@ -0,0 +1,41 @@
+/*
+ Compass
+ 
+ Try the compass both on the robot's TFT
+ and through the serial port.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the robot's screen
+  Robot.beginTFT();
+
+  // initialize the serial port
+  Serial.begin(9600);
+}
+
+void loop() {
+  // read the compass
+  int compass = Robot.compassRead();
+
+  // print out the sensor's value
+  Serial.println(compass);  
+  
+  // show the value on the robot's screen
+  Robot.drawCompass(compass);
+}
+
diff --git a/libraries/Robot_Control/examples/learn/IRArray/IRArray.ino b/libraries/Robot_Control/examples/learn/IRArray/IRArray.ino
new file mode 100644
index 0000000..36b4acf
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/IRArray/IRArray.ino
@@ -0,0 +1,44 @@
+/*
+ IR array
+ 
+ Read the analog value of the IR sensors at the 
+ bottom of the robot. The also-called line following 
+ sensors are a series of pairs of IR sender/receiver
+ used to detect how dark it is underneath the robot.
+
+ The information coming from the sensor array is stored
+ into the Robot.IRarray[] and updated using the Robot.updateIR()
+ method.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup(){
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the serial port
+  Serial.begin(9600);
+}
+
+void loop(){
+  // store the sensor information into the array 
+  Robot.updateIR();
+
+  // iterate the array and print the data to the Serial port
+  for(int i=0; i<5; i++){
+    Serial.print(Robot.IRarray[i]);
+    Serial.print(" ");
+  }
+  Serial.println("");
+}
diff --git a/libraries/Robot_Control/examples/learn/LCDDebugPrint/LCDDebugPrint.ino b/libraries/Robot_Control/examples/learn/LCDDebugPrint/LCDDebugPrint.ino
new file mode 100644
index 0000000..0078b77
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/LCDDebugPrint/LCDDebugPrint.ino
@@ -0,0 +1,37 @@
+/*
+ LCD Debug Print
+ 
+ Use the Robot's library function debugPrint() to
+ quickly send a sensor reading to the robot's creen.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+int value;
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the screen
+  Robot.beginTFT();
+}
+void loop(){
+  // read a value
+  value = analogRead(A4);
+
+  // send the value to the screen
+  Robot.debugPrint(value);
+
+  delay(40);
+}
diff --git a/libraries/Robot_Control/examples/learn/LCDPrint/LCDPrint.ino b/libraries/Robot_Control/examples/learn/LCDPrint/LCDPrint.ino
new file mode 100644
index 0000000..d34168c
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/LCDPrint/LCDPrint.ino
@@ -0,0 +1,44 @@
+/*
+ LCD Print
+ 
+ Print the reading from a sensor to the screen.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+int value;
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the robot's screen
+  Robot.beginLCD();
+}
+
+void loop() {
+  // read a analog port
+  value=Robot.analogRead(TK4);
+
+  // write the sensor value on the screen
+  Robot.fill(0, 255, 0);
+  Robot.textSize(1);
+  Robot.text(value, 0, 0);  
+
+  delay(500);
+
+  // erase the previous text on the screen
+  Robot.fill(255, 255, 255);
+  Robot.textSize(1);
+  Robot.text(value, 0, 0);  
+}
diff --git a/libraries/Robot_Control/examples/learn/LCDWriteText/LCDWriteText.ino b/libraries/Robot_Control/examples/learn/LCDWriteText/LCDWriteText.ino
new file mode 100644
index 0000000..e34a7d2
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/LCDWriteText/LCDWriteText.ino
@@ -0,0 +1,41 @@
+/*
+ LCD Write Text
+ 
+ Use the Robot's library function text() to
+ print out text to the robot's screen. Take
+ into account that you need to erase the
+ information before continuing writing.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the screen
+  Robot.beginTFT();
+}
+void loop() {
+  Robot.stroke(0, 0, 0);              // choose the color black
+  Robot.text("Hello World", 0, 0);  // print the text
+  delay(2000);
+  Robot.stroke(255, 255, 255);        // choose the color white
+  Robot.text("Hello World", 0, 0);  // writing text in the same color as the BG erases the text!
+  
+  Robot.stroke(0, 0, 0);              // choose the color black
+  Robot.text("I am a robot", 0, 0); // print the text
+  delay(3000);
+  Robot.stroke(255, 255, 255);         // choose the color black
+  Robot.text("I am a robot", 0, 0);  // print the text
+}
diff --git a/libraries/Robot_Control/examples/learn/LineFollowWithPause/LineFollowWithPause.ino b/libraries/Robot_Control/examples/learn/LineFollowWithPause/LineFollowWithPause.ino
new file mode 100644
index 0000000..a3d3fc0
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/LineFollowWithPause/LineFollowWithPause.ino
@@ -0,0 +1,49 @@
+/*
+ Line Following with Pause
+ 
+ As the robot has two processors, one to command the motors and one to
+ take care of the screen and user input, it is possible to write 
+ programs that put one part of the robot to do something and get the
+ other half to control it.
+
+ This example shows how the Control Board assigns the Motor one to
+ follow a line, but asks it to stop every 3 seconds.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the screen
+  Robot.beginTFT();
+
+  // get some time to place the robot on the ground
+  delay(3000);
+
+  // set the robot in line following mode
+  Robot.setMode(MODE_LINE_FOLLOW);
+}
+
+void loop() {
+      // tell the robot to take a break and stop
+      Robot.pauseMode(true);
+      Robot.debugPrint('p');
+      delay(3000);
+
+      // tell the robot to move on
+      Robot.pauseMode(false);
+      Robot.debugPrint('>');
+      delay(3000);
+}
diff --git a/libraries/Robot_Control/examples/learn/Melody/Melody.ino b/libraries/Robot_Control/examples/learn/Melody/Melody.ino
new file mode 100644
index 0000000..6c049a7
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/Melody/Melody.ino
@@ -0,0 +1,62 @@
+/*
+ Melody
+ 
+ Plays a melody stored in a string. 
+ 
+ The notes and durations are encoded as follows:
+
+ NOTES:
+ c  play "C"
+ C  play "#C"
+ d  play "D"
+ D  play "#D"
+ e  play "E"
+ f  play "F"
+ F  play "#F"
+ g  play "G"
+ G  play "#G"
+ a  play "A"
+ A  play "#A"
+ b  play "B"
+ -  silence
+
+ DURATIONS:
+ 1  Set as full note
+ 2  Set as half note
+ 4  Set as quarter note
+ 8  Set as eigth note
+
+ SPECIAL NOTATION:
+ .  Make the previous note 3/4 the length
+
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+
+ This code uses the Squawk sound library designed by STG. For
+ more information about it check: http://github.com/stg/squawk
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the sound library
+  Robot.beginSpeaker();
+}
+
+void loop() {
+  // array containing the melody
+  char aTinyMelody[] = "8eF-FFga4b.a.g.F.8beee-d2e.1-";
+
+  // play the melody
+  Robot.playMelody(aTinyMelody);
+}
diff --git a/libraries/Robot_Control/examples/learn/MotorTest/MotorTest.ino b/libraries/Robot_Control/examples/learn/MotorTest/MotorTest.ino
new file mode 100644
index 0000000..baaaf06
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/MotorTest/MotorTest.ino
@@ -0,0 +1,41 @@
+/*
+ Motor Test
+ 
+ Just see if the robot can move and turn.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+}
+
+void loop() {
+  Robot.motorsWrite(255,255);   // move forward
+  delay(2000);
+  Robot.motorsStop();           // fast stop
+  delay(1000);
+  Robot.motorsWrite(-255,-255); // backward
+  delay(1000);
+  Robot.motorsWrite(0,0);       // slow stop
+  delay(1000);
+  Robot.motorsWrite(-255,255);  // turn left
+  delay(2000);
+  Robot.motorsStop();           // fast stop
+  delay(1000);
+  Robot.motorsWrite(255,-255);  // turn right
+  delay(2000);
+  Robot.motorsStop();           // fast stop
+  delay(1000);
+}
diff --git a/libraries/Robot_Control/examples/learn/SpeedByPotentiometer/SpeedByPotentiometer.ino b/libraries/Robot_Control/examples/learn/SpeedByPotentiometer/SpeedByPotentiometer.ino
new file mode 100644
index 0000000..e97f48d
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/SpeedByPotentiometer/SpeedByPotentiometer.ino
@@ -0,0 +1,39 @@
+/*
+ Speed by Potentiometer
+ 
+ Control the robot's speed using the on-board
+ potentiometer. The speed will be printed on
+ the TFT screen. 
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+
+  // initialize the screen
+  Robot.beginTFT();
+}
+
+void loop() {
+  // read the value of the potentiometer
+  int val=map(Robot.knobRead(), 0, 1023, -255, 255);
+
+  // print the value to the TFT screen
+  Robot.debugPrint(val);
+
+  // set the same speed on both of the robot's wheels
+  Robot.motorsWrite(val,val);
+  delay(10);
+}
diff --git a/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino b/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino
new file mode 100644
index 0000000..543c06c
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino
@@ -0,0 +1,32 @@
+/*
+ Turn Test
+ 
+ Check if the robot turns a certain amount of degrees.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+}
+
+void loop(){  
+  Robot.turn(50); //turn 50 degrees to the right
+  Robot.motorsStop();
+  delay(1000);
+
+  Robot.turn(-100); //turn 100 degrees to the left
+  Robot.motorsStop();
+  delay(1000);
+}
diff --git a/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino.orig b/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino.orig
new file mode 100644
index 0000000..4e3624f
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/TurnTest/TurnTest.ino.orig
@@ -0,0 +1,37 @@
+/*
+ Turn Test
+ 
+ Check if the robot turns a certain amount of degrees.
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the robot
+  Robot.begin();
+}
+
+<<<<<<< HEAD
+void loop() {  
+  Robot.turn(50);  //turn 50 degrees to the right
+=======
+void loop(){  
+  Robot.turn(50);//turn 50 degrees to the right
+  Robot.motorsStop();
+>>>>>>> f062f704463222e83390b4a954e211f0f7e6e66f
+  delay(1000);
+
+  Robot.turn(-100);//turn 100 degrees to the left
+  Robot.motorsStop();
+  delay(1000);
+}
diff --git a/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino b/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino
new file mode 100644
index 0000000..5bbc0e5
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino
@@ -0,0 +1,38 @@
+/*
+ Keyboard Test
+ 
+ Check how the robot's keyboard works. This example
+ sends the data about the key pressed through the
+ serial port.
+
+ All the buttons on the Control Board are tied up to a
+ single analog input pin, in this way it is possible to multiplex a
+ whole series of buttons on one single pin.
+
+ It is possible to recalibrate the thresholds of the buttons using
+ the Robot.keyboardCalibrate() function, that takes a 5 ints long
+ array as parameter
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+void setup() {
+  // initialize the serial port
+  Serial.begin(9600);
+}
+
+void loop() {
+  // print out the keyboard readings
+  Serial.println(Robot.keyboardRead());
+  delay(100);
+}
diff --git a/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino.orig b/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino.orig
new file mode 100644
index 0000000..6ee6c05
--- /dev/null
+++ b/libraries/Robot_Control/examples/learn/keyboardTest/keyboardTest.ino.orig
@@ -0,0 +1,49 @@
+/*
+ Keyboard Test
+ 
+ Check how the robot's keyboard works. This example
+ sends the data about the key pressed through the
+ serial port.
+
+ All the buttons on the Control Board are tied up to a
+ single analog input pin, in this way it is possible to multiplex a
+ whole series of buttons on one single pin.
+
+ It is possible to recalibrate the thresholds of the buttons using
+ the Robot.keyboardCalibrate() function, that takes a 5 ints long
+ array as parameter
+ 
+ Circuit:
+ * Arduino Robot
+ 
+ created 1 May 2013
+ by X. Yang
+ modified 12 May 2013
+ by D. Cuartielles
+ 
+ This example is in the public domain
+ */
+
+#include <ArduinoRobot.h>
+
+<<<<<<< HEAD
+// it is possible to use an array to calibrate
+//int vals[] = { 0, 133, 305, 481, 724 };
+
+void setup() {
+  // initialize the serial port
+  Serial.begin(9600);
+
+  // calibrate the keyboard
+  //Robot.keyboardCalibrate(vals);//For the new robot only.
+=======
+void setup(){
+  Serial.begin(9600);
+>>>>>>> f062f704463222e83390b4a954e211f0f7e6e66f
+}
+
+void loop() {
+  // print out the keyboard readings
+  Serial.println(Robot.keyboardRead());
+  delay(100);
+}
diff --git a/libraries/Robot_Control/glcdfont.c b/libraries/Robot_Control/glcdfont.c
new file mode 100644
index 0000000..abc3631
--- /dev/null
+++ b/libraries/Robot_Control/glcdfont.c
@@ -0,0 +1,266 @@
+#include <avr/io.h>
+#include <avr/pgmspace.h> 
+ 
+#ifndef FONT5X7_H
+#define FONT5X7_H
+
+// standard ascii 5x7 font
+
+static unsigned char  font[] PROGMEM = {
+        0x00, 0x00, 0x00, 0x00, 0x00,   
+	0x3E, 0x5B, 0x4F, 0x5B, 0x3E, 	
+	0x3E, 0x6B, 0x4F, 0x6B, 0x3E, 	
+	0x1C, 0x3E, 0x7C, 0x3E, 0x1C, 
+	0x18, 0x3C, 0x7E, 0x3C, 0x18, 
+	0x1C, 0x57, 0x7D, 0x57, 0x1C, 
+	0x1C, 0x5E, 0x7F, 0x5E, 0x1C, 
+	0x00, 0x18, 0x3C, 0x18, 0x00, 
+	0xFF, 0xE7, 0xC3, 0xE7, 0xFF, 
+	0x00, 0x18, 0x24, 0x18, 0x00, 
+	0xFF, 0xE7, 0xDB, 0xE7, 0xFF, 
+	0x30, 0x48, 0x3A, 0x06, 0x0E, 
+	0x26, 0x29, 0x79, 0x29, 0x26, 
+	0x40, 0x7F, 0x05, 0x05, 0x07, 
+	0x40, 0x7F, 0x05, 0x25, 0x3F, 
+	0x5A, 0x3C, 0xE7, 0x3C, 0x5A, 
+	0x7F, 0x3E, 0x1C, 0x1C, 0x08, 
+	0x08, 0x1C, 0x1C, 0x3E, 0x7F, 
+	0x14, 0x22, 0x7F, 0x22, 0x14, 
+	0x5F, 0x5F, 0x00, 0x5F, 0x5F, 
+	0x06, 0x09, 0x7F, 0x01, 0x7F, 
+	0x00, 0x66, 0x89, 0x95, 0x6A, 
+	0x60, 0x60, 0x60, 0x60, 0x60, 
+	0x94, 0xA2, 0xFF, 0xA2, 0x94, 
+	0x08, 0x04, 0x7E, 0x04, 0x08, 
+	0x10, 0x20, 0x7E, 0x20, 0x10, 
+	0x08, 0x08, 0x2A, 0x1C, 0x08, 
+	0x08, 0x1C, 0x2A, 0x08, 0x08, 
+	0x1E, 0x10, 0x10, 0x10, 0x10, 
+	0x0C, 0x1E, 0x0C, 0x1E, 0x0C, 
+	0x30, 0x38, 0x3E, 0x38, 0x30, 
+	0x06, 0x0E, 0x3E, 0x0E, 0x06, 
+	0x00, 0x00, 0x00, 0x00, 0x00, 
+	0x00, 0x00, 0x5F, 0x00, 0x00, 
+	0x00, 0x07, 0x00, 0x07, 0x00, 
+	0x14, 0x7F, 0x14, 0x7F, 0x14, 
+	0x24, 0x2A, 0x7F, 0x2A, 0x12, 
+	0x23, 0x13, 0x08, 0x64, 0x62, 
+	0x36, 0x49, 0x56, 0x20, 0x50, 
+	0x00, 0x08, 0x07, 0x03, 0x00, 
+	0x00, 0x1C, 0x22, 0x41, 0x00, 
+	0x00, 0x41, 0x22, 0x1C, 0x00, 
+	0x2A, 0x1C, 0x7F, 0x1C, 0x2A, 
+	0x08, 0x08, 0x3E, 0x08, 0x08, 
+	0x00, 0x80, 0x70, 0x30, 0x00, 
+	0x08, 0x08, 0x08, 0x08, 0x08, 
+	0x00, 0x00, 0x60, 0x60, 0x00, 
+	0x20, 0x10, 0x08, 0x04, 0x02, 
+	0x3E, 0x51, 0x49, 0x45, 0x3E, 
+	0x00, 0x42, 0x7F, 0x40, 0x00, 
+	0x72, 0x49, 0x49, 0x49, 0x46, 
+	0x21, 0x41, 0x49, 0x4D, 0x33, 
+	0x18, 0x14, 0x12, 0x7F, 0x10, 
+	0x27, 0x45, 0x45, 0x45, 0x39, 
+	0x3C, 0x4A, 0x49, 0x49, 0x31, 
+	0x41, 0x21, 0x11, 0x09, 0x07, 
+	0x36, 0x49, 0x49, 0x49, 0x36, 
+	0x46, 0x49, 0x49, 0x29, 0x1E, 
+	0x00, 0x00, 0x14, 0x00, 0x00, 
+	0x00, 0x40, 0x34, 0x00, 0x00, 
+	0x00, 0x08, 0x14, 0x22, 0x41, 
+	0x14, 0x14, 0x14, 0x14, 0x14, 
+	0x00, 0x41, 0x22, 0x14, 0x08, 
+	0x02, 0x01, 0x59, 0x09, 0x06, 
+	0x3E, 0x41, 0x5D, 0x59, 0x4E, 
+	0x7C, 0x12, 0x11, 0x12, 0x7C, 
+	0x7F, 0x49, 0x49, 0x49, 0x36, 
+	0x3E, 0x41, 0x41, 0x41, 0x22, 
+	0x7F, 0x41, 0x41, 0x41, 0x3E, 
+	0x7F, 0x49, 0x49, 0x49, 0x41, 
+	0x7F, 0x09, 0x09, 0x09, 0x01, 
+	0x3E, 0x41, 0x41, 0x51, 0x73, 
+	0x7F, 0x08, 0x08, 0x08, 0x7F, 
+	0x00, 0x41, 0x7F, 0x41, 0x00, 
+	0x20, 0x40, 0x41, 0x3F, 0x01, 
+	0x7F, 0x08, 0x14, 0x22, 0x41, 
+	0x7F, 0x40, 0x40, 0x40, 0x40, 
+	0x7F, 0x02, 0x1C, 0x02, 0x7F, 
+	0x7F, 0x04, 0x08, 0x10, 0x7F, 
+	0x3E, 0x41, 0x41, 0x41, 0x3E, 
+	0x7F, 0x09, 0x09, 0x09, 0x06, 
+	0x3E, 0x41, 0x51, 0x21, 0x5E, 
+	0x7F, 0x09, 0x19, 0x29, 0x46, 
+	0x26, 0x49, 0x49, 0x49, 0x32, 
+	0x03, 0x01, 0x7F, 0x01, 0x03, 
+	0x3F, 0x40, 0x40, 0x40, 0x3F, 
+	0x1F, 0x20, 0x40, 0x20, 0x1F, 
+	0x3F, 0x40, 0x38, 0x40, 0x3F, 
+	0x63, 0x14, 0x08, 0x14, 0x63, 
+	0x03, 0x04, 0x78, 0x04, 0x03, 
+	0x61, 0x59, 0x49, 0x4D, 0x43, 
+	0x00, 0x7F, 0x41, 0x41, 0x41, 
+	0x02, 0x04, 0x08, 0x10, 0x20, 
+	0x00, 0x41, 0x41, 0x41, 0x7F, 
+	0x04, 0x02, 0x01, 0x02, 0x04, 
+	0x40, 0x40, 0x40, 0x40, 0x40, 
+	0x00, 0x03, 0x07, 0x08, 0x00, 
+	0x20, 0x54, 0x54, 0x78, 0x40, 
+	0x7F, 0x28, 0x44, 0x44, 0x38, 
+	0x38, 0x44, 0x44, 0x44, 0x28, 
+	0x38, 0x44, 0x44, 0x28, 0x7F, 
+	0x38, 0x54, 0x54, 0x54, 0x18, 
+	0x00, 0x08, 0x7E, 0x09, 0x02, 
+	0x18, 0xA4, 0xA4, 0x9C, 0x78, 
+	0x7F, 0x08, 0x04, 0x04, 0x78, 
+	0x00, 0x44, 0x7D, 0x40, 0x00, 
+	0x20, 0x40, 0x40, 0x3D, 0x00, 
+	0x7F, 0x10, 0x28, 0x44, 0x00, 
+	0x00, 0x41, 0x7F, 0x40, 0x00, 
+	0x7C, 0x04, 0x78, 0x04, 0x78, 
+	0x7C, 0x08, 0x04, 0x04, 0x78, 
+	0x38, 0x44, 0x44, 0x44, 0x38, 
+	0xFC, 0x18, 0x24, 0x24, 0x18, 
+	0x18, 0x24, 0x24, 0x18, 0xFC, 
+	0x7C, 0x08, 0x04, 0x04, 0x08, 
+	0x48, 0x54, 0x54, 0x54, 0x24, 
+	0x04, 0x04, 0x3F, 0x44, 0x24, 
+	0x3C, 0x40, 0x40, 0x20, 0x7C, 
+	0x1C, 0x20, 0x40, 0x20, 0x1C, 
+	0x3C, 0x40, 0x30, 0x40, 0x3C, 
+	0x44, 0x28, 0x10, 0x28, 0x44, 
+	0x4C, 0x90, 0x90, 0x90, 0x7C, 
+	0x44, 0x64, 0x54, 0x4C, 0x44, 
+	0x00, 0x08, 0x36, 0x41, 0x00, 
+	0x00, 0x00, 0x77, 0x00, 0x00, 
+	0x00, 0x41, 0x36, 0x08, 0x00, 
+	0x02, 0x01, 0x02, 0x04, 0x02, 
+	0x3C, 0x26, 0x23, 0x26, 0x3C, 
+	0x1E, 0xA1, 0xA1, 0x61, 0x12, 
+	0x3A, 0x40, 0x40, 0x20, 0x7A, 
+	0x38, 0x54, 0x54, 0x55, 0x59, 
+	0x21, 0x55, 0x55, 0x79, 0x41, 
+	0x21, 0x54, 0x54, 0x78, 0x41, 
+	0x21, 0x55, 0x54, 0x78, 0x40, 
+	0x20, 0x54, 0x55, 0x79, 0x40, 
+	0x0C, 0x1E, 0x52, 0x72, 0x12, 
+	0x39, 0x55, 0x55, 0x55, 0x59, 
+	0x39, 0x54, 0x54, 0x54, 0x59, 
+	0x39, 0x55, 0x54, 0x54, 0x58, 
+	0x00, 0x00, 0x45, 0x7C, 0x41, 
+	0x00, 0x02, 0x45, 0x7D, 0x42, 
+	0x00, 0x01, 0x45, 0x7C, 0x40, 
+	0xF0, 0x29, 0x24, 0x29, 0xF0, 
+	0xF0, 0x28, 0x25, 0x28, 0xF0, 
+	0x7C, 0x54, 0x55, 0x45, 0x00, 
+	0x20, 0x54, 0x54, 0x7C, 0x54, 
+	0x7C, 0x0A, 0x09, 0x7F, 0x49, 
+	0x32, 0x49, 0x49, 0x49, 0x32, 
+	0x32, 0x48, 0x48, 0x48, 0x32, 
+	0x32, 0x4A, 0x48, 0x48, 0x30, 
+	0x3A, 0x41, 0x41, 0x21, 0x7A, 
+	0x3A, 0x42, 0x40, 0x20, 0x78, 
+	0x00, 0x9D, 0xA0, 0xA0, 0x7D, 
+	0x39, 0x44, 0x44, 0x44, 0x39, 
+	0x3D, 0x40, 0x40, 0x40, 0x3D, 
+	0x3C, 0x24, 0xFF, 0x24, 0x24, 
+	0x48, 0x7E, 0x49, 0x43, 0x66, 
+	0x2B, 0x2F, 0xFC, 0x2F, 0x2B, 
+	0xFF, 0x09, 0x29, 0xF6, 0x20, 
+	0xC0, 0x88, 0x7E, 0x09, 0x03, 
+	0x20, 0x54, 0x54, 0x79, 0x41, 
+	0x00, 0x00, 0x44, 0x7D, 0x41, 
+	0x30, 0x48, 0x48, 0x4A, 0x32, 
+	0x38, 0x40, 0x40, 0x22, 0x7A, 
+	0x00, 0x7A, 0x0A, 0x0A, 0x72, 
+	0x7D, 0x0D, 0x19, 0x31, 0x7D, 
+	0x26, 0x29, 0x29, 0x2F, 0x28, 
+	0x26, 0x29, 0x29, 0x29, 0x26, 
+	0x30, 0x48, 0x4D, 0x40, 0x20, 
+	0x38, 0x08, 0x08, 0x08, 0x08, 
+	0x08, 0x08, 0x08, 0x08, 0x38, 
+	0x2F, 0x10, 0xC8, 0xAC, 0xBA, 
+	0x2F, 0x10, 0x28, 0x34, 0xFA, 
+	0x00, 0x00, 0x7B, 0x00, 0x00, 
+	0x08, 0x14, 0x2A, 0x14, 0x22, 
+	0x22, 0x14, 0x2A, 0x14, 0x08, 
+	0xAA, 0x00, 0x55, 0x00, 0xAA, 
+	0xAA, 0x55, 0xAA, 0x55, 0xAA, 
+	0x00, 0x00, 0x00, 0xFF, 0x00, 
+	0x10, 0x10, 0x10, 0xFF, 0x00, 
+	0x14, 0x14, 0x14, 0xFF, 0x00, 
+	0x10, 0x10, 0xFF, 0x00, 0xFF, 
+	0x10, 0x10, 0xF0, 0x10, 0xF0, 
+	0x14, 0x14, 0x14, 0xFC, 0x00, 
+	0x14, 0x14, 0xF7, 0x00, 0xFF, 
+	0x00, 0x00, 0xFF, 0x00, 0xFF, 
+	0x14, 0x14, 0xF4, 0x04, 0xFC, 
+	0x14, 0x14, 0x17, 0x10, 0x1F, 
+	0x10, 0x10, 0x1F, 0x10, 0x1F, 
+	0x14, 0x14, 0x14, 0x1F, 0x00, 
+	0x10, 0x10, 0x10, 0xF0, 0x00, 
+	0x00, 0x00, 0x00, 0x1F, 0x10, 
+	0x10, 0x10, 0x10, 0x1F, 0x10, 
+	0x10, 0x10, 0x10, 0xF0, 0x10, 
+	0x00, 0x00, 0x00, 0xFF, 0x10, 
+	0x10, 0x10, 0x10, 0x10, 0x10, 
+	0x10, 0x10, 0x10, 0xFF, 0x10, 
+	0x00, 0x00, 0x00, 0xFF, 0x14, 
+	0x00, 0x00, 0xFF, 0x00, 0xFF, 
+	0x00, 0x00, 0x1F, 0x10, 0x17, 
+	0x00, 0x00, 0xFC, 0x04, 0xF4, 
+	0x14, 0x14, 0x17, 0x10, 0x17, 
+	0x14, 0x14, 0xF4, 0x04, 0xF4, 
+	0x00, 0x00, 0xFF, 0x00, 0xF7, 
+	0x14, 0x14, 0x14, 0x14, 0x14, 
+	0x14, 0x14, 0xF7, 0x00, 0xF7, 
+	0x14, 0x14, 0x14, 0x17, 0x14, 
+	0x10, 0x10, 0x1F, 0x10, 0x1F, 
+	0x14, 0x14, 0x14, 0xF4, 0x14, 
+	0x10, 0x10, 0xF0, 0x10, 0xF0, 
+	0x00, 0x00, 0x1F, 0x10, 0x1F, 
+	0x00, 0x00, 0x00, 0x1F, 0x14, 
+	0x00, 0x00, 0x00, 0xFC, 0x14, 
+	0x00, 0x00, 0xF0, 0x10, 0xF0, 
+	0x10, 0x10, 0xFF, 0x10, 0xFF, 
+	0x14, 0x14, 0x14, 0xFF, 0x14, 
+	0x10, 0x10, 0x10, 0x1F, 0x00, 
+	0x00, 0x00, 0x00, 0xF0, 0x10, 
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+	0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 
+	0xFF, 0xFF, 0xFF, 0x00, 0x00, 
+	0x00, 0x00, 0x00, 0xFF, 0xFF, 
+	0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 
+	0x38, 0x44, 0x44, 0x38, 0x44, 
+	0x7C, 0x2A, 0x2A, 0x3E, 0x14, 
+	0x7E, 0x02, 0x02, 0x06, 0x06, 
+	0x02, 0x7E, 0x02, 0x7E, 0x02, 
+	0x63, 0x55, 0x49, 0x41, 0x63, 
+	0x38, 0x44, 0x44, 0x3C, 0x04, 
+	0x40, 0x7E, 0x20, 0x1E, 0x20, 
+	0x06, 0x02, 0x7E, 0x02, 0x02, 
+	0x99, 0xA5, 0xE7, 0xA5, 0x99, 
+	0x1C, 0x2A, 0x49, 0x2A, 0x1C, 
+	0x4C, 0x72, 0x01, 0x72, 0x4C, 
+	0x30, 0x4A, 0x4D, 0x4D, 0x30, 
+	0x30, 0x48, 0x78, 0x48, 0x30, 
+	0xBC, 0x62, 0x5A, 0x46, 0x3D, 
+	0x3E, 0x49, 0x49, 0x49, 0x00, 
+	0x7E, 0x01, 0x01, 0x01, 0x7E, 
+	0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 
+	0x44, 0x44, 0x5F, 0x44, 0x44, 
+	0x40, 0x51, 0x4A, 0x44, 0x40, 
+	0x40, 0x44, 0x4A, 0x51, 0x40, 
+	0x00, 0x00, 0xFF, 0x01, 0x03, 
+	0xE0, 0x80, 0xFF, 0x00, 0x00, 
+	0x08, 0x08, 0x6B, 0x6B, 0x08,
+	0x36, 0x12, 0x36, 0x24, 0x36, 
+	0x06, 0x0F, 0x09, 0x0F, 0x06, 
+	0x00, 0x00, 0x18, 0x18, 0x00, 
+	0x00, 0x00, 0x10, 0x10, 0x00, 
+	0x30, 0x40, 0xFF, 0x01, 0x01, 
+	0x00, 0x1F, 0x01, 0x01, 0x1E, 
+	0x00, 0x19, 0x1D, 0x17, 0x12, 
+	0x00, 0x3C, 0x3C, 0x3C, 0x3C, 
+	0x00, 0x00, 0x00, 0x00, 0x00, 
+};
+#endif
diff --git a/libraries/Robot_Control/helper.cpp b/libraries/Robot_Control/helper.cpp
new file mode 100644
index 0000000..a7a956a
--- /dev/null
+++ b/libraries/Robot_Control/helper.cpp
@@ -0,0 +1,45 @@
+#include "ArduinoRobot.h"
+
+void RobotControl::drawBase(){
+	Arduino_LCD::drawCircle(64,80,50,foreGround);
+	Arduino_LCD::drawLine(64,30,64,20,foreGround);
+}
+void RobotControl::drawDire(int16_t dire){
+	static uint8_t x_old;
+	static uint8_t y_old;
+	static uint8_t x_t_old;
+	static uint8_t y_t_old;
+
+	uint8_t x=60*sin(dire/360.0*6.28)+64;
+	uint8_t x_t=40*sin(dire/360.0*6.28)+64;
+	uint8_t y=60*cos(dire/360.0*6.28)+80;
+	uint8_t y_t=40*cos(dire/360.0*6.28)+80;
+
+	Arduino_LCD::drawLine(x_t_old,y_t_old,x_old,y_old,backGround);
+	Arduino_LCD::drawLine(x_t,y_t,x,y,RED);
+
+	x_old=x;
+	y_old=y;
+	x_t_old=x_t;
+	y_t_old=y_t;
+}
+
+void RobotControl::drawCompass(uint16_t value){
+	drawBase();
+	drawDire(value);
+	debugPrint(value,57,76);
+}
+
+//display logos 
+void RobotControl::displayLogos(){
+	_drawBMP("lg0.bmp",0,0);
+	delay(2000);
+	_drawBMP("lg1.bmp",0,0);
+	delay(2000);
+	clearScreen();
+}
+
+//wait for a button to be pressed
+void RobotControl::waitContinue(uint8_t key){
+  while(!(Robot.keyboardRead()==key));
+}
diff --git a/libraries/Robot_Control/information.cpp b/libraries/Robot_Control/information.cpp
new file mode 100644
index 0000000..c36e9ce
--- /dev/null
+++ b/libraries/Robot_Control/information.cpp
@@ -0,0 +1,41 @@
+/*#include <ArduinoRobot.h>
+//0 - 319: pic array, 
+
+//320 - 337 username, 
+#define ADDRESS_USERNAME 320
+//338 - 355 robotname, 
+#define ADDRESS_ROBOTNAME 338
+//356 - 373 cityname, 
+#define ADDRESS_CITYNAME 356
+ //374- 391 countryname, 
+#define ADDRESS_COUNTRYNAME 374
+//508-511 robot info
+#define ADDRESS_ROBOTINFO 508
+
+
+void RobotControl::getMyName(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_USERNAME,(uint8_t*)container,18);
+}
+void RobotControl::getRobotName(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_ROBOTNAME,(uint8_t*)container,18);
+}
+void RobotControl::getMyCity(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_CITYNAME,(uint8_t*)container,18);
+}
+void RobotControl::getMyCountry(char* container){
+	EEPROM_I2C::readBuffer(ADDRESS_COUNTRYNAME,(uint8_t*)container,18);
+}
+
+void RobotControl::setMyName(char* text){
+	EEPROM_I2C::writePage(ADDRESS_USERNAME,(uint8_t*)text,18);
+}
+void RobotControl::setRobotName(char* text){
+	EEPROM_I2C::writePage(ADDRESS_ROBOTNAME,(uint8_t*)text,18);
+}
+void RobotControl::setMyCity(char* text){
+	EEPROM_I2C::writePage(ADDRESS_CITYNAME,(uint8_t*)text,18);
+}
+void RobotControl::setMyCountry(char* text){
+	EEPROM_I2C::writePage(ADDRESS_COUNTRYNAME,(uint8_t*)text,18);
+}
+*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/keyboard.cpp b/libraries/Robot_Control/keyboard.cpp
new file mode 100644
index 0000000..7e647bb
--- /dev/null
+++ b/libraries/Robot_Control/keyboard.cpp
@@ -0,0 +1,65 @@
+#include "ArduinoRobot.h"
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+int pul_min[]={0,133,319,494,732};
+int pul_max[]={10,153,339,514,752};
+/*int pul_min[]={0,123,295,471,714};
+int pul_max[]={0,143,315,491,734};*/
+/*
+int pul_min[]={0,133,319,494,732};
+int pul_max[]={10,153,339,514,752};
+*/
+void sort(int* v);
+
+void RobotControl::keyboardCalibrate(int *vals){
+	for(int i=0;i<5;i++){
+		pul_min[i]=vals[i]-10;
+		pul_max[i]=vals[i]+10;
+	}
+}
+int8_t RobotControl::keyboardRead(void)
+{
+
+  int lectura_pul;
+  int8_t conta_pul=0;
+  static int anterior=0;
+
+  lectura_pul = this->averageAnalogInput(KEY);
+
+  while ((conta_pul < NUMBER_BUTTONS) && !(lectura_pul >= pul_min[conta_pul] && lectura_pul <= pul_max[conta_pul]))
+    conta_pul++;
+
+  if (conta_pul >= NUMBER_BUTTONS)
+    conta_pul = -1;
+  else
+    delay(100);
+
+  return conta_pul;
+}
+
+int RobotControl::averageAnalogInput(int pinNum)
+{
+  int vals[5];
+  for(int i=0;i<5;i++){
+	for(int j=i;j<5;j++){
+	  vals[j]=::analogRead(pinNum);
+	}
+	sort(vals);
+  }
+  return vals[0];
+}
+void sort(int* v){
+  int tmp;
+  for(int i=0;i<4;i++)
+    for(int j=i+1;j<5;j++)
+      if(v[j]<v[i]){
+        tmp=v[j];
+        v[j]=v[i];
+        v[i]=tmp;
+      }
+  v[0]=v[3];
+}
\ No newline at end of file
diff --git a/libraries/Robot_Control/lcd.cpp b/libraries/Robot_Control/lcd.cpp
new file mode 100644
index 0000000..1f9f2ce
--- /dev/null
+++ b/libraries/Robot_Control/lcd.cpp
@@ -0,0 +1,279 @@
+#include "ArduinoRobot.h"
+#include "Wire.h"
+
+#define BUFFPIXEL 20
+
+bool cmp(char* str1, char* str2, uint8_t len);
+uint16_t read16(Fat16& f);
+uint32_t read32(Fat16& f);
+//uint16_t color565(uint8_t r, uint8_t g, uint8_t b);
+
+void RobotControl::beginTFT(uint16_t foreGround, uint16_t backGround){
+	//TFT initialization
+	Arduino_LCD::initB();
+	Arduino_LCD::fillScreen(backGround);
+	Arduino_LCD::setTextColor(foreGround);
+	Arduino_LCD::setTextSize(1);
+	this->foreGround=foreGround;
+	this->backGround=backGround;
+}
+void RobotControl::_enableLCD(){
+	DDRB = DDRB & 0xEF; //pinMode(CS_SD,INPUT);
+	DDRB = DDRB | 0x20; //pinMode(CS_LCD,OUTPUT);
+}
+/*void RobotControl::_setErase(uint8_t posX, uint8_t posY){
+	Arduino_LCD::setCursor(posX,posY);
+	Arduino_LCD::setTextColor(backGround);
+	Arduino_LCD::setTextSize(1);
+}
+void RobotControl::_setWrite(uint8_t posX, uint8_t posY){
+	Arduino_LCD::setCursor(posX,posY);
+	Arduino_LCD::setTextColor(foreGround);
+	Arduino_LCD::setTextSize(1);
+}*/
+/*
+void RobotControl::text(int value, uint8_t posX, uint8_t posY, bool EW){
+	if(EW)
+		_setWrite(posX,posY);
+	else 
+		_setErase(posX,posY);
+	Arduino_LCD::print(value);
+}
+void RobotControl::text(long value, uint8_t posX, uint8_t posY, bool EW){
+	if(EW)
+		_setWrite(posX,posY);
+	else 
+		_setErase(posX,posY);
+	Arduino_LCD::print(value);
+}
+void RobotControl::text(char* value, uint8_t posX, uint8_t posY, bool EW){
+	if(EW)
+		_setWrite(posX,posY);
+	else 
+		_setErase(posX,posY);
+	Arduino_LCD::print(value);
+}
+void RobotControl::text(char value, uint8_t posX, uint8_t posY, bool EW){
+	if(EW)
+		_setWrite(posX,posY);
+	else 
+		_setErase(posX,posY);
+	Arduino_LCD::print(value);
+}
+*/
+
+void RobotControl::debugPrint(long value, uint8_t x, uint8_t y){
+	static long oldVal=0;
+	Arduino_LCD::stroke(backGround);
+	text(oldVal,x,y);
+	Arduino_LCD::stroke(foreGround);
+	text(value,x,y);
+	oldVal=value;
+}
+
+void RobotControl::clearScreen(){
+	Arduino_LCD::fillScreen(backGround);
+}
+
+void RobotControl::drawBMP(char* filename, uint8_t x, uint8_t y){
+	/*for(int j=0;j<NUM_EEPROM_BMP;j++){
+		Serial.println(_eeprom_bmp[j].name);
+		Serial.print(" ");
+		Serial.print(_eeprom_bmp[j].address);
+		Serial.print(" ");
+		Serial.print(_eeprom_bmp[j].width);
+		Serial.print(" ");
+		Serial.println(_eeprom_bmp[j].height);
+	}
+	Serial.println();*/
+	if(_isEEPROM_BMP_Allocated){
+		for(int i=0;i<NUM_EEPROM_BMP;i++){
+			if(cmp(_eeprom_bmp[i].name,filename,7)){
+				/*Serial.println(_eeprom_bmp[i].name);
+				Serial.print(" ");
+				Serial.print(_eeprom_bmp[i].address);
+				Serial.print(" ");
+				Serial.print(_eeprom_bmp[i].width);
+				Serial.print(" ");
+				Serial.println(_eeprom_bmp[i].height);*/
+				_drawBMP(_eeprom_bmp[i].address,x,y,_eeprom_bmp[i].width,_eeprom_bmp[i].height);
+				return;
+			}
+		}
+	}else{
+		_drawBMP(filename,x,y);//goes to SD
+	}
+}
+bool cmp(char* str1, char* str2, uint8_t len){
+  for(uint8_t i=0;i<len;i++){
+	if(str1[i]==' ')break;
+    if(str1[i]!=str2[i])return false;
+  }
+  return true;
+}
+
+void RobotControl::_drawBMP(uint32_t iconOffset, uint8_t x, uint8_t y, uint8_t width, uint8_t height){
+	uint8_t screenWidth=Arduino_LCD::width();
+	uint8_t screenHeight=Arduino_LCD::height();
+	if((x >= screenWidth) || (y >= screenHeight)) return;
+
+	// Crop area to be loaded
+	if((x+width-1) >= screenWidth)  width = screenWidth  - x;
+	if((y+height-1) >= screenHeight) height = screenHeight - y;
+
+	// Set TFT address window to clipped image bounds
+	Arduino_LCD::setAddrWindow(x, y, x+width-1, y+height-1);
+
+	// launch the reading command
+	_drawBMP_EEPROM(iconOffset, width, height);
+}
+
+//  Draw BMP from SD card through the filename
+void RobotControl::_drawBMP(char* filename, uint8_t posX, uint8_t posY){
+	uint8_t  bmpWidth, bmpHeight;   // W+H in pixels
+	uint8_t  bmpDepth;              // Bit depth (currently must be 24)
+	uint32_t bmpImageoffset;        // Start of image data in file
+	uint32_t rowSize;               // Not always = bmpWidth; may have padding
+	uint8_t  sdbuffer[3*BUFFPIXEL]; // pixel buffer (R+G+B per pixel)
+	uint8_t  buffidx = sizeof(sdbuffer); // Current position in sdbuffer
+	bool  goodBmp = false;       // Set to true on valid header parse
+	bool  flip    = true;        // BMP is stored bottom-to-top
+	uint8_t  w, h, row, col;
+	uint8_t  r, g, b;
+	uint32_t pos = 0;
+
+	// Open requested file on SD card
+	if ((file.open(filename,O_READ)) == NULL) {
+		return;
+	}
+
+	// Parse BMP header
+	if(read16(file) == 0x4D42) { // BMP signature
+		read32(file);//uint32_t aux = read32(file);
+		(void)read32(file); // Read & ignore creator bytes
+		bmpImageoffset = read32(file); // Start of image data
+		
+		// Read DIB header
+		(void)read32(file);//aux = read32(file);
+		bmpWidth  = read32(file);
+		bmpHeight = read32(file);
+		
+		if(read16(file) == 1) { // # planes -- must be '1'
+			bmpDepth = read16(file); // bits per pixel
+			if((bmpDepth == 24) && (read32(file) == 0)) { // 0 = uncompressed
+				goodBmp = true; // Supported BMP format -- proceed!
+
+				// BMP rows are padded (if needed) to 4-byte boundary
+				rowSize = (bmpWidth * 3 + 3) & ~3;
+
+				// If bmpHeight is negative, image is in top-down order.
+				// This is not canon but has been observed in the wild.
+				if(bmpHeight < 0) {
+					bmpHeight = -bmpHeight;
+					flip      = false;
+				}
+
+				// Crop area to be loaded
+				w = bmpWidth;
+				h = bmpHeight;
+
+				//  Start drawing
+				//_enableLCD();
+				Arduino_LCD::setAddrWindow(posX, posY, posX+bmpWidth-1, posY+bmpHeight-1);  
+
+				for (row=0; row<h; row++) { // For each scanline...
+					if(flip) // Bitmap is stored bottom-to-top order (normal BMP)
+						pos = bmpImageoffset + (bmpHeight - 1 - row) * rowSize;
+					else     // Bitmap is stored top-to-bottom
+						pos = bmpImageoffset + row * rowSize;
+						
+					if(file.curPosition() != pos) { // Need seek?
+						//_enableSD();
+						file.seekSet(pos);
+						buffidx = sizeof(sdbuffer); // Force buffer reload
+						//_enableLCD();
+					}
+					for (col=0; col<w; col++) { // For each pixel...
+						// Time to read more pixel data?
+						if (buffidx >= sizeof(sdbuffer)) { // Indeed
+							//_enableSD();
+							file.read(sdbuffer, sizeof(sdbuffer));
+							buffidx = 0; // Set index to beginning
+							//_enableLCD();
+						}
+						// Convert pixel from BMP to TFT format, push to display
+						b = sdbuffer[buffidx++];
+						g = sdbuffer[buffidx++];
+						r = sdbuffer[buffidx++];
+
+						int  color = Arduino_LCD::Color565(r,g,b);
+						
+						Arduino_LCD::pushColor(color);
+					} // end pixel
+				} // end scanline
+				//_enableSD();
+			} // end goodBmp*/
+		}
+	}
+	file.close();
+	//_enableLCD();
+}
+uint16_t read16(Fat16& f) {
+  uint16_t result;
+  f.read(&result,sizeof(result));
+  return result;
+}
+uint32_t read32(Fat16& f) {
+  uint32_t result;
+  f.read(&result,sizeof(result));
+  return result;
+}
+/*
+uint16_t color565(uint8_t r, uint8_t g, uint8_t b) {
+  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+}*/
+
+
+void RobotControl::_drawBMP_EEPROM(uint16_t address, uint8_t width, uint8_t height){
+	uint16_t u16retVal = 0;
+	EEPROM_I2C::_beginTransmission(address);
+	EEPROM_I2C::_endTransmission();
+	/*Wire.beginTransmission(DEVICEADDRESS);
+	Wire.write( (address >> 8) & 0xFF );
+	Wire.write( (address >> 0) & 0xFF );
+	Wire.endTransmission();*/
+
+	long s = width * height ;
+	for(long j = 0; j < (long) s >> 4; j++) { // divided by 32, times 2
+		Wire.requestFrom(DEVICEADDRESS, 32);
+		for(int i = 0; i < 32; i+=2) {
+			u16retVal = Wire.read();
+			u16retVal = (u16retVal << 8) + Wire.read();
+			Arduino_LCD::pushColor(u16retVal);
+		}
+	}
+
+}
+void RobotControl::beginBMPFromEEPROM(){
+	_eeprom_bmp=(EEPROM_BMP*)malloc(NUM_EEPROM_BMP*sizeof(EEPROM_BMP));
+	EEPROM_I2C::_beginTransmission(0);
+	EEPROM_I2C::_endTransmission();
+	
+	for(uint8_t j=0;j<NUM_EEPROM_BMP;j++){
+		Wire.requestFrom(DEVICEADDRESS, sizeof(EEPROM_BMP));
+		for(uint8_t i=0;i<8;i++){
+			_eeprom_bmp[j].name[i]=Wire.read();//name
+		}
+		_eeprom_bmp[j].width=Wire.read();//width
+		_eeprom_bmp[j].height=Wire.read();//height
+		
+		_eeprom_bmp[j].address=Wire.read();
+		_eeprom_bmp[j].address=_eeprom_bmp[j].address + (Wire.read() << 8);//address
+	}
+	_isEEPROM_BMP_Allocated=true;
+	
+}
+void RobotControl::endBMPFromEEPROM(){
+	free(_eeprom_bmp);
+	_isEEPROM_BMP_Allocated=false;
+}
diff --git a/libraries/Robot_Control/utility/RobotTextManager.cpp b/libraries/Robot_Control/utility/RobotTextManager.cpp
new file mode 100644
index 0000000..b516409
--- /dev/null
+++ b/libraries/Robot_Control/utility/RobotTextManager.cpp
@@ -0,0 +1,192 @@
+#include <avr/pgmspace.h>
+#include <ArduinoRobot.h>
+#include "VirtualKeyboard.h"
+#include "RobotTextManager.h"
+#include "scripts_Hello_User.h"
+
+const int TextManager::lineHeight=10;
+const int TextManager::charWidth=6;
+
+
+void TextManager::setMargin(int margin_left,int margin_top){
+	this->margin_left=margin_left;
+	this->margin_top=margin_top;
+}
+int TextManager::getLin(int lineNum){
+   return lineNum*lineHeight+margin_top;
+}
+
+int TextManager::getCol(int colNum){
+   return colNum*charWidth+margin_left;
+}
+
+void TextManager::writeText(int lineNum, int colNum, char* txt, bool onOff){
+  if(!onOff)
+	Robot.setTextColor(WHITE);
+
+  Robot.setCursor(getCol(colNum),getLin(lineNum));
+  Robot.print(txt);
+  
+  Robot.setTextColor(BLACK);
+}
+
+void TextManager::drawInput(bool onOff){
+  if(!onOff)
+    Robot.setTextColor(WHITE);
+
+  Robot.setCursor(getCol(inputCol),getLin(inputLin)+1);
+  Robot.print('_');
+  
+  Robot.setTextColor(BLACK);
+
+}
+
+void TextManager::mvInput(int dire){
+  drawInput(0);
+  if(dire<0){
+    if(inputPos>0){
+      inputPos--;
+      inputCol--;
+    }
+  }else{
+    if(inputPos<16){
+      inputPos++;
+      inputCol++;
+    }
+  }
+  drawInput(1);
+}
+
+char TextManager::selectLetter(){
+  static int oldVal;
+  char val=map(Robot.knobRead(),0,1023,32,125);
+  if(val==oldVal){
+    return 0;  //No changes
+  }else{
+    oldVal=val;
+    return val;  //Current letter
+  }
+}
+
+void TextManager::refreshCurrentLetter(char letter){
+  if(letter){
+	writeText(inputLin,inputCol,inputPool+inputPos,false);//erase
+	inputPool[inputPos]=letter;
+	writeText(inputLin,inputCol,inputPool+inputPos,true);//write
+  }
+}
+
+
+void TextManager::getInput(int lin, int col){
+  writeText(lin,col,">");  //Input indicator
+  
+  writeText(lin, col+1, inputPool);
+  
+  inputLin=lin;  //Ini input cursor
+  inputCol=col+1;
+  inputPos=0;
+  drawInput(true);
+  
+  Vkey.display(100);//Vkey is a object of VirtualKeyboard class
+  
+  while(true){
+    switch(Robot.keyboardRead()){
+      case BUTTON_LEFT:
+        //Robot.beep(BEEP_SIMPLE);
+        mvInput(-1);
+        break;
+      case BUTTON_RIGHT:
+        //Robot.beep(BEEP_SIMPLE);
+        mvInput(1);
+        break;
+      case BUTTON_MIDDLE:
+        //Robot.beep(BEEP_DOUBLE);
+        char selection=Vkey.getSelection();
+        if(selection!='\0'){
+          refreshCurrentLetter(selection);
+          mvInput(1);
+        }else{
+          drawInput(false);
+          return;
+        }
+    }
+    Vkey.run();
+    delay(10);
+  }
+}
+void TextManager::setInputPool(int code){
+  switch(code){
+    case USERNAME:
+      Robot.userNameRead(inputPool);
+      break;
+    case ROBOTNAME:
+      Robot.robotNameRead(inputPool);
+      break;
+    case CITYNAME:
+      Robot.cityNameRead(inputPool);
+      break;
+    case COUNTRYNAME:
+      Robot.countryNameRead(inputPool);
+      break;
+  }
+  for(int i=0;i<18;i++){
+    if(inputPool[i]=='\0'){
+      for(int j=i;j<18;j++){
+        inputPool[j]='\0';
+      }
+      break;
+    }
+  }
+}
+void TextManager::pushInput(int code){
+  switch(code){
+    case USERNAME:
+      Robot.userNameWrite(inputPool);
+      break;
+    case ROBOTNAME:
+      Robot.robotNameWrite(inputPool);
+      break;
+    case CITYNAME:
+      Robot.cityNameWrite(inputPool);
+      break;
+    case COUNTRYNAME:
+      Robot.countryNameWrite(inputPool);
+      break;
+  }
+  for(int i=0;i<18;i++){
+    inputPool[i]='\0';
+  }
+}
+void TextManager::input(int lin,int col, int code){
+  setInputPool(code);
+  getInput(lin,col);
+  pushInput(code);
+}
+
+void TextManager::showPicture(char * filename, int posX, int posY){
+  Robot.pause();
+  Robot._drawBMP(filename,posX,posY);
+  Robot.play();
+}
+
+void TextManager::getPGMtext(int seq){
+  //It takes a string from program space, and fill it  
+  //in the buffer
+  //if(in hello user example){
+  if(true){
+	strcpy_P(PGMbuffer,(char*)pgm_read_word(&(::scripts_Hello_User[seq])));
+  }
+}
+
+void TextManager::writeScript(int seq, int line, int col){
+  //print a string from program space to a specific line,
+  //column on the LCD
+  
+  //first fill the buffer with text from program space
+  getPGMtext(seq);
+  //then print it to the screen
+  textManager.writeText(line,col,PGMbuffer);
+}
+
+
+TextManager textManager=TextManager();
diff --git a/libraries/Robot_Control/utility/RobotTextManager.h b/libraries/Robot_Control/utility/RobotTextManager.h
new file mode 100644
index 0000000..6c0b7bd
--- /dev/null
+++ b/libraries/Robot_Control/utility/RobotTextManager.h
@@ -0,0 +1,77 @@
+#ifndef ROBOTTEXTMANAGER_H
+#define ROBOTTEXTMANAGER_H
+
+#define USERNAME  0
+#define ROBOTNAME  1
+#define CITYNAME  2
+#define COUNTRYNAME  3
+#define EMPTY 4
+
+class TextManager{
+  //The TextManager class is a collection of features specific for Hello 
+  //User example. 
+  //
+  //- It includes solution for setting text position based on
+  //  line/column. The original Robot.text(), or the more low level 
+  //  print() function can only set text position on pixels from left,
+  //  top. 
+  //
+  //- The process of accepting input with the virtual keyboard, saving
+  //  into or reading from EEPROM is delt with here.
+  //
+  //- A workflow for stop the music while displaying image. Trouble
+  //  will happen otherwise.
+  
+  public:
+	//add some margin to the text, left side only atm.
+	void setMargin(int margin_left,int margin_top);
+    
+    //print text based on line, column. 
+    void writeText(int lineNum, int colNum, char* txt, bool onOff=true);
+	
+	//print a script from the scripts library
+	void writeScript(int seq, int line, int col);
+
+	//The whole process of getting input
+	void input(int lin,int col, int code);
+    //Print a cursor and virtual keyboard on screen, and save the user's input
+    void getInput(int lin, int col);  
+    //Get user name, robot name, city name or country name from EEPROM
+    //and store in the input pool.
+    void setInputPool(int code);
+    //save user input to EEPROM
+    void pushInput(int code);
+	
+    //Replaces Robot.drawPicture(), as this one solves collision between
+    //image and music
+    void showPicture(char * filename, int posX, int posY);
+
+  private:
+    int margin_left,margin_top;
+	int getLin(int lineNum);  //Convert line to pixels from top
+    int getCol(int colNum);  //Convert line to pixels from left
+  
+    static const int lineHeight;//8+2=10
+    static const int charWidth;//5+1=6
+    
+    int inputPos;
+    int inputLin;
+    int inputCol;
+	
+    void drawInput(bool onOff);
+    void mvInput(int dire);
+
+    char selectLetter();
+    void refreshCurrentLetter(char letter);
+
+	void getPGMtext(int seq);
+	
+	char PGMbuffer[85];  //the buffer for storing strings
+    char inputPool[18];
+};
+
+//a trick for removing the need of creating an object of TextManager.
+//So you can call me.somefunction() directly in the sketch.
+extern TextManager textManager;
+
+#endif
diff --git a/libraries/Robot_Control/utility/VirtualKeyboard.cpp b/libraries/Robot_Control/utility/VirtualKeyboard.cpp
new file mode 100644
index 0000000..ad73c75
--- /dev/null
+++ b/libraries/Robot_Control/utility/VirtualKeyboard.cpp
@@ -0,0 +1,127 @@
+#include "VirtualKeyboard.h"
+
+int VirtualKeyboard::getColLin(int val){
+  uint8_t col,lin;
+  lin=val/10;
+  col=val%10; // saving 36 bytes :(
+  /*if(0<=val && 9>=val){
+    col=val;
+    lin=0;
+  }else if(10<=val && 19>=val){
+    col=val-10;
+    lin=1;
+  }else if(20<=val && 29>=val){
+    col=val-20;
+    lin=2;
+  }else if(30<=val && 39>=val){
+    col=val-30;
+    lin=3;
+  }*/
+  return (col<<8)+lin;	//Put col and lin in one int
+}
+void VirtualKeyboard::run(){
+/** visually select a letter on the keyboard
+*	The selection boarder is 1px higher than the character, 
+*	1px on the bottom, 2px to the left and 2px to the right.
+*
+*/
+  if(!onOff)return;
+  //Serial.println(onOff);
+  static int oldColLin=0;
+  uint8_t val=map(Robot.knobRead(),0,1023,0,38);
+  if(val==38)val=37;	//The last value is jumpy when using batteries
+  int colLin=getColLin(val);
+  
+  if(oldColLin!=colLin){
+    uint8_t x=(oldColLin>>8 & 0xFF)*11+10;//col*11+1+9
+    uint8_t y=(oldColLin & 0xFF)*11+1+top;//lin*11+1+top
+    uint8_t w=9;
+    if(oldColLin==1795)  //last item "Enter", col=7 lin=3
+      w=33; //(5+1)*6-1+2+2  charWidth=5, charMargin=1, count("Enter")=6, lastItem_MarginRight=0, marginLeft==marginRight=2
+    Robot.drawRect(x,y,w,9,hideColor);
+
+
+    x=(colLin>>8 & 0xFF)*11+10;
+    y=(colLin & 0xFF)*11+1+top;
+    w=9;
+    if(colLin==1795)  //last item "Enter", col=7 lin=3
+      w=33; //(5+1)*6-1+2+2  charWidth=5, charMargin=1, count("Enter")=6, lastItem_MarginRight=0, marginLeft==marginRight=2
+    Robot.drawRect(x,y,w,9,showColor);
+    oldColLin=colLin;
+  }
+}
+
+char VirtualKeyboard::getSelection(){
+  if(!onOff)return -1;
+
+  uint8_t val=map(Robot.knobRead(),0,1023,0,38);
+  if(0<=val && 9>=val)
+    val='0'+val;	
+  else if(10<=val && 35>=val)
+    val='A'+val-10;	
+  else if(val==36)
+    val=' ';
+  else if(val>=37)
+    val='\0';
+  
+  return val;
+}
+void VirtualKeyboard::hide(){
+	onOff=false;
+	Robot.fillRect(0,top,128,44,hideColor);//11*4
+}
+
+void VirtualKeyboard::display(uint8_t top, uint16_t showColor, uint16_t hideColor){
+/** Display the keyboard at y position of top
+*	formular:
+*	When text size is 1, one character is 5*7
+*	margin-left==margin-right==3,
+*	margin-top==margin-bottom==2,
+*	keyWidth=5+3+3==11,
+*	keyHeight=7+2+2==11,
+*	keyboard-margin-left=keyboard-margin-right==9
+*	so character-x=11*col+9+3=11*col+12
+*	character-y=11*lin+2+top
+*	
+**/
+  this->top=top;
+  this->onOff=true;
+  
+  this->showColor=showColor;
+  this->hideColor=hideColor;
+  
+  for(uint8_t i=0;i<36;i++){
+	Robot.setCursor(i%10*11+12,2+top+i/10*11);
+	if(i<10)
+		Robot.print(char('0'+i));
+	else
+		Robot.print(char(55+i));//'A'-10=55
+  }//for saving 58 bytes :(
+  
+  /*for(int i=0;i<10;i++){
+    Robot.setCursor(i*11+12,2+top);//11*0+2+top
+    Robot.print(char('0'+i));//line_1: 0-9
+  }
+  for(int i=0;i<10;i++){
+    Robot.setCursor(i*11+12,13+top);//11*1+2+top
+    Robot.print(char('A'+i));//line_2: A-J
+  }
+  for(int i=0;i<10;i++){
+    Robot.setCursor(i*11+12,24+top);//11*2+2+top
+    Robot.print(char('K'+i));//line_3: K-T
+  }
+  for(int i=0;i<6;i++){
+    Robot.setCursor(i*11+12,35+top);//11*3+2+top
+    Robot.print(char('U'+i));//line_4: U-Z
+  }*/
+  //space and enter at the end of the last line.
+  Robot.setCursor(78,35+top);//6*11+12=78
+  Robot.print('_');//_
+  
+  Robot.setCursor(89,35+top);//7*11+12=89
+  Robot.print("Enter");//enter
+}
+
+
+
+VirtualKeyboard Vkey=VirtualKeyboard();
\ No newline at end of file
diff --git a/libraries/Robot_Control/utility/VirtualKeyboard.h b/libraries/Robot_Control/utility/VirtualKeyboard.h
new file mode 100644
index 0000000..273edb7
--- /dev/null
+++ b/libraries/Robot_Control/utility/VirtualKeyboard.h
@@ -0,0 +1,28 @@
+#ifndef VIRTUAL_KEYBOARD_H
+#define VIRTUAL_KEYBOARD_H
+
+#include <Arduino.h>
+#include <ArduinoRobot.h>
+
+class VirtualKeyboard{
+	public:
+		//void begin();
+		void display(uint8_t top, uint16_t showColor=BLACK, uint16_t hideColor=WHITE);
+		void hide();
+		
+		char getSelection();
+		void run();
+		
+	private:
+		uint8_t top;
+		bool onOff;
+		
+		uint16_t showColor;
+		uint16_t hideColor;
+		
+		int getColLin(int val);
+
+};
+
+extern VirtualKeyboard Vkey;
+#endif
\ No newline at end of file
diff --git a/libraries/Robot_Control/utility/scripts_Hello_User.h b/libraries/Robot_Control/utility/scripts_Hello_User.h
new file mode 100644
index 0000000..29f085f
--- /dev/null
+++ b/libraries/Robot_Control/utility/scripts_Hello_User.h
@@ -0,0 +1,51 @@
+#include <avr/pgmspace.h>
+
+//an advanced trick for storing strings inside the program space
+//as the ram of Arduino is very tiny, keeping too many string in it
+//can kill the program
+
+prog_char hello_user_script1[] PROGMEM="What's your name?";
+prog_char hello_user_script2[] PROGMEM="Give me a name!";
+prog_char hello_user_script3[] PROGMEM="And the country?";
+prog_char hello_user_script4[] PROGMEM="The city you're in?";
+prog_char hello_user_script5[] PROGMEM="  Plug me to\n\n  your computer\n\n  and start coding!";
+
+prog_char hello_user_script6[] PROGMEM="     Hello User!\n\n It's me, your robot\n\n    I'm alive! <3";
+prog_char hello_user_script7[] PROGMEM="  First I need some\n\n   input from you!";
+prog_char hello_user_script8[] PROGMEM="     Use the knob\n\n   to select letters";
+prog_char hello_user_script9[] PROGMEM="   Use L/R button\n\n to move the cursor,\n\n  middle to confirm";
+prog_char hello_user_script10[] PROGMEM="  Press middle key\n   to continue...";
+prog_char hello_user_script11[] PROGMEM="  Choose \"enter\" to\n\n  finish the input";
+
+PROGMEM const char *scripts_Hello_User[]={
+  hello_user_script1,
+  hello_user_script2,
+  hello_user_script3,
+  hello_user_script4,
+  hello_user_script5,
+  hello_user_script6,
+  hello_user_script7,
+  hello_user_script8,
+  hello_user_script9,
+  hello_user_script10,
+  hello_user_script11,
+};
+
+/*
+void getPGMtext(int seq){
+  //It takes a string from program space, and fill it  
+  //in the buffer
+  strcpy_P(buffer,(char*)pgm_read_word(&(scripts[seq])));
+}
+
+void writeScript(int seq, int line, int col){
+  //print a string from program space to a specific line,
+  //column on the LCD
+  
+  //first fill the buffer with text from program space
+  getPGMtext(seq);
+  //then print it to the screen
+  textManager.writeText(line,col,buffer);
+}
+
+*/
\ No newline at end of file
diff --git a/libraries/Robot_Control/utility/twi.c b/libraries/Robot_Control/utility/twi.c
new file mode 100644
index 0000000..6b2db3c
--- /dev/null
+++ b/libraries/Robot_Control/utility/twi.c
@@ -0,0 +1,527 @@
+/*
+  twi.c - TWI/I2C library for Wiring & Arduino
+  Copyright (c) 2006 Nicholas Zambetti.  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
+
+  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+*/
+
+#include <math.h>
+#include <stdlib.h>
+#include <inttypes.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <compat/twi.h>
+#include "Arduino.h" // for digitalWrite
+
+#ifndef cbi
+#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+
+#ifndef sbi
+#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+#include "pins_arduino.h"
+#include "twi.h"
+
+static volatile uint8_t twi_state;
+static volatile uint8_t twi_slarw;
+static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
+static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
+
+static void (*twi_onSlaveTransmit)(void);
+static void (*twi_onSlaveReceive)(uint8_t*, int);
+
+static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_masterBufferIndex;
+static volatile uint8_t twi_masterBufferLength;
+
+static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_txBufferIndex;
+static volatile uint8_t twi_txBufferLength;
+
+static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_rxBufferIndex;
+
+static volatile uint8_t twi_error;
+
+/* 
+ * Function twi_init
+ * Desc     readys twi pins and sets twi bitrate
+ * Input    none
+ * Output   none
+ */
+void twi_init(void)
+{
+  // initialize state
+  twi_state = TWI_READY;
+  twi_sendStop = true;		// default value
+  twi_inRepStart = false;
+  
+  // activate internal pullups for twi.
+  digitalWrite(SDA, 1);
+  digitalWrite(SCL, 1);
+
+  // initialize twi prescaler and bit rate
+  cbi(TWSR, TWPS0);
+  cbi(TWSR, TWPS1);
+  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
+
+  /* twi bit rate formula from atmega128 manual pg 204
+  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
+  note: TWBR should be 10 or higher for master mode
+  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);
+}
+
+/* 
+ * Function twi_slaveInit
+ * Desc     sets slave address and enables interrupt
+ * Input    none
+ * Output   none
+ */
+void twi_setAddress(uint8_t address)
+{
+  // set twi slave address (skip over TWGCE bit)
+  TWAR = address << 1;
+}
+
+/* 
+ * Function twi_readFrom
+ * Desc     attempts to become twi bus master and read a
+ *          series of bytes from a device on the bus
+ * Input    address: 7bit i2c device address
+ *          data: pointer to byte array
+ *          length: number of bytes to read into array
+ *          sendStop: Boolean indicating whether to send a stop at the end
+ * Output   number of bytes read
+ */
+uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < length){
+    return 0;
+  }
+
+  // wait until twi is ready, become master receiver
+  while(TWI_READY != twi_state){
+    continue;
+  }
+  twi_state = TWI_MRX;
+  twi_sendStop = sendStop;
+  // reset error state (0xFF.. no error occured)
+  twi_error = 0xFF;
+
+  // initialize buffer iteration vars
+  twi_masterBufferIndex = 0;
+  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;
+
+  if (true == twi_inRepStart) {
+    // if we're in the repeated start state, then we've already sent the start,
+    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
+    // We need to remove ourselves from the repeated start state before we enable interrupts,
+    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
+    // up. Also, don't enable the START interrupt. There may be one pending from the 
+    // repeated start that we sent outselves, and that would really confuse things.
+    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    TWDR = twi_slarw;
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
+  }
+  else
+    // send start condition
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+
+  // wait for read operation to complete
+  while(TWI_MRX == twi_state){
+    continue;
+  }
+
+  if (twi_masterBufferIndex < length)
+    length = twi_masterBufferIndex;
+
+  // copy twi buffer to data
+  for(i = 0; i < length; ++i){
+    data[i] = twi_masterBuffer[i];
+  }
+	
+  return length;
+}
+
+/* 
+ * Function twi_writeTo
+ * Desc     attempts to become twi bus master and write a
+ *          series of bytes to a device on the bus
+ * Input    address: 7bit i2c device address
+ *          data: pointer to byte array
+ *          length: number of bytes in array
+ *          wait: boolean indicating to wait for write or not
+ *          sendStop: boolean indicating whether or not to send a stop at the end
+ * Output   0 .. success
+ *          1 .. length to long for buffer
+ *          2 .. address send, NACK received
+ *          3 .. data send, NACK received
+ *          4 .. other twi error (lost bus arbitration, bus error, ..)
+ */
+uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < length){
+    return 1;
+  }
+
+  // wait until twi is ready, become master transmitter
+  while(TWI_READY != twi_state){
+    continue;
+  }
+  twi_state = TWI_MTX;
+  twi_sendStop = sendStop;
+  // reset error state (0xFF.. no error occured)
+  twi_error = 0xFF;
+
+  // initialize buffer iteration vars
+  twi_masterBufferIndex = 0;
+  twi_masterBufferLength = length;
+  
+  // copy data to twi buffer
+  for(i = 0; i < length; ++i){
+    twi_masterBuffer[i] = data[i];
+  }
+  
+  // build sla+w, slave device address + w bit
+  twi_slarw = TW_WRITE;
+  twi_slarw |= address << 1;
+  
+  // if we're in a repeated start, then we've already sent the START
+  // in the ISR. Don't do it again.
+  //
+  if (true == twi_inRepStart) {
+    // if we're in the repeated start state, then we've already sent the start,
+    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
+    // We need to remove ourselves from the repeated start state before we enable interrupts,
+    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
+    // up. Also, don't enable the START interrupt. There may be one pending from the 
+    // repeated start that we sent outselves, and that would really confuse things.
+    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    TWDR = twi_slarw;				
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
+  }
+  else
+    // send start condition
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
+
+  // 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
+}
+
+/* 
+ * Function twi_transmit
+ * Desc     fills slave tx buffer with data
+ *          must be called in slave tx event callback
+ * Input    data: pointer to byte array
+ *          length: number of bytes in array
+ * Output   1 length too long for buffer
+ *          2 not slave transmitter
+ *          0 ok
+ */
+uint8_t twi_transmit(const uint8_t* data, uint8_t length)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < length){
+    return 1;
+  }
+  
+  // ensure we are currently a slave transmitter
+  if(TWI_STX != twi_state){
+    return 2;
+  }
+  
+  // set length and copy data into tx buffer
+  twi_txBufferLength = length;
+  for(i = 0; i < length; ++i){
+    twi_txBuffer[i] = data[i];
+  }
+  
+  return 0;
+}
+
+/* 
+ * Function twi_attachSlaveRxEvent
+ * Desc     sets function called before a slave read operation
+ * Input    function: callback function to use
+ * Output   none
+ */
+void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )
+{
+  twi_onSlaveReceive = function;
+}
+
+/* 
+ * Function twi_attachSlaveTxEvent
+ * Desc     sets function called before a slave write operation
+ * Input    function: callback function to use
+ * Output   none
+ */
+void twi_attachSlaveTxEvent( void (*function)(void) )
+{
+  twi_onSlaveTransmit = function;
+}
+
+/* 
+ * Function twi_reply
+ * Desc     sends byte or readys receive line
+ * Input    ack: byte indicating to ack or to nack
+ * Output   none
+ */
+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);
+  }else{
+	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
+  }
+}
+
+/* 
+ * Function twi_stop
+ * Desc     relinquishes bus master status
+ * Input    none
+ * Output   none
+ */
+void twi_stop(void)
+{
+  // send stop condition
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
+
+  // wait for stop condition to be exectued on bus
+  // TWINT is not set after a stop condition!
+  while(TWCR & _BV(TWSTO)){
+    continue;
+  }
+
+  // update twi state
+  twi_state = TWI_READY;
+}
+
+/* 
+ * Function twi_releaseBus
+ * Desc     releases bus control
+ * Input    none
+ * Output   none
+ */
+void twi_releaseBus(void)
+{
+  // release bus
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
+
+  // update twi state
+  twi_state = TWI_READY;
+}
+
+SIGNAL(TWI_vect)
+{
+  switch(TW_STATUS){
+    // All Master
+    case TW_START:     // sent start condition
+    case TW_REP_START: // sent repeated start condition
+      // copy device address and r/w bit to output register and ack
+      TWDR = twi_slarw;
+      twi_reply(1);
+      break;
+
+    // Master Transmitter
+    case TW_MT_SLA_ACK:  // slave receiver acked address
+    case TW_MT_DATA_ACK: // slave receiver acked data
+      // if there is data to send, send it, otherwise stop 
+      if(twi_masterBufferIndex < twi_masterBufferLength){
+        // copy data to output register and ack
+        TWDR = twi_masterBuffer[twi_masterBufferIndex++];
+        twi_reply(1);
+      }else{
+	if (twi_sendStop)
+          twi_stop();
+	else {
+	  twi_inRepStart = true;	// we're gonna send the START
+	  // don't enable the interrupt. We'll generate the start, but we 
+	  // avoid handling the interrupt until we're in the next transaction,
+	  // at the point where we would normally issue the start.
+	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+	  twi_state = TWI_READY;
+	}
+      }
+      break;
+    case TW_MT_SLA_NACK:  // address sent, nack received
+      twi_error = TW_MT_SLA_NACK;
+      twi_stop();
+      break;
+    case TW_MT_DATA_NACK: // data sent, nack received
+      twi_error = TW_MT_DATA_NACK;
+      twi_stop();
+      break;
+    case TW_MT_ARB_LOST: // lost bus arbitration
+      twi_error = TW_MT_ARB_LOST;
+      twi_releaseBus();
+      break;
+
+    // Master Receiver
+    case TW_MR_DATA_ACK: // data received, ack sent
+      // put byte into buffer
+      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
+    case TW_MR_SLA_ACK:  // address sent, ack received
+      // ack if more bytes are expected, otherwise nack
+      if(twi_masterBufferIndex < twi_masterBufferLength){
+        twi_reply(1);
+      }else{
+        twi_reply(0);
+      }
+      break;
+    case TW_MR_DATA_NACK: // data received, nack sent
+      // put final byte into buffer
+      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
+	if (twi_sendStop)
+          twi_stop();
+	else {
+	  twi_inRepStart = true;	// we're gonna send the START
+	  // don't enable the interrupt. We'll generate the start, but we 
+	  // avoid handling the interrupt until we're in the next transaction,
+	  // at the point where we would normally issue the start.
+	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+	  twi_state = TWI_READY;
+	}    
+	break;
+    case TW_MR_SLA_NACK: // address sent, nack received
+      twi_stop();
+      break;
+    // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case
+
+    // Slave Receiver
+    case TW_SR_SLA_ACK:   // addressed, returned ack
+    case TW_SR_GCALL_ACK: // addressed generally, returned ack
+    case TW_SR_ARB_LOST_SLA_ACK:   // lost arbitration, returned ack
+    case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
+      // enter slave receiver mode
+      twi_state = TWI_SRX;
+      // indicate that rx buffer can be overwritten and ack
+      twi_rxBufferIndex = 0;
+      twi_reply(1);
+      break;
+    case TW_SR_DATA_ACK:       // data received, returned ack
+    case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
+      // if there is still room in the rx buffer
+      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
+        // put byte in buffer and ack
+        twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
+        twi_reply(1);
+      }else{
+        // otherwise nack
+        twi_reply(0);
+      }
+      break;
+    case TW_SR_STOP: // stop or repeated start condition received
+      // put a null char after data if there's room
+      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
+        twi_rxBuffer[twi_rxBufferIndex] = '\0';
+      }
+      // sends ack and stops interface for clock stretching
+      twi_stop();
+      // callback to user defined callback
+      twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
+      // since we submit rx buffer to "wire" library, we can reset it
+      twi_rxBufferIndex = 0;
+      // ack future responses and leave slave receiver state
+      twi_releaseBus();
+      break;
+    case TW_SR_DATA_NACK:       // data received, returned nack
+    case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
+      // nack back at master
+      twi_reply(0);
+      break;
+    
+    // Slave Transmitter
+    case TW_ST_SLA_ACK:          // addressed, returned ack
+    case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
+      // enter slave transmitter mode
+      twi_state = TWI_STX;
+      // ready the tx buffer index for iteration
+      twi_txBufferIndex = 0;
+      // set tx buffer length to be zero, to verify if user changes it
+      twi_txBufferLength = 0;
+      // request for txBuffer to be filled and length to be set
+      // note: user must call twi_transmit(bytes, length) to do this
+      twi_onSlaveTransmit();
+      // if they didn't change buffer & length, initialize it
+      if(0 == twi_txBufferLength){
+        twi_txBufferLength = 1;
+        twi_txBuffer[0] = 0x00;
+      }
+      // transmit first byte from buffer, fall
+    case TW_ST_DATA_ACK: // byte sent, ack returned
+      // copy data to output register
+      TWDR = twi_txBuffer[twi_txBufferIndex++];
+      // if there is more to send, ack, otherwise nack
+      if(twi_txBufferIndex < twi_txBufferLength){
+        twi_reply(1);
+      }else{
+        twi_reply(0);
+      }
+      break;
+    case TW_ST_DATA_NACK: // received nack, we are done 
+    case TW_ST_LAST_DATA: // received ack, but we are done already!
+      // ack future responses
+      twi_reply(1);
+      // leave slave receiver state
+      twi_state = TWI_READY;
+      break;
+
+    // All
+    case TW_NO_INFO:   // no state information
+      break;
+    case TW_BUS_ERROR: // bus error, illegal stop/start
+      twi_error = TW_BUS_ERROR;
+      twi_stop();
+      break;
+  }
+}
+
diff --git a/libraries/Robot_Control/utility/twi.h b/libraries/Robot_Control/utility/twi.h
new file mode 100644
index 0000000..6526593
--- /dev/null
+++ b/libraries/Robot_Control/utility/twi.h
@@ -0,0 +1,53 @@
+/*
+  twi.h - TWI/I2C library for Wiring & Arduino
+  Copyright (c) 2006 Nicholas Zambetti.  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
+*/
+
+#ifndef twi_h
+#define twi_h
+
+  #include <inttypes.h>
+
+  //#define ATMEGA8
+
+  #ifndef TWI_FREQ
+  #define TWI_FREQ 100000L
+  #endif
+
+  #ifndef TWI_BUFFER_LENGTH
+  #define TWI_BUFFER_LENGTH 32
+  #endif
+
+  #define TWI_READY 0
+  #define TWI_MRX   1
+  #define TWI_MTX   2
+  #define TWI_SRX   3
+  #define TWI_STX   4
+  
+  void twi_init(void);
+  void twi_setAddress(uint8_t);
+  uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t);
+  uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t);
+  uint8_t twi_transmit(const uint8_t*, uint8_t);
+  void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );
+  void twi_attachSlaveTxEvent( void (*)(void) );
+  void twi_reply(uint8_t);
+  void twi_stop(void);
+  void twi_releaseBus(void);
+
+#endif
+