Hi guys, following is the code for Arduino program to control a Lynxmotion AL5D manipulator using analog joysticks. Though it looks like a long code, it is pretty easy as it has nothing but identical functions repeating, only the name is changed. Copy-paste it in the Arduino IDE and make the connections as below (download and enlarge the image for better view):
Thanks
Rohit
// CODE STARTS FROM HERE
// Joystick control of lynxmotion AL5D manipulator
#include<Servo.h>
Servo servo1_base;
Servo servo2_shoulder;
Servo servo3_elbow;
Servo servo4_wrist;
Servo servo5_gripper;
int button1;
int button2;
//int base=2;
//int shoulder=10;
//int elbow=9;
//int wrist=11;
//int gripper=13;
float pos1;
float pos2;
float pos3;
float pos4;
float factor;
int center;
float val; // global variables
int increment_angle=1;
int angle=90;
void joystick_1_calibrateM()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int left=0;
int right=0;
int center=0;
here: // loops until left, right, and center are found
button1 = digitalRead(8);// the position is recorded when the joystick button is pressed
float val = analogRead(0); // joystick position input
if (button1==1)// if button is pressed
{
if (left==0)//if left not assigned ( move the joystick to the left and press the button.)
{
left=val; //asign the left value
Serial.print("joystick 1 left ");
Serial.println(left);
delay(300);// gives time to move release the button
}
else if (right==0)////if right not assigned ( move the joystick to the left and press the button.)
{
right=val; // same as above
Serial.print("joystick 1 right ");
Serial.println(right);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 1 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{goto here;}
// float
// range=(left-right)/2;// range of joystick
//
// factor=range/255;// corection factor
// Serial.println(factor);
}
void joystick_1_calibrateS()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int top=0;
int bottom=0;
int center=0;
here: // loops until left, right, and center are found
button1 = digitalRead(8);// the position is recorded when the joystick button is pressed
float val = analogRead(2); // joystick position input
if (button1==1)// if button is pressed
{
if (top==0)//if left not assigned ( move the joystick to the left and press the button.)
{
top=val; //asign the left value
Serial.print("joystick 1 top ");
Serial.println(top);
delay(300);// gives time to move release the button
}
else if (bottom==0)////if right not assigned ( move the joystick to the left and press the button.)
{
bottom=val; // same as above
Serial.print("joystick 1 bottom ");
Serial.println(bottom);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 1 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{
goto here;
}
}
void joystick_1_joyM() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(0); // joystick position input
//pos=2.17*val-1116.44; // calculates the position in the desired escale
pos1=0.857*val-352.2857;
if (pos1>180)// if above range, set to max value
{pos1=180;}
if(pos1<0)// if below range, set to min value
{pos1=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
Serial.print("joystick 1 Axis 1 position is ");
Serial.println(pos1);
// delay(1500);
}
void joystick_1_joyS() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(2); // joystick position input
// Serial.print("val");
// Serial.println(val);
pos2=0.857*val-352.2857; // calculates the position in the desired escale
if (pos2>180)// if above range, set to max value
{pos2=180;}
if(pos2<0)// if below range, set to min value
{pos2=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
// Serial.print("joystick position is ");
// Serial.println(pos);
Serial.print("joystick 1 Axis 2 position is ");
Serial.println(pos2);
//delay(1500);
}
void joystick_2_calibrateM()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int left=0;
int right=0;
int center=0;
here: // loops until left, right, and center are found
button2 = digitalRead(4);// the position is recorded when the joystick button is pressed
float val = analogRead(3); // joystick position input
if (button2==1)// if button is pressed
{
if (left==0)//if left not assigned ( move the joystick to the left and press the button.)
{
left=val; //asign the left value
Serial.print("joystick 2 left ");
Serial.println(left);
delay(300);// gives time to move release the button
}
else if (right==0)////if right not assigned ( move the joystick to the left and press the button.)
{
right=val; // same as above
Serial.print("joystick 2 right ");
Serial.println(right);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 2 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{goto here;}
// float
// range=(left-right)/2;// range of joystick
//
// factor=range/255;// corection factor
// Serial.println(factor);
}
void joystick_2_calibrateS()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int top=0;
int bottom=0;
int center=0;
here: // loops until left, right, and center are found
button2 = digitalRead(4);// the position is recorded when the joystick button is pressed
float val = analogRead(6); // joystick position input
if (button2==1)// if button is pressed
{
if (top==0)//if left not assigned ( move the joystick to the left and press the button.)
{
top=val; //asign the left value
Serial.print("joystick 2 top ");
Serial.println(top);
delay(300);// gives time to move release the button
}
else if (bottom==0)////if right not assigned ( move the joystick to the left and press the button.)
{
bottom=val; // same as above
Serial.print("joystick 2 bottom ");
Serial.println(bottom);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 2 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{
goto here;
}
}
void joystick_2_joyM() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(3); // joystick position input
//pos=2.17*val-1116.44; // calculates the position in the desired escale
pos3=3.4615*val-526.1538;
if (pos3>180)// if above range, set to max value
{pos3=180;}
if(pos3<0)// if below range, set to min value
{pos3=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
Serial.print("joystick 2 Axis 1 position is ");
Serial.println(pos3);
//delay(1500);
}
void joystick_2_joyS() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(6); // joystick position input
// Serial.print("val");
// Serial.println(val);
pos4=0.857*val-352.2857; // calculates the position in the desired escale
if (pos4>180)// if above range, set to max value
{pos4=180;}
if(pos4<0)// if below range, set to min value
{pos4=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
// Serial.print("joystick position is ");
// Serial.println(pos);
Serial.print("joystick 2 Axis 2 position is ");
Serial.println(pos4);
//delay(1500);
}
void function_servo_1_base()
{
int a=digitalRead(8);
if(pos1>90 &&a==1) //// reads the position of joystick where when button1 is pressed
{
servo1_base.write(pos1); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos1<90 && a==1)
{
servo1_base.write(pos1); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_2_shoulder()
{
int a=digitalRead(8);
if(pos2>90 && a==1)
{
servo2_shoulder.write(pos2); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos2<90 && a==1)
{
servo2_shoulder.write(pos2); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_3_elbow()
{
int c=digitalRead(4);
if(pos3>90 && c==1)
{
servo3_elbow.write(pos3); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos3<90 && c==1)
{
servo3_elbow.write(pos3); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_4_wrist()
{
int c=digitalRead(4);
if(pos4>90)
{
servo4_wrist.write(pos4); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos4<90)
{
servo4_wrist.write(pos4); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_5_gripper()
{ button2=digitalRead(4);
if(button2==1)
{
servo5_gripper.write(90); // tell servo to go to position in variable 'pos'
delay(30); // waits 15ms for the servo to reach the position
}
else
{
servo5_gripper.write(0); // tell servo to go to position in variable 'pos'
delay(30); // waits 15ms for the servo to reach the position
}
}
void setup()
{
Serial.begin(9600);
pinMode(8,OUTPUT);
pinMode(4,OUTPUT);
servo1_base.attach(2);
servo2_shoulder.attach(11);
servo3_elbow.attach(5);
servo4_wrist.attach(10);
servo5_gripper.attach(12);
joystick_1_calibrateM();
joystick_1_calibrateS();
joystick_2_calibrateM();
joystick_2_calibrateS();
servo1_base.write(90);
servo2_shoulder.write(90);
servo3_elbow.write(90);
servo4_wrist.write(90);
servo5_gripper.write(90);
}
void loop()
{
joystick_1_joyM();
function_servo_1_base();
joystick_1_joyS();
function_servo_2_shoulder();
joystick_2_joyM();
function_servo_3_elbow();
joystick_2_joyS();
function_servo_4_wrist();
function_servo_5_gripper();
}
Thanks
Rohit
NOTE: In above diagram, connections
for wrist servo, gripper servo and Joystick 2 are not shown. They are done in
same way as for other servos and joystick 1. Only the Arduino pins differ as
stated in following table.
// CODE STARTS FROM HERE
// Joystick control of lynxmotion AL5D manipulator
#include<Servo.h>
Servo servo1_base;
Servo servo2_shoulder;
Servo servo3_elbow;
Servo servo4_wrist;
Servo servo5_gripper;
int button1;
int button2;
//int base=2;
//int shoulder=10;
//int elbow=9;
//int wrist=11;
//int gripper=13;
float pos1;
float pos2;
float pos3;
float pos4;
float factor;
int center;
float val; // global variables
int increment_angle=1;
int angle=90;
void joystick_1_calibrateM()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int left=0;
int right=0;
int center=0;
here: // loops until left, right, and center are found
button1 = digitalRead(8);// the position is recorded when the joystick button is pressed
float val = analogRead(0); // joystick position input
if (button1==1)// if button is pressed
{
if (left==0)//if left not assigned ( move the joystick to the left and press the button.)
{
left=val; //asign the left value
Serial.print("joystick 1 left ");
Serial.println(left);
delay(300);// gives time to move release the button
}
else if (right==0)////if right not assigned ( move the joystick to the left and press the button.)
{
right=val; // same as above
Serial.print("joystick 1 right ");
Serial.println(right);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 1 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{goto here;}
// float
// range=(left-right)/2;// range of joystick
//
// factor=range/255;// corection factor
// Serial.println(factor);
}
void joystick_1_calibrateS()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int top=0;
int bottom=0;
int center=0;
here: // loops until left, right, and center are found
button1 = digitalRead(8);// the position is recorded when the joystick button is pressed
float val = analogRead(2); // joystick position input
if (button1==1)// if button is pressed
{
if (top==0)//if left not assigned ( move the joystick to the left and press the button.)
{
top=val; //asign the left value
Serial.print("joystick 1 top ");
Serial.println(top);
delay(300);// gives time to move release the button
}
else if (bottom==0)////if right not assigned ( move the joystick to the left and press the button.)
{
bottom=val; // same as above
Serial.print("joystick 1 bottom ");
Serial.println(bottom);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 1 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{
goto here;
}
}
void joystick_1_joyM() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(0); // joystick position input
//pos=2.17*val-1116.44; // calculates the position in the desired escale
pos1=0.857*val-352.2857;
if (pos1>180)// if above range, set to max value
{pos1=180;}
if(pos1<0)// if below range, set to min value
{pos1=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
Serial.print("joystick 1 Axis 1 position is ");
Serial.println(pos1);
// delay(1500);
}
void joystick_1_joyS() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(2); // joystick position input
// Serial.print("val");
// Serial.println(val);
pos2=0.857*val-352.2857; // calculates the position in the desired escale
if (pos2>180)// if above range, set to max value
{pos2=180;}
if(pos2<0)// if below range, set to min value
{pos2=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
// Serial.print("joystick position is ");
// Serial.println(pos);
Serial.print("joystick 1 Axis 2 position is ");
Serial.println(pos2);
//delay(1500);
}
void joystick_2_calibrateM()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int left=0;
int right=0;
int center=0;
here: // loops until left, right, and center are found
button2 = digitalRead(4);// the position is recorded when the joystick button is pressed
float val = analogRead(3); // joystick position input
if (button2==1)// if button is pressed
{
if (left==0)//if left not assigned ( move the joystick to the left and press the button.)
{
left=val; //asign the left value
Serial.print("joystick 2 left ");
Serial.println(left);
delay(300);// gives time to move release the button
}
else if (right==0)////if right not assigned ( move the joystick to the left and press the button.)
{
right=val; // same as above
Serial.print("joystick 2 right ");
Serial.println(right);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 2 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{goto here;}
// float
// range=(left-right)/2;// range of joystick
//
// factor=range/255;// corection factor
// Serial.println(factor);
}
void joystick_2_calibrateS()// calibrates the joystick, and outputs the center and calibration factors
{ // this code only runs in the setup()
int top=0;
int bottom=0;
int center=0;
here: // loops until left, right, and center are found
button2 = digitalRead(4);// the position is recorded when the joystick button is pressed
float val = analogRead(6); // joystick position input
if (button2==1)// if button is pressed
{
if (top==0)//if left not assigned ( move the joystick to the left and press the button.)
{
top=val; //asign the left value
Serial.print("joystick 2 top ");
Serial.println(top);
delay(300);// gives time to move release the button
}
else if (bottom==0)////if right not assigned ( move the joystick to the left and press the button.)
{
bottom=val; // same as above
Serial.print("joystick 2 bottom ");
Serial.println(bottom);
delay(300);
}
else if (center==0)// same as above
{
center=val;
Serial.print("joystick 2 center ");
Serial.println(center);
delay(300);
}
}
if (center==0)// when all the 3 values are assigned, prossed, otherwise keeps looping back to here
{
goto here;
}
}
void joystick_2_joyM() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(3); // joystick position input
//pos=2.17*val-1116.44; // calculates the position in the desired escale
pos3=3.4615*val-526.1538;
if (pos3>180)// if above range, set to max value
{pos3=180;}
if(pos3<0)// if below range, set to min value
{pos3=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
Serial.print("joystick 2 Axis 1 position is ");
Serial.println(pos3);
//delay(1500);
}
void joystick_2_joyS() //this runs in the loop, and use the corection factor
{ //to calculate the proper output position in the desired escale
val = analogRead(6); // joystick position input
// Serial.print("val");
// Serial.println(val);
pos4=0.857*val-352.2857; // calculates the position in the desired escale
if (pos4>180)// if above range, set to max value
{pos4=180;}
if(pos4<0)// if below range, set to min value
{pos4=0;}
// if(pos>-30 && pos < 30) // sets a dead band in the center
// {pos=0;}
// Serial.print("joystick position is ");
// Serial.println(pos);
Serial.print("joystick 2 Axis 2 position is ");
Serial.println(pos4);
//delay(1500);
}
void function_servo_1_base()
{
int a=digitalRead(8);
if(pos1>90 &&a==1) //// reads the position of joystick where when button1 is pressed
{
servo1_base.write(pos1); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos1<90 && a==1)
{
servo1_base.write(pos1); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_2_shoulder()
{
int a=digitalRead(8);
if(pos2>90 && a==1)
{
servo2_shoulder.write(pos2); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos2<90 && a==1)
{
servo2_shoulder.write(pos2); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_3_elbow()
{
int c=digitalRead(4);
if(pos3>90 && c==1)
{
servo3_elbow.write(pos3); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos3<90 && c==1)
{
servo3_elbow.write(pos3); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_4_wrist()
{
int c=digitalRead(4);
if(pos4>90)
{
servo4_wrist.write(pos4); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
if(pos4<90)
{
servo4_wrist.write(pos4); // tell servo to go to position in variable 'pos'
delay(15); // waits 15ms for the servo to reach the position
}
}
void function_servo_5_gripper()
{ button2=digitalRead(4);
if(button2==1)
{
servo5_gripper.write(90); // tell servo to go to position in variable 'pos'
delay(30); // waits 15ms for the servo to reach the position
}
else
{
servo5_gripper.write(0); // tell servo to go to position in variable 'pos'
delay(30); // waits 15ms for the servo to reach the position
}
}
void setup()
{
Serial.begin(9600);
pinMode(8,OUTPUT);
pinMode(4,OUTPUT);
servo1_base.attach(2);
servo2_shoulder.attach(11);
servo3_elbow.attach(5);
servo4_wrist.attach(10);
servo5_gripper.attach(12);
joystick_1_calibrateM();
joystick_1_calibrateS();
joystick_2_calibrateM();
joystick_2_calibrateS();
servo1_base.write(90);
servo2_shoulder.write(90);
servo3_elbow.write(90);
servo4_wrist.write(90);
servo5_gripper.write(90);
}
void loop()
{
joystick_1_joyM();
function_servo_1_base();
joystick_1_joyS();
function_servo_2_shoulder();
joystick_2_joyM();
function_servo_3_elbow();
joystick_2_joyS();
function_servo_4_wrist();
function_servo_5_gripper();
}
