#include <Servo.

h>
#include <Pixy2.h>
#include <Adafruit_RGBLCDShield.h>
#include <utility/Adafruit_MCP23017.h>
#include <Wire.h>
Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

// This is the main Pixy object

Pixy2 pixy;
Servo myservo;
#define RED 0x1
#define YELLOW 0x3
#define GREEN 0x2
#define TEAL 0x6
#define BLUE 0x4
#define VIOLET 0x5
#define WHITE 0x7

int posball;
int posServo=90;
void setup()
{
Serial.begin(115200);
Serial.print("Starting...\n");
lcd.begin(16, 2);

int time = millis();

time = millis() - time;
Serial.print("Took "); Serial.print(time); Serial.println(" ms");
lcd.setBacklight(WHITE);
pixy.init();

myservo.attach(9);
myservo.write(posServo);
}

void loop()
{
int i;
// grab blocks!
pixy.ccc.getBlocks();

// If there are detect blocks, print them!

if (pixy.ccc.numBlocks)
{
int x = pixy.ccc.blocks[0].m_x;
int posball = x - 158;

//Serial.print("Detected ");
if(posball>0)
{posServo = posServo +posball/10;
/*if(posServo<180)

posServo++;*/
}
else (posServo = posServo +posball/10);
 if(posball>0)
{
posServo--;
//posServo--;
}

Serial.println(pixy.ccc.numBlocks);
lcd.setCursor(0,1);
lcd.print(x);
myservo.write(posServo);
}
/*else if (!pixy.ccc.numBlocks)
{
posball=90;
myservo.write(posball);

}*/
}