ARDUINO CODE

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

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();
              }