#include <Servo.h>
#include <Stepper.h>
#define STEPS 1500 // the number of steps in one revolution of the elbow stepper motors
#define STEPS_B 2048 // the number of steps in one revolution of the base stepper motor
#define elbow_speed 10 //
#define elbow_steps 10 //
#define base_speed 15 //
#define base_steps 25 //


const int arm_servo = 9;      
const int wrist_servo = 10;       
const int gripper_servo = 11;
const int joyH = A1;        // elbow stepper motors
const int joyV = A0;        // base stepper motor
const int Green_LED = 22;
const int Red_LED = 23;
const int button_R = 33;
const int button_L = 31;
int left_limit_button, right_limit_button, potState, pot1State = 0;
int arm_pos, wrist_pos, gripper_pos, X, Y, Z;
int arm_pin = A3;
int wrist_pin = A2;
int gripper_pin = A4;


Stepper stepper_L(STEPS, 50, 51, 52, 53);// *to pins 1, 2, 3 & 4 of left stepper motor's driver, respectivatelly*
Stepper stepper_R(STEPS, 46, 47, 48, 49);// *to pins 1, 2, 3 & 4 of right stepper motor's driver, respectivatelly*
Stepper stepper_base(STEPS_B, 42, 43, 44, 45); // *to pins 1, 2, 3 & 4 of base stepper motor's driver, respectivatelly*



Servo myarm_servo;  
Servo mywrist_servo;  
Servo mygripper_servo;



void setup() {

pinMode (arm_pin, INPUT);
pinMode (wrist_pin, INPUT);
pinMode (gripper_pin, INPUT);


  pinMode(left_limit_button, INPUT_PULLUP);
  pinMode(right_limit_button, INPUT_PULLUP);
  pinMode(Red_LED, OUTPUT);
  pinMode(Green_LED, OUTPUT);
  

  myarm_servo.attach(arm_servo);  
  mywrist_servo.attach(wrist_servo);  
  mygripper_servo.attach(gripper_servo);

}


void loop()

{

 Limit_Swtiches(); // Delete line 63 and lines 67 to 90 if you're not using limit switches
  Motors();
  
}
void Limit_Swtiches() {

digitalWrite(Green_LED, HIGH);
left_limit_button = digitalRead(button_L);
right_limit_button = digitalRead(button_R);

 if (left_limit_button == HIGH)
  {
   stepper_base.step(-70);
   delay(1);
   digitalWrite(Red_LED, HIGH);
      delay(10);
      }


  if (right_limit_button == HIGH)
  {
  stepper_base.step(70);
   delay(1);
digitalWrite(Red_LED, HIGH);
   delay(10);
      }
  
}


void Motors() {
  
digitalWrite(Green_LED, HIGH);
  potState = analogRead(joyH); 
  pot1State = analogRead(joyV);

  stepper_L.setSpeed(elbow_speed);
 stepper_R.setSpeed(elbow_speed);
stepper_base.setSpeed(base_speed);

  if (potState > 800)
  {
    stepper_L.step(-elbow_steps);
    stepper_R.step (elbow_steps);

  }

  if (potState < 200) 
  {
    stepper_L.step(elbow_steps);
    stepper_R.step(-elbow_steps);
  }

  if (pot1State < 200)
  {
    stepper_base.step(-base_steps);
   // delay(5);
  }
  if (pot1State > 800) 
  {
    stepper_base.step(base_steps);
  }

arm_pos = myarm_servo.read();
wrist_pos = mywrist_servo.read();
gripper_pos = mygripper_servo.read();


X = analogRead(arm_pin);
Y = analogRead(wrist_pin);
Z = analogRead(gripper_pin);

if(X>800) {
  arm_pos = arm_pos + 1;
  myarm_servo.write(arm_pos);
  delay(15);
}
if(X<200) {
  arm_pos = arm_pos -1;
  myarm_servo.write(arm_pos);
  delay(15);
}

if(Y>800) {
  wrist_pos = wrist_pos * 1;
  mywrist_servo.write(wrist_pos);
  delay(15);
}
if(Y<200) {
  wrist_pos = wrist_pos -1;
  mywrist_servo.write(wrist_pos);
  delay(15);
}


if(Z>800) {
  gripper_pos = gripper_pos - 1;
  mygripper_servo.write(gripper_pos);
  delay(15);
}
if(Z<200) {
  gripper_pos = gripper_pos +1;
  mygripper_servo.write(gripper_pos);
delay(15);
}

}