       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

      enableInterrupt(encoderPin2, interruptFunctionRight, RISING);
    }
    if (pixyIsReporting) {
      numPixyBlocks = pixy.getBlocks();
      Firmata.write(START_SYSEX);
      Firmata.write(PIXY_DATA);
      Firmata.write(numPixyBlocks);
      if (maxPixyBlocks != 0 && numPixyBlocks > maxPixyBlocks) {
        numPixyBlocks = maxPixyBlocks;
      }
      for (pixyBlockIndex = 0; pixyBlockIndex < numPixyBlocks; pixyBlockIndex++) {
        writePixyBlock(pixyBlockIndex);
      }
      Firmata.write(END_SYSEX);
    }
  }
  if( keepAliveInterval ) {
    currentMillis = millis();
