Arduino sketch:

#include <Servo.h>

Servo myservoLeft,myservoRight, myservoCenter,myservoWeight; // create servo object to control a servo

int servoRead(int servoNumber) {

int servoCurrent;

if (servoNumber== 3) { servoCurrent = myservoLeft.read(); }

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

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

return servoCurrent;

}

void servoWrite(int servoNumber, int offset) {

if (servoNumber==3) { myservoLeft.write(offset); }

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

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

}

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

int curAngle;

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

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

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

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

if (curAngle < newAngle) {

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

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

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

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

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

delay(incDelay); }

}

else if (curAngle > newAngle) {

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

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

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

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

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

delay(incDelay); }

}

}

void interpolate3Servos( int servo3,int servo3Position,

int servo4,int servo4Position,

int servo5,int servo5Position,

int numberSteps,int timeDelay){

int servo3Current,servo4Current,servo5Current;

servo3Current = servoRead(servo3);

servo4Current = servoRead(servo4);

servo5Current = servoRead(servo5);

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.println(" ");

int cOffset = (servo3Position - servo3Current); cOffset = abs(cOffset)/numberSteps;

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

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

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

cOffsetTotal = servo3Current;

dOffsetTotal = servo4Current;

eOffsetTotal = servo5Current;

for (int x=0; x<numberSteps;x++) {

if (servo3Position > servo3Current) { cOffsetTotal = cOffsetTotal + cOffset; }

else { cOffsetTotal = cOffsetTotal - cOffset; }

if (servo4Position > servo4Current) { dOffsetTotal = dOffsetTotal + dOffset; }

else { dOffsetTotal = dOffsetTotal - dOffset; }

if (servo5Position > servo5Current) { eOffsetTotal = eOffsetTotal + eOffset; }

else { eOffsetTotal = eOffsetTotal - eOffset; }

if (servo3Position != servo3Current) servoWrite(servo3,cOffsetTotal);

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

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

// 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 (servo3Position != servo3Current) servoWrite(servo3,servo3Position);

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

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

}

void leftArm() {

interpolate3Servos( 3,170,4,140,5,150,20,50); //

delay(500);

interpolate3Servos( 3,90,4,120,5,150,20,50); // shift monkey right

delay(500);

myServo(90,1,1,2); // let go of left hook

delay(500);

interpolate3Servos( 3,40,4,100,5,70,20,50); //

delay(500);

interpolate3Servos( 3,20,4,90,5,20,20,50); //

delay(1000);

interpolate3Servos( 3,20,4,90,5,10,20,50); // extend left arm up

delay(1000);

myServo(120,1,1,2); // grab left hook

delay(1000);

interpolate3Servos( 3,20,4,110,5,70,20,50); // shift monkey left

delay(500);

interpolate3Servos( 3,30,4,90,5,50,20,50); // let loose right arm

delay(500);

interpolate3Servos( 3,30,4,80,5,55,20,50); // let loose right arm

delay(1000);

interpolate3Servos( 3,130,4,85,5,150,10,50); // pull right arm up to left peg

delay(1000);

interpolate3Servos( 3,150,4,160,5,150,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,130,4,140,5,140,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,120,4,130,5,140,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,110,4,110,5,150,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,110,4,90,5,150,20,50); // reach right arm up to right peg

// last arm movement

interpolate3Servos( 3,80,4,100,5,70,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,70,4,90,5,10,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,40,4,140,5,0,20,50); // reach right arm up to right peg

delay(500);

interpolate3Servos( 3,40,4,140,5,100,20,50); // reach right arm up to right peg

delay(500);

delay(15000);

exit(0);

}

void setup()

{

Serial.begin(9600);

delay(100);

myservoLeft.attach(3); // attaches the servo on pin 3 to the servo object

myservoLeft.write(90);

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

myservoCenter.write(90);

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

myservoRight.write(90);

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

interpolate3Servos( 3,90,4,90,5,90,10,50); // neutral

delay(500);

}

void loop() {

leftArm();

//}

// delay(1000);

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

}