//servo library
#include <Servo.h>
//stepper
#include <AccelStepper.h>
AccelStepper stepperr(1,8,9);  //8 hoa el step / 9 dir

int stepper_val =0;
int previous_stepper = 0;
int long newval_stepper =0;
int stepPin =  8;
int dirPin =   9;
int Step = 3200;
int Delay = 200;


// first joystickk
int joy1x_pin = A0;
int joy1_xval ;

int joy1y_pin = A1;
int joy1_yval ;

//second joystick 
int joy2x_pin = A2;
int joy2_xval ;

int joy2y_pin = A3;
int joy2_yval ;

//third joystick
int joy3x_pin = A4;
int joy3_xval ;

int joy3y_pin = A5;
int joy3_yval ;



//for first servo shoulder1
Servo s1 ;
int position1 = 90;
int servo1 = 3;

//for second servo shoulder2
Servo s2 ;
int position2 = 90;
int servo2 = 3;

//for third servo elbow
Servo s3 ;
int position3 = 90;
int servo3 = 4;

//for forth servo wrist joint1
Servo s4 ;
int position4 = 90;
int servo4 = 5;

//for third servo wrist joint2
Servo s5 ;
int position5 = 90;
int servo5 = 6;

//for third servo gripper
Servo s6 ;
int position6 = 90;
int servo6 = 7;



void setup() {
  Serial.begin(9600);
  pinMode(A0 , INPUT);
  pinMode(A1 , INPUT);
  pinMode(A3 , INPUT);
  pinMode(A4 , INPUT);
  pinMode(A5 , INPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  stepperr.setMaxSpeed (4800);
  stepperr.setAcceleration (4800);
 
  s1.attach(3);
  s1.write(position1);
  delay (15); 

  s2.attach(3);
  s2.write(position2);
  delay(15);

  s3.attach(4);
  s3.write(position3);
  delay(15);

  s4.attach(5);
  s3.write(position4);
  delay(15);

  s5.attach(6);
  s3.write(position5);
  delay(15);

  s6.attach(7);
  s3.write(position6);
  delay(15);
  

}
void loop() {
  joy1_xval = analogRead(A0);
  delay(100);
  Serial.print("joy 1 val :");
  Serial.println(joy1_xval);

  if (joy1_xval >= 550)
  {
    if (position1 <=180  &&  position2 <=180)
    {
    position1 = position1 + 10;
    s1.write(position1);

    position2 = position2 +10;
    s2.write(position2);
  }
  }
  else if (joy1_xval <=500)
  {

     if (position1 >= 0   &&  position2 >= 0)
    {
    position1 = position1 - 10;
    s1.write(position1);

    position2 = position2 -10;
    s2.write(position2);
    }
  }
  joy1_yval = analogRead(A1);
  delay(100);
  Serial.print("joy 1 y val :");
  Serial.println(joy1_yval);
  if (joy1_yval >= 550)
  {
    if (position3 <=180)
    {
    position3 = position3 + 10;
    s3.write(position3);
    }
  }
  else if (joy1_yval <=500)
  {

    if (position3 >= 0)
    {
    position3 = position3 - 10;
    s3.write(position3);
    }
  }

  joy2_xval = analogRead(A2);
  delay(100);
  Serial.print("joy2 x val :");
  Serial.println(joy2_xval);
  if (joy2_xval >= 550)
  {
    if (position4 <=180)
    {
    position4 = position4 + 10;
    s4.write(position4);
    }
  }
  else if (joy2_xval <=500)
  {

    if (position4 >= 0)
    {
    position4 = position4 - 10;
    s4.write(position4);
    }
  }
  joy2_yval = analogRead(A3);
  delay(100);
  Serial.print("joy2 y val :");
  Serial.println(joy2_yval);
  if (joy2_yval >= 550)
  {
    if (position5 <=180)
    {
    position5 = position5 + 10;
    s5.write(position5);
    }
  }
  else if (joy2_yval <=500)
  {

    if (position5 >= 0)
    {
    position5 = position5 - 10;
    s5.write(position5);
    }
  }

  joy3_xval = analogRead(A4);
  delay(100);
  Serial.print("joy3 x val :");
  Serial.println(joy3_xval);
  if (joy3_xval >= 550)
  {
    if (position6 <=180)
    {
    position6 = position6 + 10;
    s6.write(position6);
    }
  }
  else if (joy3_xval <=500)
  {

    if (position6 >= 0)
    {
    position6 = position6 - 10;
    s6.write(position6);
    }
  }

  joy3_yval = analogRead(A5);
  delay(100);
  Serial.print("joy3 y val :");
  Serial.println(joy3_yval);
  if (joy3_yval >= 550)
  {
    stepper_right();
  }
  else if (joy3_xval <=500)
  {
    stepper_left();
    
  }


}

void stepper_right () {


  digitalWrite(dirPin, LOW); // Enables the motor to move in a particular direction

  for (int x = 0; x < Step; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(Delay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(Delay);
  }
  delay(50);
}
void stepper_left ()
{
  digitalWrite(dirPin, HIGH); //Changes the rotations direction

  for (int x = 0; x < Step; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(Delay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(Delay);
  }
}