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

unsigned int axis_one_ctr = 90;//90
unsigned int axis_two_ctr = 130; // min70 , max150
unsigned int axis_three_ctr = 110;//
unsigned int axis_four_ctr = 10;
unsigned int x = 20;
unsigned 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);
  axis_four.attach(13);
  axis_one.write(axis_one_ctr);
  axis_two.write(axis_two_ctr);
  axis_three.write(axis_three_ctr);
  axis_four.write(axis_four_ctr);
  lgUpdateTime = millis();
  //while(1);
}

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();
      Serial.println(in);
   // }

    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 > 175)
  {
    axis_three_ctr = 175;
  }
}

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

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