 double compassTotal = 0;
 
 
+double readUltraSonic() {
+  double total = 0;
+  for(i = 0; i < 50; i++) {
+    total += analogRead(ultrasonicpin);
+  }
+  return ((total / 50.0) / 2.0) * 2.54;
+}
+
 void printData() {
 
   int dif;

#include <Timer.h>

#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
Pixy pixy;
LIDARLite lidar;
uint16_t blocks;
boolean debug = false;

  static int i = 0;
  int CenterOfBlocks;
  char buf[7];
//
//  if (blocks) {
//    //Serial.println("Block Count: " + String(blocks));
//    if (blocks == 1) {
//      CenterOfBlocks = pixy.blocks[0].x;
//    } else if (blocks == 2) {
//      CenterOfBlocks = (pixy.blocks[0].x + pixy.blocks[1].x) / 2;
//    } else {
//      CenterOfBlocks = 0;
//    }
//    //if (blocks == 1 || blocks == 2) {
//    dif = X_CENTER - CenterOfBlocks;
//    if (dif < -5) {
//      sprintf(buffer, "^%s~%s~%s~", "PIXY", "direction", "right");
//      Serial.write(buffer);
//    } else if (dif > 5) {
//      sprintf(buffer, "^%s~%s~%s~", "PIXY", "direction", "left");
//      Serial.write(buffer);
//    } else {
//      sprintf(buffer, "^%s~%s~%s~", "PIXY", "direction", "straight");
//      Serial.write(buffer);
//    }
//    if (debug) {
//      Serial.println("");
//    }
//
//  }
  dtostrf(readUltraSonic(), 5, 2 , buf);
  sprintf(buffer, "^%s~%s~%s~", "ULTRASONIC", "cm", buf);
  Serial.write(buffer);
  if (debug) {
    Serial.println("");
  }

  //LIDAR STUFF
  sprintf(buffer, "^%s~%s~%d~", "LIDAR", "cm", lidar.distance());
