Scara Arm for the Tower of Hanoi

To create a scara arm with two big metal servos and to make its end move horizontally. Just think it over, what can we do with scara arm? As you see, the vertically moving scara arm can easily catch and move the Tower of Hanoi. I guess you must be surprised by its continuous and smooth actions, aren’t you? This is the scara robotic arm I built to solve the Towers of Hanoi puzzle. It uses two MEDS150 Servos, a mini gripper and a lifting platform with 9g Micro Servos. It provides a circular work envelope.

More tutorials please visit www.makeblock.com

Ourcommunity: openlab.makeblock.com

www.facebook.com/Makeblock

BUY NOW

·
·1 x Mini Pan - Tilt Kit

· ·2 x Robot Servo Pack-Blue

· ·3 x Beam0824-048-Blue (4-Pack)

· ·1 x Mini Gripper

· ·3 x Me RJ25 Adapter

· ·1 x Me Orion(Base on Arduino UNO)

· ·3 x 6P6C RJ25 cable-20cm(4-Pack)

· ·Socket Cap Screw M4x8-Button Head (50-Pack)

· ·Socket Cap Screw M4x14-Button Head (50-Pack)

· ·Nut 4mm(50-Pack)

· ·4 x Plastic Rivet R4100 (50-Pack)

· ·1 x Base Bracket B

· ·1 x Micro USB Cable

· ·1 x G Claw

buy now

VIEW MORE

Robotic Arm Learn more

Robotic Arm learn more

Scara first servo <-> Port3 Slot1

Scara second servo <-> Port3 Slot2

Gripper Servo <-> Port6 Slot2

Lifting Servo <-> Port6 Slot1

Pan Servo <-> Port4 Slot1

more

BUY NOW

To program your Arduino from your browser, install the codebender plugin or app. Learn more.

#define MeOrion_H

#include

#include

int value = 0;

String buffer = "";

double angle_rad = PI / 180.0;

double angle_deg = 180.0 / PI;

MePort armPort(3);

MePort clawPort(6);

MePort panPort(4);

Servo servo1;//inside

Servo servo2;//outside

Servo servoTilt;

Servo servoClaw;

Servo servoPan;

const double L1 = 100.0; //armLength,inside

const double L2 = 170.0; //armLength,outside

struct Angle

{

double x;//inside

double y; //outside

double z;

double c;

};

struct Queue{

int array[5];

int length;

};

void pushPoint(Angle p);

Angle pointToAngles(float x,float y,float z,float c);

void addPoint(float x, float y,int pen);

void push(Queue* queue, int value);

int pop(Queue* queue);

void parseBuffer();

float prevX;

float prevY;

float prevZ;

float prevC;

long lastTime = 0;

long runTime = 0;

int len = 0;

boolean isRunning = false;

Angle currentAngle;

Angle lastAngle;

Queue hanoi[3];

float degs = 180.0/PI;

int posY = 220;

int posW = 100;

int openClaw = 80;

int closeClaw[3] = {150,130,110};

int tilt[4] = {100,110,120,130};

void setup() {

Serial.begin(115200);

delay(100);

servo1.attach(armPort.pin1());

servo2.attach(armPort.pin2());

servoTilt.attach(clawPort.pin1());

servoClaw.attach(clawPort.pin2());

servoPan.attach(panPort.pin1());

servoTilt.write(tilt[3]);

servoPan.write(90);

hanoi[0].length = 0;

hanoi[2].length = 0;

hanoi[1].array[0] = 3;

hanoi[1].array[1] = 2;

hanoi[1].array[2] = 1;

hanoi[1].length = 3;

addPoints(0,220,20,100);

delay(5000);

}

void loop() {

if (Serial.available()) {

char c = Serial.read();

if (c == '\n') {

parseBuffer();

} else {

buffer += c;

}

}

translate(1,2,0);

delay(2000);

translate(2,0,1);

delay(2000);

translate(0,1,2);

delay(2000);

}

void parseBuffer() {

buffer = buffer + "/";

int count = 0;

int startIndex = 0;

int endIndex = 0;

int len = buffer.length();

if (len < 1) {

return;

}

String tmp;

String values[6];

while (true) {

startIndex = buffer.indexOf("/", endIndex);

endIndex = buffer.indexOf("/", startIndex + 1);

tmp = buffer.substring(startIndex + 1, endIndex);

values[count] = tmp;

count++;

if (endIndex == len - 1) break;

}

if (values[0].equals("pos")) {

addPoint(values[1].toInt(), values[2].toInt(),values[3].toInt(),values[4].toInt());

}

buffer = "";

}

void translate(int from,int to,int other){

moveTo(from,to);

moveTo(from,other);

moveTo(to,other);

moveTo(from,to);

moveTo(other,from);

moveTo(other,to);

moveTo(from,to);

}

void moveTo(int fromPos,int toPos){

float x1,y1,z1,x2,y2,z2,z3,c1,c2;

y1 = posY;

y2 = posY;

x1 = (fromPos-1)*posW;

x2 = (toPos-1)*posW;

z1 = tilt[3];

z2 = tilt[hanoi[fromPos].length-1];

z3 = tilt[hanoi[toPos].length];

c1 = openClaw;

c2 = closeClaw[hanoi[fromPos].array[hanoi[fromPos].length-1]-1];

addPoints(x1,y1,z1,c1);

addPoints(x1,y1,z2,c1);

addPoints(x1,y1,z2,c2);

addPoints(x1,y1,z1,c2);

addPoints(x2,y2,z1,c2);

addPoints(x2,y2,z3,c2);

addPoints(x2,y2,z3,c1);

addPoints(x2,y2,z1,c1);

push(&hanoi[toPos],pop(&hanoi[fromPos]));

}

void addPoints(float x,float y,float z,float c){

int len = 12;

for(int i=0;i

float tx = prevX+(x-prevX)/len*i;

float ty = prevY+(y-prevY)/len*i;

float tz = prevZ+(z-prevZ)/len*i;

float tc = prevC+(c-prevC)/len*i;

pushPoint(pointToAngles(tx,ty,tz,tc));

}

prevX = x;

prevY = y;

prevZ = z;

prevC = c;

}

void addPoint(float x, float y,float tilt,float claw)

{

servo1.write(x);

servo2.write(y);

servoTilt.write(tilt);

servoClaw.write(claw);

delay(50);

//Serial.println("ok");

}

void pushPoint(Angle p){

if(lastAngle.x!=p.x||lastAngle.y!=p.y||lastAngle.z!=p.z||lastAngle.c!=p.c){

addPoint(p.x,p.y,p.z,p.c);

}

lastAngle = p;

}

Angle pointToAngles(float x,float y,float z,float c){

long pow1 = L1*L1;

long pow2 = L2*L2;

long powR = x*x+y*y;

double R = sqrt(powR);

Angle a;

a.z = z;

a.c = c;

if(powR L1+L2){

a.x = 90;

a.y = 90;

return a;

}

int t = 1;

a.x = floor(((atan2(x,y)-acos((powR+pow1-pow2)/(2*R*L1)))*degs+90.0)*t)/t;

a.y = floor((acos((pow1+pow2-powR)/(2*L1*L2))-PI/2)*degs*t)/t;

return a;

}

void push(Queue* queue, int value){

if (queue->length >= 5)

return;

queue->array[queue->length] = value;

queue->length++;

}

int pop(Queue* queue){

if (queue->length <= 0)

return 0;

queue->length--;

int v = queue->array[queue->length];

queue->array[queue->length] = 0;

return v;

}

We have updated the project of tower of Hanoi for Scara Arm on our official purchase website (http://makeblock.com/activity/scara-pack-project) and the Our community:www.makeblock.com