Demo: Pick and Place

Here is a very simple example of using the servo control function that was created early just to have the robot perform a set of preprogrammed tasks.

In order to get the positions I used the gui program to take it through the sequence. I recorded the positions and then entered the sequence into the arduino code.

In the future I will add a recording feature to the python code so that the user can use the gui to move the arm through positions and then just have it repeat the motions automatically.

Here is the Pick and Place code

For any other resources visit the Project Links Page

If you don't want the file you can copy and paste the code below

#include <Servo.h>  //arduino library
#include <math.h>   //standard c library

#define PI 3.141


Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo gripperServo;

int command;

struct jointAngle{
  int base;
  int shoulder;
  int elbow;
};

int servoSpeed = 15;

//+++++++++++++++FUNCTION DECLARATIONS+++++++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo);

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void setup()
{
  Serial.begin(9600);
  baseServo.attach(9);        // attaches the servo on pin 9 to the servo object
  shoulderServo.attach(10);
  elbowServo.attach(11);
  gripperServo.attach(6);

  Serial.setTimeout(50);      //ensures the the arduino does not read serial for too long
  Serial.println("started");
  baseServo.write(90);        //intial positions of servos
  shoulderServo.write(150);
  elbowServo.write(110);
}

//primary arduino loop
void loop()
{

    // intialize
    servoControl(106, servoSpeed, baseServo);
    servoControl(100, servoSpeed, shoulderServo);
    servoControl(90, servoSpeed, elbowServo);
    servoControl(5, servoSpeed, gripperServo);


    servoControl(106, servoSpeed, baseServo);
    servoControl(160, servoSpeed, elbowServo);
    servoControl(90, servoSpeed, elbowServo);
    servoControl(160, servoSpeed, elbowServo);

    servoControl(18, servoSpeed, shoulderServo);
    servoControl(60, servoSpeed, gripperServo);
    servoControl(49, servoSpeed, elbowServo) ;
    servoControl(5, servoSpeed, baseServo);
     servoControl(160, servoSpeed, elbowServo);
    servoControl(161, servoSpeed, shoulderServo);
    servoControl(160, servoSpeed, baseServo);

    //return and set down
    servoControl(95, servoSpeed, baseServo);
    servoControl(18, servoSpeed, shoulderServo);
    servoControl(160, servoSpeed, elbowServo);
    servoControl(5, servoSpeed, gripperServo);

}

//++++++++++++++++++++++++++++++FUNCTION DEFITNITIONS++++++++++++++++++++++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo){
  //thePos is the desired position the servo should be driven to
  // theSpeed is the delay between each increment of the servo position in milliseconds
  // theServo is the servo object that is to be controled.

  Serial.println("in servoControl");

  int startPos = theServo.read();        //read the current pos
  int newPos = startPos;

  //define where the pos is with respect to the command
  // if the current position is less that the actual move up
  if (startPos < thePos){
        while (newPos < (thePos - 5)){
          newPos = newPos + 1;
          theServo.write(newPos);
          delay(theSpeed);
        }  
  }

  else{
       while (newPos > (thePos + 5)){
         newPos = newPos - 1;
         theServo.write(newPos);
         delay(theSpeed);
       }  
  }
} // end of function