# Scara Arm for the Tower of Hanoi

2,504

15

## Introduction: 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.

Ourcommunity: openlab.makeblock.com

## Step 5: Wiring

Scara first servo <-> Port3 Slot1

Scara second servo <-> Port3 Slot2

Gripper Servo <-> Port6 Slot2

Lifting Servo <-> Port6 Slot1

Pan Servo <-> Port4 Slot1

more

## Step 6: Coding

#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;

delay(5000);

}

void loop() {

if (Serial.available()) {

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")) {

}

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];

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){

}

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;

}

## Step 7:

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

Participated in the
Circuits Contest 2016

270 30K
65 4.3K
400 30K
13 2.4K