Kenan Paralija
Published © CC BY

The 4 Axis movable Arm with remote Control and Camera

A DIY Robot Arm built on 2WD Arduino car Kit with full remote and local control and built-in camera

AdvancedWork in progress4 hours100
The 4 Axis movable Arm with remote Control and Camera

Things used in this project

Hardware components

ELEGOO EL-KIT-012 UNO Project Smart Robot Car Kit V 3.0
ELEGOO EL-KIT-012 UNO Project Smart Robot Car Kit V 3.0
×1

Software apps and online services

Elegoo IDE Arduino

Hand tools and fabrication machines

Soldering Iron Kit, Weller XNT/THM Tips
Soldering Iron Kit, Weller XNT/THM Tips

Story

Read more

Schematics

circuit von RobotArm

The drawing shows the complete circuit von RobotArm

Code

DIY_Robot_Arm_Dino

C/C++
The program enables full local and remote control over the movement of the robot arm, as well as saving the executed steps of the arm. In conjunction with the corresponding app, it is also possible to transfer images to a mobile phone.
//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  
      }  
      }

Credits

Kenan Paralija

Kenan Paralija

3 projects • 1 follower
I started my career by designing analog and digital circuits and moved to telecommunication and finally web and IT technology.

Comments