MonkeyBot - 3 Servo Climbing Robot

Have added new video of Version 2 of MonkeyBot that uses standard size robot servos and aluminum brackets.

Version 1 uses microservos and wooden arms but does not have enough power to lift it's own power packs so it is tethered.

I wanted to build a proof of concept robot using only 3 servo that would climb a series of pegs using two "arms". It is constructed of the following items:

3 micro servos

fishing pole sections for the arms

big paper clips (not standard size) for the "hands" or grippers

Arduino Nano

zip ties, epoxy, and hot glue

6 volt AA battery pack for the servos

5 long screws for the climbing pegs and a board (in my case a bamboo cutting board)

The robot works well but is tethered to the power supplies as the micro servos are not powerful enough to lift the battery packs that would be required for untethered use. Possibly I will try to make a version 2 using standard sized servos that would also carry the Nano and battery packs.

Note the hands or grippers are opposed to each other. I tried then facing each other but it would not work as sometimes you need the arms to force against each other to move correctly and they would slip off the pegs. Also you need to keep the voltage as high as possible (6v) for the servos or you will end up programming in movements that wont work at different voltages as the voltage drops over time. I used AAA batteries at first but they drained too quickly because the servos have to do a lot of work lifting the robot and also at times pushing against each other and the pegs. AA batteries worked pretty well.

Kind of an interesting project that has no obvious use except to find out if it can be done.

Programming was rather tedious. The video shows two programming efforts: the first I programmed each servo to move separately and this resulted in a rather jerky moving robot. The second effort I used an interpolated routine to basically move all the servos at the same time and this was decidedly less jerky though could be done much smoother.

If the pegs are at different positions like in my project, you have to program for each movement to each peg. If the pegs were equi-distance and regular then the programming would be much simpler.

The grippers are just bent large paper clips, zipped tied to the arm and then hotglued to keep them from moving. Same with the servo arms. Also zipped tied and hotglued servos to larger arm sections.

The schematic shows how I wired the servos, 6v power supply and Nano. Just be sure to ground 6v to the Nano.

Here is the program for the interpolated servos. Interpolated means that I move all the servos at close to the same time and same relative distance. This results in a smoother moving arm rather than moving each servo separately. Also the coding is easier and faster.

#include

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

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

}