# Lynxmotion AL5D Robot arm!

Hello,

I have finally ordered the Lynxmotion Robot Arm (it's been my dream for a while). I plan on controlling the arm with my Arduino Mega. I know how to control servos, however, I would like to construct a program were I can input x,y,z coordinates and have the robot arm travel to that location. I have no idea how to accomplish such a program, and I was wondering if anybody knew if this was even possible with an Arduino. If it is I suppose I would have to start with a formula that took into account each degree of freedom (4).

Any info/suggestions on the topic would be awesome!

I have no idea how to accomplish such a program, and I was wondering if anybody knew if this was even possible with an Arduino.

The arm that you link to can rotate about a fixed base. Beyond that, it has 4 other degrees of freedom.

Converting an x, y, z location to the appropriate angles for each of those 5 degrees of freedom is possible, although by no means trivial. Whether the Arduino has the power to do the computations really depends on how fast you need it to perform them, and on whether the gripper should move linearly from where it is to the new location (that is, whether any intermediate motions and positions are important). Avoiding obstructions between here and there may, or may not, be important. Whether the arm should "go home" then go to the new position, or whether it should go directly to the new position, as a pick and place robot would, matters, too.

In response to your question's the robot does not need to avoid objects, and I don't care if it goes back into the starting position once it has accomplished its task. Speed is not important.

I will read up on inverse kinematics.

Thanks!

I read about inverse kinematics and i was able to understand it, but I did not get the equations it used. To simplify things, would it be possible to start off using 3 servos and just use x,z coordinates? (the robot would not move left or right but face straight)

I have neve done anything like this and it does not help that I have not done physics or trig!

Any chance a moderator could move this to project guidance?

Thanks!

I contacted linxmoton by their forums and they sent me some code. They said to delete the input from the keyboard/remote controller and that I would be good to go. Well the problem is that I can not seem to understand the code.

``````#include <Servo.h>
#include <PS2X_lib.h>

//#define DEBUG

//comment to disable the Force Sensitive Resister on the gripper
//#define FSRG

//FSRG pin Must be analog!!
#define FSRG_pin A1

//Select which arm by uncommenting the corresponding line
//#define AL5A
//#define AL5B
#define AL5D

//uncomment for digital servos in the Shoulder and Elbow
//that use a range of 900ms to 2100ms
//#define DIGITAL_RANGE

#ifdef AL5A
const float A = 3.75;
const float B = 4.25;
#elif defined AL5B
const float A = 4.75;
const float B = 5.00;
#elif defined AL5D
const float A = 5.75;
const float B = 7.375;
#endif

//PS2 pins
#define DAT 6
#define CMD 7
#define ATT 8
#define CLK 9

//Arm Servo pins
#define Base_pin 2
#define Shoulder_pin 3
#define Elbow_pin 4
#define Wrist_pin 10
#define Gripper_pin 11
#define WristR_pin 12

//Onboard Speaker
#define Speaker_pin 5

const float rtod = 57.295779;

//Arm Speed Variables
float Speed = 1.0;
int sps = 3;

//Servo Objects
Servo Elb;
Servo Shldr;
Servo Wrist;
Servo Base;
Servo WristR;
Servo Gripper;

//Arm Current Pos
float X = 4;
float Y = 4;
int Z = 90;
int G = 90;
int WR = 90;
float WA = 0;

//Arm temp pos
float tmpx = 4;
float tmpy = 4;
int tmpz = 90;
int tmpg = 90;
int tmpwr = 90;
float tmpwa = 0;

//PS2X Variables
PS2X ps2x;
int error = 0;
byte type = 0;

boolean mode = true;

void setup()
{
#ifdef DEBUG
Serial.begin(115200);
#endif

error = ps2x.config_gamepad(CLK,CMD,ATT,DAT, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

#ifdef DEBUG
if(error == 0)
Serial.println("Found Controller, configured successful");

else if(error == 1)
Serial.println("No controller found");

else if(error == 2)
Serial.println("not accepting commands");

else if(error == 3)
Serial.println("refusing Pressures mode");

switch(type)
{
case 0:
Serial.println("Unknown");
break;
case 1:
Serial.println("DualShock");
break;
case 2:
Serial.println("GuitarHero");
break;
}
#endif

Base.attach(Base_pin);
Shldr.attach(Shoulder_pin);
Elb.attach(Elbow_pin);
Wrist.attach(Wrist_pin);
Gripper.attach(Gripper_pin);
WristR.attach(WristR_pin);
}

int Arm(float x, float y, float z, int g, float wa, int wr) //Here's all the Inverse Kinematics to control the arm
{
float M = sqrt((y*y)+(x*x));
if(M <= 0)
return 1;
float A1 = atan(y/x);
if(x <= 0)
return 1;
float A2 = acos((A*A-B*B+M*M)/((A*2)*M));
float Elbow = acos((A*A+B*B-M*M)/((A*2)*B));
float Shoulder = A1 + A2;
Elbow = Elbow * rtod;
Shoulder = Shoulder * rtod;
if((int)Elbow <= 0 || (int)Shoulder <= 0)
return 1;
float Wris = abs(wa - Elbow - Shoulder) - 90;
#ifdef DIGITAL_RANGE
Elb.writeMicroseconds(map(180 - Elbow, 0, 180, 900, 2100  ));
Shldr.writeMicroseconds(map(Shoulder, 0, 180, 900, 2100));
#else
Elb.write(180 - Elbow);
Shldr.write(Shoulder);
#endif
Wrist.write(180 - Wris);
Base.write(z);
WristR.write(wr);
#ifndef FSRG
Gripper.write(g);
#endif
Y = tmpy;
X = tmpx;
Z = tmpz;
WA = tmpwa;
#ifndef FSRG
G = tmpg;
#endif
WR = tmpwr;
return 0;
}

void loop()
{

int LSY = 128 - ps2x.Analog(PSS_LY);
int LSX = ps2x.Analog(PSS_LX) - 128;
int RSY = 128 - ps2x.Analog(PSS_RY);
int RSX = ps2x.Analog(PSS_RX) - 128;

tmpy = max(Y + RSY/1000.0*Speed, -1);

tmpx = max(X + RSX/1000.0*Speed, -0.5);

tmpwa = constrain(WA + LSY/100.0*Speed, 0, 180);

tmpz = constrain(Z + LSX/100.0*Speed, 0, 180);

if(ps2x.Button(PSB_R1))
{
#ifdef FSRG
{
break;
#ifdef DEBUG
Serial.print(" ");
#endif
delay(10);
}
#else
tmpg = min(G + 5*Speed, 170);
#endif
}
if(ps2x.Button(PSB_R2))
{
#ifdef FSRG
{

#ifdef DEBUG
#endif
delay(10);
}
#else
tmpg = max(G - 5*Speed, 10);
#endif
}

if(ps2x.ButtonPressed(PSB_BLUE))
{
pos -= pos % 10;
while(pos != 1500)
{
pos += ((pos - 1500 > 0)? -10 : 10);
Gripper.writeMicroseconds(pos);
delay(25);
}
tmpg = 90;
G = 90;
}

if(ps2x.ButtonPressed(PSB_GREEN))
{
pos -= pos % 10;
while(pos != 1500)
{
pos += ((pos - 1500 > 0)? -10 : 10);
WristR.writeMicroseconds(pos);
delay(25);
}
tmpwr = 90;
WR = 90;
}

if(ps2x.Button(PSB_L1))
tmpwr = max(WR + 2*Speed, 0);
else if(ps2x.Button(PSB_L2))
tmpwr = min(WR - 2*Speed, 180);

{
sps = min(sps + 1, 5);
tone(Speaker_pin, sps*500, 200);
}
{
sps = max(sps - 1, 1);
tone(Speaker_pin, sps*500, 200);
}

Speed = sps*0.20 + 0.60;

if(Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr))
{
#ifdef DEBUG
#endif
}

if(tmpx < 2 && tmpy < 2 && RSX < 0)
{
tmpy = tmpy + 0.05;
Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);
}
else if(tmpx < 1 && tmpy < 2 && RSY < 0)
{
tmpx = tmpx + 0.05;
Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);
}
delay(30);
}
``````

I will delete out what they said and see if it makes a little more sense! I will post the new code when i’m finished.

I was able to delete out the remote control/keyboard parts of the code and here is what i’m left with:

``````#include <Servo.h>

//#define DEBUG

//comment to disable the Force Sensitive Resister on the gripper
//#define FSRG

//FSRG pin Must be analog!!
#define FSRG_pin A1

//Select which arm by uncommenting the corresponding line
//#define AL5A
//#define AL5B
#define AL5D

//uncomment for digital servos in the Shoulder and Elbow
//that use a range of 900ms to 2100ms
//#define DIGITAL_RANGE

#ifdef AL5A
const float A = 3.75;
const float B = 4.25;
#elif defined AL5B
const float A = 4.75;
const float B = 5.00;
#elif defined AL5D
const float A = 5.75;
const float B = 7.375;
#endif

//Arm Servo pins
#define Base_pin 2
#define Shoulder_pin 3
#define Elbow_pin 4
#define Wrist_pin 10
#define Gripper_pin 11
#define WristR_pin 12

const float rtod = 57.295779;

//Arm Speed Variables
float Speed = 1.0;
int sps = 3;

//Servo Objects
Servo Elb;
Servo Shldr;
Servo Wrist;
Servo Base;
Servo WristR;
Servo Gripper;

//Arm Current Pos
float X = 4;
float Y = 4;
int Z = 90;
int G = 90;
int WR = 90;
float WA = 0;

//Arm temp pos
float tmpx = 4;
float tmpy = 4;
int tmpz = 90;
int tmpg = 90;
int tmpwr = 90;
float tmpwa = 0;

boolean mode = true;

void setup()
{
Serial.begin(115200);

Base.attach(Base_pin);
Shldr.attach(Shoulder_pin);
Elb.attach(Elbow_pin);
Wrist.attach(Wrist_pin);
Gripper.attach(Gripper_pin);
WristR.attach(WristR_pin);

}

int Arm(float x, float y, float z, int g, float wa, int wr) //Here's all the Inverse Kinematics to control the arm
{
float M = sqrt((y*y)+(x*x));
if(M <= 0)
return 1;
float A1 = atan(y/x);
if(x <= 0)
return 1;
float A2 = acos((A*A-B*B+M*M)/((A*2)*M));
float Elbow = acos((A*A+B*B-M*M)/((A*2)*B));
float Shoulder = A1 + A2;
Elbow = Elbow * rtod;
Shoulder = Shoulder * rtod;
if((int)Elbow <= 0 || (int)Shoulder <= 0)
return 1;
float Wris = abs(wa - Elbow - Shoulder) - 90;
#ifdef DIGITAL_RANGE
Elb.writeMicroseconds(map(180 - Elbow, 0, 180, 900, 2100  ));
Shldr.writeMicroseconds(map(Shoulder, 0, 180, 900, 2100));
#else
Elb.write(180 - Elbow);
Shldr.write(Shoulder);
#endif
Wrist.write(180 - Wris);
Base.write(z);
WristR.write(wr);
#ifndef FSRG
Gripper.write(g);
#endif
Y = tmpy;
X = tmpx;
Z = tmpz;
WA = tmpwa;
#ifndef FSRG
G = tmpg;
#endif
WR = tmpwr;
return 0;
}

void loop()
{
}
``````

What do I put in the loop to make it go into the position I want? After I can figure that out I will be able to start working with the arm!

My guess is that I type in something like this…

Arm(float 6, float 7, float 5, int 9, float12, int 13)

Would that be correct? Also It appears like the arm moves back to the starting position each time. Am I right about that?

Thanks!

It looks to me as if the Arm function will move the hand to the position and orientation you specify with the arguments, and then leave it there until you call Arm again. The arguments seem to be X, Y, Z coordinates, gripper position, wrist angle and wrist rotation. The wrist angle seems to be absolute (i.e. relative to horizontal) rather than relative to the angle of the arm. If it works, that will do more or less what you need and you just need to write the code to work out what position you want to move the arm to, and call Arm to move it there.

How do I call the arm? Is this how I call it to the correct position….

Arm(float 6, float 7, float 5, int 9, float12, int 13)

(I used random numbers for the example)

Thanks!