Robotic Arm With Servo Motors

Part of my PhD research at NYU-Poly involves predicting energy consumption in robotic systems, so I needed a robotic system to work with as a research platform to validate results.  So I convinced my group mates in my Mechatronics class in the Fall of 2011 that we should build a robotic arm.  I also wanted to use it as a project to learn motor control.  I had some high torque servo motors lying around from another project, but the control boards had been fried.  Since a servo motor is just a DC motor, some gears, and an integrated potentiometer, I could still use those if just the control board was fried.  So, I unscrewed the bottom cover, and used some desoldering braid to suck up the solder around the two motor terminals.

Then I was able to pull off the PCB.  However, the 3 wires from the potentiometer were still attached, so I cut those and ended up with something that looked like this:

I had also seen this post on Instructables by Chris Anderson on adding some potentiometers to a toy robotic arm to enable closed loop control.  Since I decided to work with these hacked servos, my potentiometers were already integrated, so I was set.  I want to share the process with you so you can start a little higher on the learning curve than I did.

By building a robotic arm, students will acquire assembly skills and be able to implement basic forward and inverse kinematics in simple Arduino code.

All of the parts for the arm itself were from Lynxmotion.com:

Part #    Name     Quantity     $total

HUB-08  Aluminum Tubing Connector Hub (pair)  2   $16.00
AT-04  Aluminum Tubing - 6.0"  2  $7.20
ASB-09  Aluminum "C" Servo Bracket with Ball Bearings Two Pack  1  $12.90
ASB-10  Aluminum Long "C" Servo Bracket with Ball Bearings Two Pack  1  $12.90
ASB-04  Aluminum Multi-Purpose Servo Bracket Two Pack  1  $11.95

Total: $60.95

You'll also want a base of wood or metal to screw the first servo motor and bracket down to. I found a scrap piece of aluminum u-channel in the basement machine shop at my school, but a 2x4 would work just as well.

I used an Arduino as the brains of the operation augmented by an Adafruit motor shield:

Vendor     Name   Quantity     $total
Adafruit     Arduino UNO     1     $29.95
Adafruit     Motor/Stepper/Servo Shield     1     $19.50
Sparkfun     Ribbon Cable - 10 wire (15ft)     1     $4.95
Sparkfun     Ribbon Crimp Connector - Breadboad Friendly (2x5, Female)     1     $1.90
Adafruit     1/2 size breadboard     1     $5.00

Total:     $61.30

These ribbon crimp connectors are pretty great - all you have to do is squish the ribbon cable with them, and little metal teeth pierce the wire insulation and make contact with the actual wire, so your connector is ready for the breadboard.

Strip and solder the ribbon cable to the motors. Each motor will need 5 wires: 3 for the potentiometer wires, and 2 for the motor terminals.  Use heat shrink (or electric or kapton tape) to insulate the connections and ensure they won't touch later on.

The servo brackets will come with fasteners you can use to screw one to the base and secure the servo motors in the brackets.  The C brackets come with bearings and fasteners as well. The servos I used came with thin white wheels the were easy to mount to the C brackets. I chose to use the regular C bracket for the base or "shoulder" motor, then a long C bracket for the "elbow" motor so I had a larger workspace at the end of the 2nd link. 

Mount the half size breadboard next to the shoulder motor, and the Arduino next to that. After soldering together the motor shield kit, stack that on top of the Arduino.

First, you'll need a couple libraries:
AFMotor - this library was written by Adafruit and makes using the motor shield a breeze. Read the associated documentation.
PID_V1 - this is an amazing library with equally amazing documentation.

Now, look through the code and make sure your robotic arm is wired correctly (pay attention to the potentiometer pins). I had a benchtop power supply wired to the motor shield so the shield was getting 6V at up to 3A, then the Arduino was just powered through the USB cable.  The two wires from each motor are wired to the M3 and M4 blocks on the motor shield.  Of the three potentiometer wires from each motor, one goes to ground, the other goes to 5V power on the Arduino, and the other is a signal wire that gets plugged into an analog input on the Arduino. There will be soldering no matter what setup you go with.

Now, copy and paste this fairly well commented code:

/*
Robot arm control using hacked servos
+y is vertical, +x is to the right
by Dustyn Roberts 2012-05
*/

#include <AFMotor.h>
#include <math.h>
#include <PID_v1.h>

// Instantiate both motors
AF_DCMotor shoulder(3); 
AF_DCMotor elbow(4);

// declare pins
int ElbowPin = A1;  // to potentiometer on elbow motor
int ShoulderPin = A0; // to potentiometer on shoulder motor

// INITIALIZE CONSTANTS
double Kp_Elbow = 20;    // this is the proportional gain
double Kp_Shoulder = 20;
double Ki_Elbow = 0.1;    // this is the integral gain
double Ki_Shoulder = 0.1;
double Kd_Elbow = 1.0;  // this is the derivative gain
double Kd_Shoulder = 0.75;

double Elbow_neg = 970;      // joint limits of robotic arm using right hand rule for sign
double Elbow_pos = 31;
double Shoulder_neg = 210;
double Shoulder_pos = 793;

const double a1 = 200;     // shoulder-to-elbow "bone" length (mm)
const double a2 = 220;     // elbow-to-wrist "bone" length (mm) - longer c bracket

double highY = 350; // line drawing targets
double lowY = 250;
double constantX = 200;

boolean elbowup = false; // true=elbow up, false=elbow down

// VARIABLES
double rawElbowAngle = 0.0;      // initialize all angles to 0
double rawShoulderAngle = 0.0;

double elbowAngle = 0.0;      // initialize all angles to 0
double shoulderAngle = 0.0;  

double theta1 = 0.0;  // target angles as determined through IK
double theta2 = 0.0;

double actualX = 0.0;
double actualY = 0.0;
double y = 0.0;

double c2 = 0.0; // is btwn -1 and 1
double s2 = 0.0;

double pwmTemp = 0.0;

double pwmElbow = 100.0;  // between 0 to 255
double pwmShoulder = 100.0;

//Syntax: PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID elbowPID(&elbowAngle, &pwmElbow, &theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, DIRECT);
PID shoulderPID(&shoulderAngle, &pwmShoulder, &theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, DIRECT);

void setup() { 
  Serial.begin(115200);           // set up Serial library

  elbowPID.SetSampleTime(5);  // (ms) how often new PID calc is performed, default is 1000
  shoulderPID.SetSampleTime(5);

  elbow.setSpeed(pwmElbow);     // set the speed to pwmElbow
  shoulder.setSpeed(pwmShoulder);     // set the speed to pwmElbow

  elbowPID.SetMode(AUTOMATIC);
  shoulderPID.SetMode(AUTOMATIC);

  elbowPID.SetOutputLimits(-255,255);
  shoulderPID.SetOutputLimits(-255,255);

  move_to_start();  //get to starting position
}

void loop() { 
  line_y();
}

void move_to_start() {
  do { 
    get_angles(constantX, lowY);
    drive_joints();  // drive joints until actual equals expected
  } 
  while( abs(theta1 - shoulderAngle) > 2 && abs(theta2 - elbowAngle) > 2 ); // bail when it's close
}

void line_y() {
  for(y = lowY; y < highY; y += .5) {  // draw straight line up
    get_angles(constantX,y);
    drive_joints();
  }
  for(y = highY; y > lowY; y -= .5) {  // draw straight line down
    get_angles(constantX, y);
    drive_joints();
  }
}

// Given theta1, theta2 solve for target(Px, Py) (forward kinematics)
void get_xy() {
  actualX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
  actualY = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
}

// Given target(Px, Py) solve for theta1, theta2 (inverse kinematics)
void get_angles(double Px, double Py) {
  // first find theta2 where c2 = cos(theta2) and s2 = sin(theta2)
  c2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); // is btwn -1 and 1

  if (elbowup == false) {
    s2 = sqrt(1 - pow(c2,2));  // sqrt can be + or -, and each corresponds to a different orientation
  }
  else if (elbowup == true) {
    s2 = -sqrt(1 - pow(c2,2));
  }
  theta2 = degrees(atan2(s2,c2));  // solves for the angle in degrees and places in correct quadrant

  // now find theta1 where c1 = cos(theta1) and s1 = sin(theta1)
  theta1 = degrees(atan2(-a2*s2*Px + (a1 + a2*c2)*Py, (a1 + a2*c2)*Px + a2*s2*Py));
}

void drive_joints() {
  // read the actual values from all the pots
  rawElbowAngle = analogRead(ElbowPin);
  rawShoulderAngle = analogRead(ShoulderPin);

  // constrain robot arm to ignore out of range values
  elbowAngle = constrain(rawElbowAngle, Elbow_pos, Elbow_neg);
  shoulderAngle = constrain(rawShoulderAngle, Shoulder_neg, Shoulder_pos);

  // now map the angles so they correspond correctly
  elbowAngle = map(elbowAngle, Elbow_neg, Elbow_pos, -110.0, 85.0); 
  shoulderAngle = map(shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0); 

  elbowPID.Compute();
  shoulderPID.Compute(); 

  // DRIVE ELBOW: CW is forward, CCW is backward
  pwmTemp = abs(pwmElbow);  
  elbow.setSpeed(pwmTemp);     // now use the new PID output to set the speed

  if (pwmElbow < 0) {
    elbow.run(FORWARD);      // turn it on  
  }
  else if ( pwmElbow > 0) {
    elbow.run(BACKWARD);      // turn it on 
  }
  else elbow.run(RELEASE);      // stopped

  // DRIVE SHOULDER: CCW is forward, CW is backward
  pwmTemp = abs(pwmShoulder);
  shoulder.setSpeed(pwmTemp);     // set the speed

  if (pwmShoulder < 0) {
    shoulder.run(BACKWARD);      // turn it on
  }
  else if (pwmShoulder > 0) {
    shoulder.run(FORWARD);      // turn it on
  }
  else  shoulder.run(RELEASE);      // stopped  

get_xy();
//serial print data here if desired
Serial.print(actualX);
Serial.print(" , ");
Serial.println(actualY);
}

Admittedly there is a LOT going on in that code.  The PID post I linked to describes motor control in general and PID control in great detail, so I won't do that here, but it's well worth the read.  Basically this PID control replaces the PCB that used to control your servo with a software version that is customizable.  You may have to tweak the PID gains depending on the performance of your bot.  The code also has functions for inverse kinematics (figuring out joint angles given the end effector position) and forward kinematics (figuring out the end effector position when given the joint angles). I only included a simple function that makes the robotic arm draw a short vertical line over and over.  You can also get a couple of potentiometers and popsicle sticks and make a master-slave type controller.  In one phase of this robot arm's life, it was a drawing robot, and I had a solenoid on the end of second link that pushed a marker onto a canvas or retracted it.  Have fun with it!