#include <Servo.h>
int data[3], v[2];
int i=0, j=0, n=0, prov=320, num, plus=320, plus1=240, aum=90, aum1=90;
int centerX=320, centerY=240,distX=0,distY=0, changleX=0,changleY=0;
char c;
Servo servo1;
Servo servo2;

int pos = 0;

void setup()
{
  servo1.attach(5); 
  servo2.attach(6); 

  Serial.begin(9600);
}

void loop()
{  
 
  //Read Serial bus data received from Python and write them into servo position
  if(Serial.available())
  {
    prov=Serial.read()-48;
    if(prov!=72)
    {
      data[i]=prov;
      i++;
    }
    else
    {
      if(i==0)
        num=0;
      if(i==1)
        num=data[0];
      if(i==2)
        num=data[1]+data[0]*10;
      if(i==3)
        num=data[2]+data[1]*10+data[0]*100;
      i=0;
      v[n]=num;
      n++;
      if(n==2)
      {
        //Start X axis
        distX=v[0]-320;
        changleX=int(distX/30);
        if(30<=aum && aum<=150){
          aum=aum-changleX;

          servo1.write(aum);
          delay(15);
        }else if(aum<30){
          aum=30;
        }else{
          aum=150;
        }
        
        
        
        //aum=90;
        //Start Y axis
        distY=v[1]-240;
        changleY=int(distY/40);
        if(60<=aum1 && aum1<=150){
          aum1=aum1-changleY;
          servo2.write(aum1);
          delay(15);
        }else if(aum1<60){
          aum1=60;
        }else{
          aum1=150;
        }
        //plus1=240;
        //aum1=45;
        n=0;
      }
    }
  }
}