Micro Robot Arm SG90 servos

My version of a much copied project.
https://www.youtube.com/watch?v=lsiVESW-GXY

Arduino NANO V3.0
4 x Servos  Tower Pro SG90
4 x Linear 10k Poteniometers
Battery 4 x 1.2Volt AA NiMh







Code to run on Arduino NANO

copy - paste into Arduino Sketch
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include <Servo.h>

const int PinButton1 = 2;  // pin 2   
const int PinButton2 = 3;  // pin 3
int mode = 1;     // case 1 program robot arm. case 2 replay positions 
int bounce = 0;
volatile int buttonPress = 0;
unsigned long lastButtonPressTime = 0;
volatile unsigned long bounceTime = 0;
Servo Arm0Servo;      //servo objects
Servo Arm1Servo;      // Ihave renumbered servos 26 1 2017
Servo Arm2Servo;
Servo Arm3Servo;
int PosArm[4] ;       // array 
int ArmPos[100][4];   // array to hold arm positions up to 100
int PosCount = 0;     //  to count number of positions increased when button pressed
int PosCountMax = 0;  //  number of positions recorded when double button push initiates replay mode
int PosReached = 0;   // used to check that arm has moved from one recorded position to next position
void setup() {  
     for(int i = 0; i <100 ; i++ ){
         for(int p = 0; p <4 ; p++ ){
             ArmPos[i][p] = -1;
         }
     }
    pinMode(PinButton1 , OUTPUT);
    digitalWrite(PinButton1 ,LOW);
    pinMode(PinButton2, INPUT_PULLUP);
    //  I have made a small change here due to problem using some Arduinos
    //attachInterrupt( digitalPinToInterrupt(PinButton2),ButtonPress , LOW );
    // digitalPinToInterrupt(PinButton2) may not be defined!
    attachInterrupt( 1,ButtonPress , LOW );   // interupt to capture button presses
    // attach servos to relervent pins on arduino nano
    Arm0Servo.attach(12); // grip    90 to 180 open    limits of servo movement
    Arm1Servo.attach(11); // elbow      to 130 up
    Arm2Servo.attach(10); // shoulder   10 to 50 down
    Arm3Servo.attach(9);  // turn    0  to 180 right 
}

void loop() {
  switch(mode){
    case 1 :  //  program robot arm. 1 press to remember position. 2 presses to progress next case 2 replay mode
              // analogRead(pin) that reads poteniometers on training arm
        PosArm[0] = map(analogRead(14),480,1024,180,90); // map (480,1024 value from potentiometer to 180,90 value sent to servo)
        PosArm[1] = map(analogRead(15),180,1000,130,10);
        PosArm[2] = map(analogRead(16),950,400,80,10);
        PosArm[3] = map(analogRead(17),0,1024,180,10);
        MoveArm();                          // call method  
        if(buttonPress == 1){               // flag set by interupt when button is pressed          
             buttonPress = 0;               // reset flag 
             if( millis() > (lastButtonPressTime + 1000)){    // only one button press in one secound
                 // record  position of arm PosArm to array[100][] of armpositions        
                 ArmPos[PosCount][0] = PosArm[0];
                 ArmPos[PosCount][1] = PosArm[1];
                 ArmPos[PosCount][2] = PosArm[2];
                 ArmPos[PosCount][3] = PosArm[3];
                 if( PosCount < 100) {      // stop recording if over 100 positions recorded (memory limitations)
                     PosCount++;         
                 }
             }else{                         //  more than one button press
                 mode = 2;                  // go to next phase
                 PosCountMax  = PosCount;   // set number of arm positions recorded
                 PosCount = 0;              // reset count ready to read  arm position array from begining 
             }
        lastButtonPressTime = millis();              
        }
      break;
    case 2 :        // read arm position array
        PosReached = 0;
        for(int i = 0; i <4 ; i++ ){   //adjust servo positions   
            // we move the servos in small steps and the delay(20)  makes arm motion smooth and slow
            if( PosArm[i] > ArmPos[PosCount][i]) PosArm[i] = PosArm[i] - 1;  // if actual position is greater than requird position reduce position by 1
            if( PosArm[i] < ArmPos[PosCount][i]) PosArm[i] = PosArm[i] + 1;  // if actual position is less than required position value increase position valuue by 1
            if( PosArm[i] == ArmPos[PosCount][i]) PosReached++;              // check if servo  has reached required position/angle
        }
        if(PosReached == 4) PosCount++;        // if all 4 servos have reached position  then increase array index (PosCount)to next position in array
        if(PosCount == PosCountMax) PosCount = 0;   //  if end of array reached reset index to 0 and repeat.
        MoveArm();   // physically move arm to updated position, this is broken into small steps
        delay(20);   // pause between moves so over all motion is slowed down
      break;
    default :
      break;
  }    
}

void MoveArm() {   // write arm position data to servos
  for (int i = 0 ; i < 4 ; i++) {
    if (PosArm[i] < 5) PosArm[i] = 5;  // limit servo movement to prevent hitting end stops
    if (PosArm[i] > 175) PosArm[i] = 175;  // servo.write limited 5 - 175  
  }
  Arm0Servo.write(PosArm[0]);
  Arm1Servo.write(PosArm[1]);
  Arm2Servo.write(PosArm[2]);
  Arm3Servo.write(PosArm[3]);
}
void ButtonPress(){   //  interupt to capture button press
    if(micros() > (bounceTime + 3000)){ // debounce timer 
        bounceTime = micros();          // ingnore interupts due to button bounce
        buttonPress = 1;     // flag for button pressed
    }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

15 comments:

  1. hello what kind of pot you used for the robotic arm,,

    ReplyDelete
  2. sir can i have a schematic for this for me to understand it easily..??
    and sir what did you use to teach and play mode it..?? thankyou

    ReplyDelete
  3. Good night, esquemático eletric projecto ?

    ReplyDelete
  4. Hi,very very good project,but is possible if you save the position of all step in one sd ??
    in this system,is possible have one program on the sd cards,and change the sd,change the program....
    how is possible that if you ??
    thanks and best regards

    ReplyDelete
  5. great information thank you for sharing. this is going to help me learn alot. nice web page as well!

    ReplyDelete
  6. Hi Peter,
    The circuit diagram implies the pots are connected to A0-A3, however the code at line 43-46 implies analogread(14)-(17). I am an awful coder, my 10 year old grandson is better than me, so where are the potpins declared, please, please?

    ReplyDelete
  7. Can you attach Lazer or claw on arm

    ReplyDelete
  8. Dear Peter, great job! I very like it!
    I making a servo arm with 6 micro servo (5 axes and a gripper) like yours. Do you know if i Can modify your program just adding 2 servo motors and 2 potentiometer? The Arduino memory is sufficient? Thanks in advance. Damiano

    ReplyDelete
  9. Hi Peter,
    Nice job. Your instructions are great, with the photos, code and the wiring schematic. One question, how long are each of the arms. They also appear to be about 2cm in diameter, correct?
    Dan

    ReplyDelete