#include <Stepper.h>
#include <Servo.h>
float steps_per_revolution1 = 0.0;
float steps_per_revolution2 = 0.0;
Stepper myStepper1(250, 0, 1, 2, 3);
Stepper myStepper2(250, 4, 5, 6, 7);
Servo servo1_base;
Servo servo2_arm;
Servo servo3_grip;
int pin1 = 11 ; 
int pin2 = 10 ; 
int pin3 = 9 ; 
int grip_key_up = 13;
int grip_key_down = 12;
int y_key_1 = A0;
int x_key_1 = A1;
int y_key_2 = A2;
int x_key_2 = A3;
int base_position = 0;
int arm_position = 0;
int grip_position = 0;
int x_pos_1;
int y_pos_1;
int x_pos_2;
int y_pos_2;
void setup() {
  myStepper1.setSpeed(18);
  myStepper2.setSpeed(18);
  servo1_base.attach(pin1);
  servo2_arm.attach(pin2);
  servo3_grip.attach(pin3);
  servo1_base.write(0);
  servo2_arm.write(0);
  servo3_grip.write(0);
  pinMode(x_key_1, INPUT);
  pinMode(y_key_1, INPUT);
  pinMode(x_key_2, INPUT);
  pinMode(y_key_2, INPUT);
  pinMode(grip_key_up, INPUT_PULLUP);
  pinMode(grip_key_down, INPUT_PULLUP);

}

void loop() {
  x_pos_1 = analogRead(x_key_1);
  y_pos_1 = analogRead(y_key_1);
  x_pos_2 = analogRead(x_key_2);
  y_pos_2 = analogRead(y_key_2);
    if (digitalRead(grip_key_up) == LOW && grip_position <88){
    grip_position = grip_position + 1; 
    servo3_grip.write(grip_position);
    delay(50);
  }

  if (digitalRead(grip_key_down)== LOW && grip_position > 0 ){
    grip_position = grip_position - 1; 
    servo3_grip.write(grip_position);
    delay(50);
  }
  if (x_pos_2 == 0 && y_pos_2 == 512){
    base_position = base_position + 1;
    servo1_base.write(base_position);
    delay(50);
  }
  if (x_pos_2 == 1023 && y_pos_2 == 512){
    base_position = base_position - 1;
    servo1_base.write(base_position);
    delay(50);
  }
  if (x_pos_2 == 512 && y_pos_2 == 1023){
    arm_position = arm_position + 1;
    servo2_arm.write(arm_position);
    delay(50);
  }
  if (x_pos_2 == 512 && y_pos_2 == 0){
    arm_position = arm_position - 1;
    servo2_arm.write(arm_position);
    delay(50);
  }

  if (x_pos_1 == 512 && y_pos_1 == 1023){ //y up
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ; 
    steps_per_revolution2 = steps_per_revolution2 + 1.8 ; 
    myStepper1.step(steps_per_revolution1);
    myStepper2.step(-steps_per_revolution2);
    delay(50);
  }
  if (x_pos_1 == 512 && y_pos_1 == 0){ //y down
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ; 
    steps_per_revolution2 = steps_per_revolution2 + 1.8 ; 
    myStepper1.step(-steps_per_revolution1);
    myStepper2.step(steps_per_revolution2);
    delay(50);
  }
  
  if (x_pos_1 == 0 && y_pos_1 <= 512){ //x right
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ; 
     steps_per_revolution2 = steps_per_revolution2 + 1.8 ; 
    myStepper1.step(steps_per_revolution1);
     myStepper2.step(steps_per_revolution2);

    delay(50);
  }
  if (x_pos_1 == 1023 && y_pos_1 >= 512){ //x left
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ;  
     steps_per_revolution2 = steps_per_revolution2 + 1.8 ; 
      myStepper2.step(-steps_per_revolution2);
    myStepper1.step(-steps_per_revolution1);
    delay(50);
  }

  


  if (x_pos_1 == 1023 && y_pos_1 == 1023){ //up left
    steps_per_revolution2 = steps_per_revolution2 + 1.8 ;  
    myStepper2.step(steps_per_revolution2);
    delay(50);
  }
  if (x_pos_1 == 0 && y_pos_1 == 1023){ //up right
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ;  
    myStepper1.step(-steps_per_revolution1);
    delay(50);
  }
  
  if (x_pos_1 == 1023 && y_pos_1 <= 0){ //down left
    steps_per_revolution1 = steps_per_revolution1 + 1.8 ;  
    myStepper1.step(steps_per_revolution1);
    delay(50);
  }
  if (x_pos_1 == 0 && y_pos_1 >= 0){ //down right
    steps_per_revolution2 = steps_per_revolution2 + 1.8 ;  
    myStepper2.step(-steps_per_revolution2);
    delay(50);
  }
  
}