#include <Servo.h>
Servo axis_one;
Servo axis_two;
Servo axis_three;
Servo grip; //0 30

int axis_one_ctr = 90;
int axis_two_ctr = 130; // min70 , max150
int axis_three_ctr = 40;
int x = 20;
int y = 50;
char in;
unsigned long lgUpdateTime; //was unsigned int

void setup()
{
         Serial.begin(9600);

         axis_one.attach(8);
         axis_two.attach(9);
         axis_three.attach(10);
         grip.attach(13);
         axis_one.write(axis_one_ctr);
         axis_two.write(axis_two_ctr);
         axis_three.write(axis_three_ctr);
         grip.write(0);

         lgUpdateTime = millis();
}

void loop()
{
         //Excute loop every 20 milliseconds, faster than the App sends to avoid buffer over-run

	     if((millis() - lgUpdateTime) > 20)
         {
               lgUpdateTime = millis();

               if(Serial.available() > 0)
               {
                             in = Serial.read();
                     switch (in)
                     {
                         case 'X':
                                   axis_one_left();
                                   break;

                         case 'x';
                                   axis_one_right();
                                   break;

                         case 'Y';
                                   axis_two_forward();
                                   break;

                         case 'y'
                                   axis_two_backword();
                                   break;

                         case 'Z'
                                   axis_three_forward();
                                   break;

                         case 'z'
                                   axis_three_backword();
                                   break;

                         case 'O'
                                   axis_two_ctr = 1;
                                   axis_two_forward();
                                   break;

                         case 'C'
                                   axis_two_ctr = 0;
                                   axis_two_backword();
                                   break;

                         case 'S'
                                   //do something;
                                   break;
                     }
               }
         }
         // Serial.println('S');
}

void axis_one_right()
{
         axis_one_ctr++;
         axis_one.write(axis_one_ctr);
         // Serial.println("Come");

         if (axis_one_ctr > 170)
         {
               axis_one_ctr = 170;
         }
}

void axis_one_left()
{
         axis_one_ctr--;
         axis_one.write(axis_one_ctr);
         //Serial.println("Come");

         if (axis_one_ctr < 10)
         {
               axis_one_ctr = 10;
         }
}

void gripper_open() {

}
void gripper_close() {

}
void axis_two_forward()
{
         axis_two_ctr++;
         axis_two.write(axis_two_ctr);

         if (axis_two_ctr > 150)
         {
               axis_two_ctr = 150;
         }
}

void axis_two_backword()
{
         axis_two_ctr--;
         axis_two.write(axis_two_ctr);
         // Serial.println("Come");

         if (axis_two_ctr < 70)
         {
               axis_two_ctr = 70;
         }
}

void axis_three_forward()
{
         axis_three_ctr++;
         axis_three.write(axis_three_ctr);

         if (axis_three_ctr > 120)
         {
               axis_three_ctr = 120;
         }
}

void axis_three_backword()
{
         axis_three_ctr--;
         axis_three.write(axis_three_ctr);

         if (axis_three_ctr < 5)
         {
               axis_three_ctr = 5;
         }
}