The 4 Axis movable Arm with remote Control and Camera

//Kenan 08.01.2021

#include <Servo.h>  // add the servo libraries 
Servo myservo1;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;  

// -------- globale Varijable

//+++++  S E R V O ++++++++++++++++++++++
// start positio  for all servo pos1-4
int pos1=90, pos2=90, pos3=90, pos4=90;  
// define the variable of 4 servo angle,and assign the initial value (that is the boot posture angle value)

//the time required for a new position
int t= 10;

// it is necessary to define the working range for all servos 
//currently: 4.K 10 - 110; 3.L 10 -130;  2.D  60 - 180;  1.C 0 -180 
const int End1H = 180, End1L =0;
const int End2H = 180, End2L =25;
const int End3H = 130, End3L =20;
const int End4H = 110, End4L = 10;

//------------ JOYSTICK -------
const int right_X = A2; // define the right X pin to A2 
const int right_Y = A5; // define the right Y pin to A5
const int right_key = 7; // define the right key pin to 7that is the value of Z1

const int left_X = A3; // define the left X pin to A3
const int left_Y = A4;  // define the left X pin to A4
const int left_key = 8; //define the left key pin to 8that is the value of Z2

int x1,y1,z1;  // define the variable, used to save the joystick value it read.
int x2,y2,z2;

// Memory 
int s1,s2,s3,s4;
int memPo1[20];  // define 4 array, separately used to save the angle of four servo.
int memPo2[20];  //array length is 10namely can save angle data of 0~20 servo 
int memPo3[20];  // if need to save more data, just change the number 20 to be more larger number.
int memPo4[20];
int i=0; // for loop
int j=0; // save the last value of i


//---------- BT value Bluetooth comander
bool Bx1L = LOW;
bool Bx1H = LOW;
bool Bx2L = LOW;
bool Bx2H = LOW;
bool By1L = LOW;
bool By1H = LOW;
bool By2L = LOW;
bool By2H= LOW;
//+++++
// define IO pin 
//+++++++++ S O N O R +++++++++++++++++++
int Echo = A0;  // ultrasonic module   ECHO to A0
int Trig = A1;  // ultrasonic module   TRIG to A1
// int rightDistance = 0, leftDistance = 0, middleDistance = 0;
//+++++++++++++ Motor Pins +++++++++++
#define EN 5
#define IN1 3
#define IN2 4
#define IN3 12
#define IN4 11
#define LED 13
bool state = LOW;   //set initial state
char getstr;
// set initial car speed 130
int carSpeed = 130; //set initial car speed

//*************Obstacle_Avoidance_Car
int oaCar=0; //inaktiv - Obstacle_Avoidance_Car 

//++++++++++++++++++++++++
void setup() 
{ 
  Serial.begin(9600); //  set the baud rate to 9600
  stop();
  stp();
  pinMode(LED, OUTPUT); //before useing io pin, pin mode must be set first 
  pinMode(IN1,OUTPUT); //before useing io pin, pin mode must be set first 
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(EN,OUTPUT);
  
  // pinMode S o n o r
  pinMode(Echo, INPUT);  //Echo = A0; ultrasonic module   ECHO to A0
  pinMode(Trig, OUTPUT);  //Trig = A1; ultrasonic module   TRIG to A1
    //******************SERVO**************************
 // boot posture 
  myservo1.attach(2);  // set the control pin of servo 1 to 2
  myservo2.attach(10);  // set the control pin of servo 2 to 10
  myservo3.attach(6);   //set the control pin of servo 3 to 6
  myservo4.attach(9);   // set the control pin of servo 4 to 9

  myservo1.write(pos1);  
  delay(1000);
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);
  delay(1500);

  pinMode(right_key, INPUT_PULLUP);   // set the right/left key to INPUT
  pinMode(left_key, INPUT_PULLUP);    // with pullup resistor
  
  pinMode(LED, OUTPUT); //before useing io pin, pin mode must be set first 
 }


//++++++++++++++++++++++++++
void loop() 
{ 
  x1 = analogRead(right_X); //read the right X value 
  y1 = analogRead(right_Y);  // read the right Y value 
  z1 = digitalRead(right_key);  //// read the right Z value 
  
  x2 = analogRead(left_X);  //read the left X value
  y2 = analogRead(left_Y);  //read the left Y value 
  z2 = digitalRead(left_key);  // read the left Z value  
  
  // The "Claw" or "Gripper" works by opening or closing to "grab things."
  claw();
  // The "Base", or "Waist", usually can turn the arm 180 
  rotacija();
  // The "Elbow" -upper arm will make the arm "go forward or backward".
  desni();
  //The "Shoulder" -lower arm , is the responsible for "raising or lowering" the arm vertically
  lijevi();
  
   // Bluetooth control
  if(Serial.available())   // if receive the data
  {
    getstr=Serial.read();    // read the received data
      Serial.print(getstr);
      if (state == LOW)
      {
  switch(getstr){
    case 'A': forward(); break; // execute the corresponding function when receive the value 
    case 'D': back();   break;
    case 'B': left();   break;
    case 'C': right();  break;
    case 'E': stop();   break;
    case '5': stateChange(); break;  //set new state
    case '1': accPlus(); break; //change the speed "SPEED UP"
    case '2': oaCar=1; break; //Obstacle_Avoidance_Car
    case '3': park(); break; // Park function set all servo to position 90
    case '4': accMin(); break; // reduce the speed "SLOW DOWN"
    default:  break;
     }
  }
   else    //if (state == HIGH)
      {
        switch(getstr){
    case 'A': stp(); Bx1H = HIGH; break; // execute the corresponding function when receive the value 
    case 'D': stp(); Bx1L = HIGH;   break; //set new
    case 'B': stp(); By1H = HIGH;   break;  //set new
    case 'C': stp(); By1L = HIGH;  break;  //set new
    case 'E': stp();   break;  // stop - set all LOW
    case '5': stateChange(); break;  //set new state
    case '1': stp(); Bx2H = HIGH; break; //set new
    case '2': stp(); By2H = HIGH; break; //set new
    case '3': stp(); By2L = HIGH; break; //set new
    case '4': stp(); Bx2L = HIGH; break; //set new
    default:  break;
  }
      }
  }
 if(oaCar==1) oaMiniCar();
 
 //------ P a r k  and  M e m o r y ------------------------------------------
 // Park with Z1 AND Z2
   if (z1 == LOW) // if the left key is pressed
  {
    delay(200);
    if(z2==LOW) park(); // if the both keys are pressed
  if (z1 == LOW) memAktiv();//judge again if the right key is pressed -Memori funktion is aktiv 
    }
    
if (z2 == LOW) // if the left key is pressed
  {
    delay(200);
    if(z1==LOW) park(); // if the both keys are pressed - judge again if the right key is pressed
  if (z2 == LOW) memDone(); //judge again 
  //if the right key is pressed -
  //make the servo operate a posture saved in the variable 
    }
}

// *********** SUBROUTINE and Methoden
void stp() {
Bx1L = LOW;
Bx1H = LOW;
Bx2L = LOW;
Bx2H = LOW;
By1L = LOW;
By1H = LOW;
By2L = LOW;
By2H= LOW;
}
//---------
//-----------------  S E R V O -------------------------

// "Claw" or "Gripper" 
void claw()
{
    //claw y2=0 if push the left joystick to the right,
  if(y2<200||By2L==HIGH) // if push the left joystick to the right
                          //ODER By2L = 1 get value 3 over Bluetooth
  {
      pos4=pos4-2;  //current angle of servo 4 subtracts 4change the value you subtract, thus change the closed speed of claw
      //Serial.println(pos4)
           if(pos4<End4L) pos4=End4L;  // if pos4 value subtracts to ENDL4, the claw in 10 degrees we have tested is closed.
                                  //should change the value based on the fact
           // stop subtraction when reduce to ENDL4
      myservo4.write(pos4);  // servo 4 operates the motion, the claw gradually OPENS to pos4=10.
      if (y2>50)  delay(5*t);  // Second gear by Joystick and default for BT
      delay(t);
   }
  //claw y2=0 if push the left joystick to the left, **pos4 is 110- CLOSED 
  if(y2>800||By2H==HIGH) // if push the left joystick to the left ODER get value 3 over Bluetooth
  {
      pos4=pos4+1; // current angle of servo 4 plus 2/1change the value you plus, thus change the open speed of claw
      if(pos4>End4H) pos4=End4H; // limit the largest angle when open the claw  
      myservo4.write(pos4); // servo 4 operates the action, claw is gradually closed.
       if (y2<1000)  delay(5*t);  //Second gear by Joystick and default for BT
      delay(t);
  }
}
 // turn  "Base", or "Waist", 
  void rotacija()
{
  // turn R: y1=0 if push the right joystick to the right, **pos1 is 0 
  if(y1<200||By1L==HIGH)  // if push the right joystick to the right
  {
    pos1=pos1-1;  //pos1 subtracts 1
    if(pos1<End1L) pos1=End1L;  // limit the angle when turn right
    myservo1.write(pos1);  // servo 1 operates the motion, the arm turns right.
    if (y1>50)  delay(3*t);  // Second gear by Joystick and default for BT 
    delay(t);
    
     }
  // turn L: y1=1024 if push the left joystick to the lefft **pos1 is 180
  if(y1>800||By1H==HIGH)  // if push the right joystick to the left
  {
    pos1=pos1+1;  //pos1 plus 1
    if(pos1>End1H) pos1=End1H; // limit the angle when turn left 
    myservo1.write(pos1);  // arm turns left 
    if (y1<1000)  delay(3*t);  // Second gear by Joystick and default for BT
    delay(t);
    }
}


//The "Shoulder" upper arm
void desni()
{
  if(x1<200||Bx1H==HIGH)  // if push the right joystick upward, 
              //upper arm will go up
  {
    pos2=pos2+1;  // pos2 go up to max 180
    if(pos2>End2H) pos2=End2H;  // limit the lifting angle
    myservo2.write(pos2);   // the upper arm will lift
    if (x1>50)  delay(5*t);  // Second gear by Joystick 
    delay(t);     //First gear by Joystick and default gear for BT
    }
  
  //upper arm will go down  
      if(x1>800||Bx1L==HIGH)     //if push the right joystick downward, pos2 go to min ENDL 60
  {
    pos2=pos2-1;
     if(pos2<End2L) pos2=End2L; // limit the angle when go down
    myservo2.write(pos2);  // upper arm will go down 
    if (x1<1000)  delay(5*t);  // Second gear by Joystick and default for BT
    delay(t);         //First gear by Joystick and default gear for BT
   
      }
  
}


// lower arm The "Elbow" 
void lijevi()
{
    if(x2<200||Bx2H==HIGH)              // if push the left joystick upward 
  {
    pos3=pos3+1;           //pos3 grow up to 130
    if(pos3>End3H) pos3=End3H;  // limit the stretched angle- pos3 limited
    myservo3.write(pos3);  // lower arm will stretch out 
    if (x2>50)  delay(5*t);  // Second gear by Joystick and default for BT
    delay(t);
    
      }
  if(x2>800||Bx2L==HIGH)     // if push the left joystick downward
  {
    pos3=pos3-1;  // pos put down to min ENDL=10
    if(pos3<End3L)  pos3=End3L; // limit the retracted angle 
    myservo3.write(pos3);  // lower arm will draw back
    if (x2<1000)  delay(5*t);  // Second gear by Joystick and default for BT
    delay(t);
    
     }
}
//********************* m i n i C a r ***********************
void forward(){ 
  analogWrite(EN,carSpeed);
  // analogWrite(ENB,carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Forward");
}

void back(){
  analogWrite(EN,carSpeed);
  // analogWrite(ENB,carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  //Serial.println("Back");
}
//********************************************
void left(){
  analogWrite(EN,carSpeed);
  // analogWrite(ENB,carSpeed);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH); 
  //Serial.println("Left");
}

void right(){
  analogWrite(EN,carSpeed);
  // analogWrite(ENB,carSpeed);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  //Serial.println("Right");
}
//********************************************
void stop(){
  digitalWrite(EN,LOW); //Motor is off 
  // digitalWrite(ENB,LOW);
  //Serial.println("Stop!");
  oaCar=0;
  
}
//******************** M O D - S t a t e ************************
void stateChange(){
  state = !state;   //state variable changed STATUS
  // ******* Attention: Attention: Signalization whether miniCar or RobotArm controlled as well as the program recording
  digitalWrite(LED, state);
   delay(1000);
  //Serial.println("Light");  
}
//******************acceleration- carSpeed +/- "SLOW DOWN" or "SPEED UP"
//******************
// set car speed as "+" or "-" 90, 100, 110, 120, 130 and 170, 210, 250
void accPlus()
{//"SPEED UP"
  int da;
  if (carSpeed<130) da=10;
  else da=40;
carSpeed = carSpeed + da;
if (carSpeed >= 250) carSpeed=250;
}

void accMin()
{//"SLOW DOWN"
  int da;
  if (carSpeed<130) da=10;
  else da=40;
carSpeed = carSpeed - da;
if (carSpeed <= 90) carSpeed=90;
}

//*************Obstacle_Avoidance_Car
void oaMiniCar(){
  mov2(90);
  mov3(90);
  int distance=Ultrasonic_Ranging();  
  Serial.print("distance=");
  Serial.println(distance);
   if(distance<30&&distance>2){  //distance<30cm
    if(distance<18){  //if distance<18, go back
      back();
      delay(360);
    }
    else{ //18<distance<30turn right
     right();
      delay(360);
    }
  }
  else{   //distance>30, go forward
    forward();
  } 
    
}

//Ultrasonic distance measurement Sub function
int Ultrasonic_Ranging(){  //function of Ultrasonic Ranging
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(10);  //send least 20/10us high level to trigger ultrasonic waves to trig pin
  digitalWrite(Trig, LOW);    
  float distance = pulseIn(Echo, HIGH);  // reading the duration of high level
  //int distance = pulseIn(Echo, HIGH);
  distance = distance/58;   // Transform pulse time to distance     
  delay(50); 
  return distance;    //return distance to function
}
//********************************************
// *****************Metod and Funktion ************************

void mov1(int p)
{ 
    // base servo motor moves into position p (or  90)
  
    if (pos1<p)
  {
 for(pos1;pos1<p;pos1++)
  {
    myservo1.write(pos1);
    delay(t);      // delay 20/5ms"t" is used to adjust the servo speed
   }
  }
  else
  {
    for(pos1;pos1>p;pos1--)
  {
    myservo1.write(pos1);
    delay(t);      // delay t=20/5msused to adjust the servo speed
 
  }
    }
  }

void mov2(int p)
{ 
    // right servo motor moves into position p (or  90)
  if(p<End2L) p=End2L;  // if pos4 value subtracts to ENDL4,
  if(p>End2H) p=End2H; // limit the largest angle when open the claw
  //for servo1 End1H=, End1L=
    if (pos2<p)
  {
 for(pos2;pos2<p;pos2++)
  {
    myservo2.write(pos2);
    delay(t);      // delay 20/5msused to adjust the servo speed
   }
  }
  else
  {
    for(pos2;pos2>p;pos2--)
  {
    myservo2.write(pos2);
    delay(t);      // delay 20/5msused to adjust the servo speed
 
  }
    }
  }

void mov3(int p)
{ // left servo motor moves into position p (or  90)
  if(p<End3L) p=End3L;  // if pos4 value subtracts to ENDL4,
  if(p>End3H) p=End3H; // limit the largest angle when open the claw
  //for servo1 End1H=, End1L=
    if (pos3<p)
  {
 for(pos3;pos3<p;pos3++)
  {
    myservo3.write(pos3);
    delay(t);      // delay 20/5msused to adjust the servo speed
   }
  }
  else
  {
    for(pos3;pos3>p;pos3--)
  {
    myservo3.write(pos3);
    delay(t);      // delay 20/5msused to adjust the servo speed
 
  }
    }
  }

void mov4(int p)
{  //servo motor 4 moves into position p (or  90) 
  if(p<End4L) p=End4L;  // if pos4 value subtracts to ENDL4,
  if(p>End4H) p=End4H; // limit the largest angle when open the claw  
    
    if (pos4<p)
  {
 for(pos4;pos4<p;pos4++)
  {
    myservo4.write(pos4);
    delay(t);      // delay 20/5msused to adjust the servo speed
   }
  }
  else
  {
    for(pos4;pos4>p;pos4--)
  {
    myservo4.write(pos4);
    delay(t);      // delay 20/5msused to adjust the servo speed
 
  }
    }
  }
// funktion "Park" Park function set all servo to position 90
void park()
{ 
    // all servo motori in safety position 90
  sgn(100,2);
  mov1(90);
  //delay(500);
    mov2(90);
  //delay(500);
   mov3(90);
  //delay(500);
   mov4(90);
  sgn(500,6);
  Serial.println("safety position");
}
//----------- Memori -----------------------
void memAktiv()
{
      s1=myservo1.read();    // read the angle value of each servo  
      delay(100);
      Serial.println(s1);
      s2=myservo2.read();
      delay(100);
      Serial.println(s2);
      s3=myservo3.read();
      delay(100);
      Serial.println(s3);
      s4=myservo4.read();
      delay(100);
      Serial.println(s4);  
      memPo1[i]=s1;   // Save the read servo value to the array sequentially
      memPo2[i]=s2;
      memPo3[i]=s3;
      memPo4[i]=s4;
      i++;     //i value plus 1
      j=i;     // assign the last value of i to j
      sgn(700,1);//1 x 700 ms signal 
      //delay(100);
      Serial.println(i);   // on the serial monitor, print out the value i
    }

void memDone()
{
      i=0;  // assign i to 0prepare for the next memory cyklus
      pos1 = myservo1.read();    // actuel angle value of 4 servo posture
      pos2 = myservo2.read();
      pos3 = myservo3.read();
      pos4 = myservo4.read();
      sgn(30,2);
       //  while(state==1)  //loop, make the arm repeat the action.
     // {  all folow code   } That is, when robot arm performs 
     //all the memorized actions, it will not stop,
     // and continue to repeat those actions.
      MEM:
       if(Serial.available())   // if receive the data
      {
    getstr=Serial.read();    // read the received data MOD
      //Serial.print(getstr);
      if (getstr=='5') stateChange();
      }
      for(int k=0;k<j;k++)   // loop for j times, perform all actions saved.
      {
      //servo 1-4 performs the action
        mov1(memPo1[k]); //repeat j times, execute all saved motions 
        mov2(memPo2[k]);
        mov3(memPo3[k]);
        mov4(memPo4[k]);
        sgn(30,1); // signalisation 2 x 30 ms
       }
       if (state == HIGH) goto MEM;  // tests whether "state" is HIGH and
        // do  loop
}

  void sgn(int p, int m)
  {
     // signalisation  -pause p (=50 or 700), -repeat  m (=2 or 4) 
      int cadenc=0;
      while (cadenc < m) // tests if less than m  
       {  
      stateChange();
      delay(p);
      stateChange();// flashes
      delay(p);  
      cadenc++;          // increases variable by 1  
      }  
      }