Robot Arm Throwing Paper Airplane (Arduino Servo Positioning)

This is a project to demonstrate an Arduino sketch that positions a robotic arm (of any number of servos) by using interpolation, that is moving each servo a little bit at a time so that they all seem to be moving at the same time.

This project is similar to my xyz positioning instructable except that is uses absolute servo angle positioning which is actually much simpler and it allows positioning the servo in places that inverse kinematic routines cannot reach.

No other hardware is required though I use a servo/sensor shield to attach my servos to the Arduino Uno. The shield does not require any custom library.

The function call is very simple: interpolate4Servos( 4,90,5,170,6,10,7,70,10,70);

The 4,5,6,7 are just the servo pin numbers, the number beside the pin number is the servo angle you want it to go to. The last two parameters are the number of steps and the servo speed.

It only takes a few function calls to make the robot arm do something interesting.

Try it, you'll like it.

If you use the sensor shield that I am using you need to do one thing to it: bend the pin that connects the 5v from the shield to the Arduino Uno out of the way so that it does not connect to the Uno board. You want to use the external voltage on the shield to power only your servos, not the Arduino Uno or it may destroy the Uno, I know as I burned up two Uno boards when my external voltage was 6 volts rather than 5. This allows you to use higher than 5v to power your servos but if your external voltage is higher than 5 volts then do not connect any 5 volt sensors to the shield or they will be fried.

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// created by Jim Demello, Shangluo University, 2017

// you are free to use, manipulate, do whatever you wish with this code, my name is not required

// This routine allows any number of servos to be interpolated, just add new lines if number of servos exceeds 4

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include servo.h

Servo myservoBase,myservoShoulder, myservoElbow,myservoWrist,myservoGripper; // create servo object to control a servo

int servoRead(int servoNumber) {

int servoCurrent;

if (servoNumber== 4) { servoCurrent = myservoBase.read(); }

if (servoNumber== 5) { servoCurrent = myservoShoulder.read(); }

if (servoNumber== 6) { servoCurrent = myservoElbow.read(); }

if (servoNumber== 7) { servoCurrent = myservoWrist.read(); }

return servoCurrent;

}

void servoWrite(int servoNumber, int offset) {

if (servoNumber==4) { myservoBase.write(offset); }

if (servoNumber==5) { myservoShoulder.write(offset); }

if (servoNumber==6) { myservoElbow.write(offset); }

if (servoNumber==7) { myservoWrist.write(offset); }

}

void myServo(int newAngle,int angleInc,int incDelay,int servoNum) {

int curAngle;

if (servoNum== 1) { curAngle = myservoBase.read(); }

if (servoNum== 2) { curAngle = myservoShoulder.read(); }

if (servoNum== 3) { curAngle = myservoElbow.read(); }

if (servoNum== 4) { curAngle = myservoWrist.read(); }

if (curAngle < newAngle) {

for(int angle=curAngle;angle < newAngle;angle += angleInc) {

if (servoNum == 1) myservoBase.write(angle);

if (servoNum == 2) myservoShoulder.write(angle);

if (servoNum == 3) myservoElbow.write(angle);

if (servoNum == 4) myservoWrist.write(angle);

delay(incDelay); }

}

else if (curAngle > newAngle) {

for(int angle=curAngle;angle > newAngle;angle -= angleInc) {

if (servoNum == 1) myservoBase.write(angle);

if (servoNum == 2) myservoShoulder.write(angle);

if (servoNum == 3) myservoElbow.write(angle);

if (servoNum == 4) myservoWrist.write(angle);

delay(incDelay); }

}

}

void interpolate4Servos( int servo4,int servo4Position,

int servo5,int servo5Position,

int servo6,int servo6Position,

int servo7,int servo7Position,

int numberSteps,int timeDelay){

int servo4Current,servo5Current,servo6Current,servo7Current;

servo4Current = servoRead(servo4);

servo5Current = servoRead(servo5);

servo6Current = servoRead(servo6);

servo7Current = servoRead(servo7);

// Serial.print("Servo3Pos and Current "); Serial.print(servo3Position); Serial.print(" "); Serial.println(servo3Current);

// Serial.print("Servo4Pos and Current "); Serial.print(servo4Position); Serial.print(" "); Serial.println(servo4Current);

// Serial.print("Servo5Pos and Current "); Serial.print(servo5Position); Serial.print(" "); Serial.println(servo5Current);

// Serial.print("Servo6Pos and Current "); Serial.print(servo6Position); Serial.print(" "); Serial.println(servo6Current);

// Serial.println(" ");

int cOffset = (servo4Position - servo4Current); cOffset = abs(cOffset)/numberSteps;

int dOffset = (servo5Position - servo5Current); dOffset = abs(dOffset)/numberSteps;

int eOffset = (servo6Position - servo6Current); eOffset = abs(eOffset)/numberSteps;

int fOffset = (servo7Position - servo7Current); fOffset = abs(fOffset)/numberSteps;

int cOffsetTotal=0,dOffsetTotal=0,eOffsetTotal=0,fOffsetTotal=0;

cOffsetTotal = servo4Current;

dOffsetTotal = servo5Current;

eOffsetTotal = servo6Current;

fOffsetTotal = servo7Current;

for (int x=0; x

if (servo4Position > servo4Current) { cOffsetTotal = cOffsetTotal + cOffset; }

else { cOffsetTotal = cOffsetTotal - cOffset; }

if (servo5Position > servo5Current) { dOffsetTotal = dOffsetTotal + dOffset; }

else { dOffsetTotal = dOffsetTotal - dOffset; }

if (servo6Position > servo6Current) { eOffsetTotal = eOffsetTotal + eOffset; }

else { eOffsetTotal = eOffsetTotal - eOffset; }

if (servo7Position > servo7Current) { fOffsetTotal = fOffsetTotal + fOffset; }

else { fOffsetTotal = fOffsetTotal - fOffset; }

if (servo4Position != servo4Current) servoWrite(servo4,cOffsetTotal);

if (servo5Position != servo5Current) servoWrite(servo5,dOffsetTotal);

if (servo6Position != servo6Current) servoWrite(servo6,eOffsetTotal);

if (servo7Position != servo7Current) servoWrite(servo7,fOffsetTotal);

// Serial.print(" a and b Offset "); Serial.print(aOffsetTotal); Serial.print(" ");Serial.println( bOffsetTotal); delay(10);

delay(timeDelay);

}// end for

//////////////////////////////////////

// take care of modulo remainders //

/////////////////////////////////////

if (servo4Position != servo4Current) servoWrite(servo4,servo4Position);

if (servo5Position != servo5Current) servoWrite(servo5,servo5Position);

if (servo6Position != servo6Current) servoWrite(servo6,servo6Position);

if (servo7Position != servo7Current) servoWrite(servo7,servo7Position);

}

void setup()

{

Serial.begin(9600);

delay(100);

myservoBase.attach(4); // attaches the servo on pin 4 to the servo object

myservoBase.write(90);

myservoShoulder.attach(5); // attaches the servo on pin 5 to the servo object

myservoShoulder.write(90);

myservoElbow.attach(6); // attaches the servo on pin 6 to the servo object

myservoElbow.write(90);

myservoWrist.attach(7); // attaches the servo on pin 7 to the servo object

myservoWrist.write(90);

myservoGripper.attach(9); // attaches the servo on pin 9 to the servo object

myservoGripper.write(90);

delay(1000); // waits for the servo to get there

interpolate4Servos( 4,90,5,90,6,90,7,0,10,40); // neutral

delay(500);

}

void loop() {

myservoGripper.write(90);// close gripper

delay(3000);

interpolate4Servos( 4,90,5,170,6,10,7,70,10,70); // pull back

delay(2000);

interpolate4Servos( 4,90,5,90,6,60,7,90,8,3); // forward throw

delay(380);

myservoGripper.write(50);// open gripper

delay(1);

interpolate4Servos( 4,90,5,90,6,90,7,90,10,60); // neutral

delay(500);

// delay(1000);

exit(0); //pause program - hit reset to continue

}