#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;
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);
   //while (1);

}

void loop() {
  while ( Serial.available() > 0) {
    in = Serial.read();
    Serial.println(in);

  }
  if (in == 'X') {
    axis_one_left();
  }
  if (in == 'x') {
    axis_one_right();
  }
  if (in == 'Y') {
    axis_two_forward();
  }
  if (in == 'y') {
    axis_two_backword();
  }
  if (in == 'Z') {
    axis_three_forward();
  }
  if (in == 'z') {
    axis_three_backword();
  }
  if (in == 'O') {
    axis_two_ctr = 1;
    axis_two_forward();
  }
  if (in == 'C') {
    axis_two_ctr = 0;
    axis_two_backword();
  }
  ///Serial.println('S');
}
void axis_one_right() {
  axis_one_ctr++;
  axis_one.write(axis_one_ctr);
  // Serial.println("Come");
  delay(x);
  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");
  delay(x);
  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);
  delay(x);
  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");
  delay(x);
  if (axis_two_ctr < 70) {
    axis_two_ctr = 70;
  }
  
}
void axis_three_forward() {
  axis_three_ctr++;
  axis_three.write(axis_three_ctr);
  delay(x);
  if (axis_three_ctr > 120) {
    axis_three_ctr = 120;
  }
 
}

void axis_three_backword() {
  axis_three_ctr--;
  axis_three.write(axis_three_ctr);
  delay(x);
  if (axis_three_ctr < 5) {
    axis_three_ctr = 5;
  }
 
}