       enableInterrupt(encoderPin1, interruptFunctionLeft, RISING);
       enableInterrupt(encoderPin2, interruptFunctionRight, RISING);
       break ;
-//    case PIXY_CONFIG:
-//      TODO: Implement
-//      break;
-
+    case PIXY_INIT:
+      pixy.init()
+      pixyIsReporting = true;
+      maxPixyBlocks = argv[0]; 
+      break;
+    case PIXY_SET_COMMAND:
+      if (argv[0] == PIXY_SET_SERVOS) {
+        int s0 = argv[1] + (argv[2] << 7);
+        int s1 = argv[3] + (argv[4] << 7);
+        pixy.setServos(s0, s1);
+      } else if (argv[0] == PIXY_SET_BRIGHTNESS) {
+        int brightness = argv[1] + (argv[2] << 7);
+        pixy.setBrightness(brightness);
+      } else if (argv[0] == PIXY_SET_LED) {
+        int r = argv[1] + (argv[2] << 7);
+        int g = argv[3] + (argv[4] << 7);
+        int b = argv[5] + (argv[6] << 7);
+        pixy.setLED(r, g, b);
+      }
+      break;
     case TONE_DATA:
       byte toneCommand, pin;
       int frequency, duration;

#include <NewPing.h>
#include <Stepper.h>
#include <EnableInterrupt.h>
#include <avr/wdt.h>

#define I2C_WRITE                   B00000000

}


void writePixyBlock(byte pixyBlockIndex) {
//pixy.blocks[i].signature The signature number of the detected object (1-7 for normal signatures)
//pixy.blocks[i].x The x location of the center of the detected object (0 to 319)
//pixy.blocks[i].y The y location of the center of the detected object (0 to 199)
//pixy.blocks[i].width The width of the detected object (1 to 320)
//pixy.blocks[i].height The height of the detected object (1 to 200)
//pixy.blocks[i].angle The angle of the object detected object if the detected object is a color code.
//pixy.blocks[i].print() A member function that prints the detected object information to the serial port
  Firmata.write(pixy.blocks[pixyBlockIndex].signature & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].signature >> 7) & 0x7f);
  Firmata.write(pixy.blocks[pixyBlockIndex].x & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].x >> 7) & 0x7f);
  Firmata.write(pixy.blocks[pixyBlockIndex].y & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].y >> 7) & 0x7f);
  Firmata.write(pixy.blocks[pixyBlockIndex].width & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].width >> 7) & 0x7f);
  Firmata.write(pixy.blocks[pixyBlockIndex].height & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].height >> 7) & 0x7f);
  Firmata.write(pixy.blocks[pixyBlockIndex].angle & 0x7f);
  Firmata.write((pixy.blocks[pixyBlockIndex].angle >> 7) & 0x7f);
}


void printData(char * id,  long data)
{
