Before I begin explaining the firmware, and how it controls each joint, I believe it is important for the reader to grasp how the Limbi looks, and the name of each joint. This will help the reader understand what I’m referring to in the rest of this section.

Limbi 2DAs seen in the Figure above, the Limbi has 4 joints. In this report, I will be referring to Joint 1 as Vernier Control 1, Joint 2 as Middle Control 1, Joint 3 as Middle Control 2, and Joint 4 as Vernier Control 2.

The firmware for our project contains 7 custom commands, and 1 command redefinition. These custom commands are: Move, Dock 1, Dock 2, Vernier Control 1, Vernier Control 2, Middle Control 1, Middle Control 2, and Inverse Kinematics Selector. These are briefly described in the Mission Command and Control Section above.

Below is the most recent code for Limbi.

#define DOCK1       0x40  // Dock 1 Custom Command Address

#define DOCK2       0x41  // Dock 2 Custom Command Address

#define VERNIER1    0x42  // Vernier 1 Custom Command Address

#define VERNIER2    0x43  // Vernier 2 Custom Command Address

#define MIDDLE1     0X44  // Middle 1 Custom Command Address

#define MIDDLE2     0X45  // Middle 2 Custom Command Address

#define IKSELECTOR  0x46 // Inverse Kinematics Selector Custom Command Address

#define L1 154L           // Define the length of length 1 (measurement is in mm)       

#define L2 125L           // Define the length of length 2 (measurement is in mm)     


const uint8_t CMD_LIST_SIZE = 8;   

// Adding 8 commands (MOVE, DOCK1, DOCK2, VERNIER1, VERNIER2, MIDDLE1, MIDDLE2, IKSELECTOR)


int globalIKSelector = 0;

int globalPhi1;         // Saves angle last written to mid1 Servo for use in Inverse Kinematics

int globalTheta1;       // Saves angle last written to end1 Servo for use in Inverse Kinematics

int globalPhi2;         // Saves angle last written to mid2 Servo for use in Inverse Kinematics

int globalTheta2;       // Saves angle last written to end2 Servo for use in Inverse Kinematics

int globalCount = 1;    // Used for conditional statements to prevent both docks from being undocked at the same time




// Create variable speed servo objects

VarSpeedServo mid1;     // Joint 2

VarSpeedServo mid2;     // Joint 3

VarSpeedServo end1;     // Joint 1

VarSpeedServo end2;     // Joint 4

VarSpeedServo dock1;    // Dock 1

VarSpeedServo dock2;    // Dock 2




void dock1Handler (uint8_t cmd, uint8_t param[], uint8_t n)

{

   dock1.attach(2);            // Attach dock1 servo to pin 2

   if(param[0] == 1 ){         // Using boolean: if button = 1 = ON DOCK TO MODULE

     dock1.slowmove(41,40);    // Move dock 1 Servo to angle of 41 degrees at speed 40/255

     delay(2000);              // Delay allows servo to get to position before detaching

     globalCount = 1;          // Set Global Count to 1 when Dock 1 is connected

   }

   else{                       // Else: button = 0 = OFF UNDOCK FROM MODULE

     if(globalCount%2==0){     // Takes remainder of division globalCount/2, to check if it's

 // odd or even

                               // If it's even, it allows the dock to be undocked

       digitalWrite(10,HIGH);  // Set's Push Pull Solenoid to High so that it retracts

       delay(1000);

       dock1.slowmove(90,40);  // Move dock 1 Servo to angle of 90 degrees at speed 40/255

       delay(1000);

       digitalWrite(10,LOW);    // Set's Push Pull Solenoid to Low so power is not being drawn

       delay(500);

     }

   }

   dock1.detach();             // Detach dock1 servo from pin

}  // dock 1 Handler




void dock2Handler (uint8_t cmd, uint8_t param[], uint8_t n)

{

   dock2.attach(4);           // Attach dock2 servo to pin 4

   if(param[0] == 1){          // Using boolean: if button = 1 = ON DOCK TO MODULE

     dock2.slowmove(41,40);    // Move dock 2 Servo to angle of 41 degrees at speed 40/255

     delay(2000);              // Delay allows servo to get to position before detaching

     globalCount = 2;          // Set Global Count to 2 when Dock 2 is connected

   }

   else{                       // Else: button = 0 = OFF UNDOCK FROM MODULE

     if(globalCount%2==1){     // Takes remainder of division globalCount/2

                               // If it's odd, it allows the dock to be undocked

       digitalWrite(9,HIGH);   // Set's Push Pull Solenoid to High so that it retracts

       delay(1000);

       dock2.slowmove(90,40);  // Move dock 2 Servo to angle of 90 degrees at speed 40/255

       delay(1000);

       digitalWrite(9,LOW);   // Set's Push Pull Solenoid to Low so power is not being drawn

       delay(500);

     }

   }

   dock2.detach();             // Detach dock2 servo from pin

}  // dock 2 Handler




void vernier1Handler (uint8_t cmd, uint8_t param[], uint8_t n)//param[0] = unsigned byte (1-180)

{

   end1.attach(3);             // Attach end1 servo to pin 3

   end1.slowmove(param[0],40); // Move end1 Servo to angle of param[0] degrees at speed 40/255

   delay(2500);

   end1.detach();              // Detach end1 servo from pin 3

   globalTheta1 = param[0];    // Saves end1 servo angle for use in Inverse Kinematics

}  //  vernier 1 Handler




void vernier2Handler (uint8_t cmd, uint8_t param[], uint8_t n)//param[0] = unsigned byte (1-180)

{

   end2.attach(5);             // Attach end2 servo to pin 5

   end2.slowmove(param[0],40); // Move end2 Servo to angle of param[0] degrees at speed 40/255

   delay(2500);

   end2.detach();              // Detach end2 servo from pin 5

   globalTheta2 = param[0];    // Saves end2 servo angle for use in Inverse Kinematics

}  // vernier 2 Handler




void middle1Handler (uint8_t cmd, uint8_t param[], uint8_t n)//param[0] = unsigned byte (1-180)

{

   mid1.attach(12);             // Attach mid1 servo to pin 12

   mid1.slowmove(param[0],30);  // Move mid1 Servo to angle of param[0] degrees at speed 30/255

   delay(2500);

   mid1.detach();               // Detach mid1 servo from pin 12

   globalPhi1 = param[0];       // Saves mid1 servo angle for use in Inverse Kinematics

}  // middle 1 Handler




void middle2Handler (uint8_t cmd, uint8_t param[], uint8_t n)//param[0] = unsigned byte (1-180)

{

   mid2.attach(11);             // Attach mid2 servo to pin 11

   mid2.slowmove(param[0],30); // Move mid2 Servo to angle of param[0] degrees at speed 30/255

   delay(2500);

   mid2.detach();              // Detach mid2 servo from pin 11

   globalPhi2 = param[0];      // Saves mid2 servo angle for use in Inverse Kinematics

}  // middle 2 Handler




double forwardKinematicsX(){    // Calculates the current X Position of the endpoint on either end of Limbi

 int theta;

 int phi;

 if(globalIKSelector == 0){    // INVERSE KINEMATICS FOR SIDE 1

   theta = globalTheta1;       // Reads angle last written to end1 servo

   phi = 180 - globalPhi1;     // Reads angle last written to mid1 servo

 }

 else{                         // INVERSE KINEMATICS FOR SIDE 2

   theta = globalTheta2;       // Reads angle last written to end2 servo

   phi = 180 - globalPhi2;     // Reads angle last written to mid2 servo

 }




 if(theta<=90){                // Adjusts angle value, due to servo position on Limbi

   theta = 90 - theta;

 }

 else{

   theta = 360 - (theta - 90);

 }




 // Forward Kinematics equation for two DOF

 double xCalc = L1*cos(phi*PI/180) + L2*cos((phi+theta)*PI/180);  

 return xCalc;                   // Returns current X Position

}










double forwardKinematicsY(){      // Calculates the current Y Position of the endpoint on Side 1 of Limbi

 int theta;

 int phi;

 if(globalIKSelector == 0){      // INVERSE KINEMATICS FOR SIDE 1

   theta = globalTheta1;     // Reads angle last written to end1 servo

   phi = 180 - globalPhi1;   // Reads angle last written to mid1 servo

 }

 else{                           // INVERSE KINEMATICS FOR SIDE 2

   theta = globalTheta2;     // Reads angle last written to end2 servo

   phi = 180 - globalPhi2;   // Reads angle last written to mid2 servo

 }




 if(theta<=90){                // Adjusts angle value, due to servo position on Limbi

   theta = 90 - theta;

 }

 else{

   theta = 360 - (theta - 90);

 }




 // Forward Kinematics equation for two DOF

 double yCalc = L1*sin(phi*PI/180) + L2*sin((theta+phi)*PI/180);

 return yCalc;                 // Returns current Y Position

}




//  Inverse Kinematics Subroutine. Calculates Phi angle necessary for desired position

int inverseKinematicsPhi(double x, double y){ // Inputs are desired X and Y Position

 // Inverse Kinematics Variable Calculations

 double C = (x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2);

 double S = sqrt(1 - C*C);

 double K1 = L1 + L2*C;

 double K2 = L2*S;




 //Inverse Kinematics equation. Calculates Phi Angle necessary for desired X,Y Position

 double phi = 180 - ((atan2(y, x) - atan2(K2, K1))*180/PI);




 phi = constrain(phi, 0, 180);   // Constrains the value of Phi between 1 and 180

 int phiPass = (int)round(phi);  // Rounds double to the closest integer

 return phiPass;                 // Returns angle to be written to mid1 Servo

}




//  Inverse Kinematics Subroutine. Calculates Theta angle necessary for desired position

int inverseKinematicsTheta(double x, double y){ // Inputs are desired X and Y Position

 // Inverse Kinematics Variable Calculations

 double C = (x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2);

 double S = sqrt(1 - C*C);




 //Inverse Kinematics equation. Calculates Phi Angle necessary for desired X,Y Position

 double theta = (atan2(S,C))*180/PI;




 if(theta<=90){                      // Adjusts theta angle due to servo position on Limbi

   theta = 90 - theta;

 }

 else{

   theta = 360 - (theta - 90);

 }

 theta = constrain(theta, 0, 180);   // Constrains the value of Phi between 1 and 180

 int thetaPass = (int)round(theta);  // Rounds double to the closest integer

 return thetaPass;                   // Returns angle to be written to end1 Servo

}







void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n){

 double x = forwardKinematicsX();      // Calls subroutine to calculate the current X Position

 double y = forwardKinematicsY();      // Calls subroutine to calculate the current Y Position

 if(param[0] == 1 && param[2] == 1){       // If the forward position on Joystick is pressed

   y =y + 5;                               // Increases the Y position, moving Limbi end piece forward

 }

 else if(param[0] == 2 && param[2] == 2){  // If the back position on Joystick is pressed

   y = y - 5;                              // Decreases the Y position, moving Limbi end piece backwards

 }

 else if(param[0] == 1 && param[2] == 2){  // If the right position on Joystick is pressed

   x = x + 5;                              // Increases the X position, moving Limbi end piece to the Right

 }

 else {   // If the left position on Joystick is pressed

   x = x - 5;                              // Decreases the X position, moving Limbi end piece to the Left

 }

 int phiFinal = inverseKinematicsPhi(x, y);      // Calls subroutine to calculate Phi necessary to get desired X,Y position

 int thetaFinal = inverseKinematicsTheta(x, y);  // Calls subroutine to calculate Theta necessary to get desired X,Y position

 if(globalIKSelector == 0){        // INVERSE KINEMATICS FOR SIDE 1

   end1.attach(3);                 // Attach end1 servo to Pin 3

   mid1.attach(12);                 // Attach mid1 servo to Pin 12

   end1.slowmove(thetaFinal, 20);  // Move end1 Servo to calculated Theta

   mid1.slowmove(phiFinal, 20);    // Move mid1 Servo to calculated Phi

   delay(1000);

   end1.detach();                  // Detach Servo

   mid1.detach();                  // Detach Servo

   globalTheta1 = thetaFinal;      // Saves value written to end1 servo for use in Inverse Kinematics

   globalPhi1 = phiFinal;          // Saves value written to mid1 servo for use in Inverse Kinematics

 }

 else{                             // INVERSE KINEMATICS FOR SIDE 2

   end2.attach(5);                 // Attach end2 servo to Pin 5

   mid2.attach(11);                 // Attach mid2 servo to Pin 11

   end2.slowmove(thetaFinal, 20);  // Move end2 Servo to calculated Theta

   mid2.slowmove(phiFinal, 20);    // Move mid2 Servo to calculated Phi

   delay(1000);

   end2.detach();                  // Detach Servo

   mid2.detach();                  // Detach Servo

   globalTheta2 = thetaFinal;      // Saves value written to end1 servo for use in Inverse Kinematics

   globalPhi2 = phiFinal;          // Saves value written to mid1 servo for use in Inverse Kinematics

 }

}  // moveHandler




void ikSelectHandler (uint8_t cmd, uint8_t param[], uint8_t n)

{

 globalIKSelector = param[0]; // Saves value of what end Inverse Kinematics will be performed on

}







ArxRobot::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{MOVE,moveHandler}, {DOCK1,dock1Handler}, {DOCK2,dock2Handler},

   {VERNIER1,vernier1Handler}, {VERNIER2,vernier2Handler}, {MIDDLE1,middle1Handler}, {MIDDLE2,middle2Handler}, {IKSELECTOR, ikSelectHandler}};




Packet motorPWM(MOTOR2_CURRENT_ID);  // Initialize the packet properties to default values




void setup()

{

 Serial.begin(9600);               // Default = 115200

 ArxRobot.begin();

 ArxRobot.setOnCommand(onCommand, CMD_LIST_SIZE);




 motorPWM.setAccuracy(1);          // Change sensor accuracy from +/-2 DN to +/-1 DN

 motorPWM.setSamplePeriod(500);    // Change sample period from 1 second to 0.5 seconds




 // Initialize servos

 dock2.attach(4);       // Attach Servos to corresponding Pin

 end1.attach(3);

 end2.attach(5);

 mid1.attach(12);

 mid2.attach(11);

 dock2.slowmove(90,40);  // Move Servo to Initial Position

 end1.slowmove(90,40);

 end2.slowmove(90,40);

 mid1.slowmove(90,40);

 mid2.slowmove(90,40);

 delay(5000);      

 dock2.detach();         // Detach Servos from pin

 end1.detach();

 end2.detach();

 mid1.detach();

 mid2.detach();

 pinMode(9, OUTPUT);     // Set Pins as Outputs for Solenoid Signal

 pinMode(10, OUTPUT);

 digitalWrite(9, LOW);   // Initialize Solenoids to LOW or Off

 digitalWrite(10, LOW);

 delay(1000);

 globalPhi1 = 90;

 globalTheta1 = 90;

 globalPhi2 = 90;

 globalTheta2 = 90;

}




void loop(){

 ArxRobot.loop();




#if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega16U4__)

   uint16_t pwm_reading = (uint16_t) OCR4D;  // read 8-bit Output Compare Register Timer 4D and cast to 16-bit signed word

   motorPWM.sendSensor(pwm_reading);

 #else

   // Timer/Counter 0 registers set by UNO Bootloader (freq. = 1 Khz)

   // Timer/Counter Control Registers TCCR0B = 0x03 (Prescaler of 64) and TCCR0A (Fast PWM, TOP = 0xFF) = 0x03

   uint16_t pwm_reading = (uint16_t) OCR0B;

   motorPWM.sendSensor(pwm_reading);

 #endif

}

Libraries, Global Variables, and Initialization

Lets begin with the libraries being used in this code. The first library that is being used in this code is the “ArxRobot.h” library. This library is being used for it’s bluetooth capabilities, and ability to connect with the Arxterra App/Control Panel. The second Library being used is the “EEPROM” library. This library allows the Arduino to keep certain values when the board is turned off. The third library being used is the “Wire.h” library which allows for communication with I2C devices. The fourth library being used is the “VarSpeedServo.h” library. This library is being used to control the servos. The reason this library was chosen instead of the “Servo.h” library, is that this library allows the servos to be controlled with variable speed, and this way there wasn’t a need for a custom code to be written to control the speed of the servos. The “Servo.h” library supports up to 8 servos, and allows for simultaneous, asynchronous movement of all servos. The variable speed is from 1 to 255, with 1 being the slowest, and 255 being the fastest. The last library being used is the Math.h Library. This library allows for math functions and operations, and is used in the forward/inverse kinematics section of the code.

After the libraries were added to the code, we have our definitions, and creation of servos as objects. In this section, the addresses of our custom commands are defined. Two other definitions that were added are L1 and L2. L1 and L2 are the lengths of length 1 and length 2 of the Limbi in mm. Also in this section, all 6 servos were created as objects using the VarSpeedServo.h library.

Finally, the command list size was set to 8, and the global variables were set. The command list size is the number of custom commands that we are using, and it ties back the address of the command to the subroutine name. The global variables being used are the following. globalIKSelector–stores the value of the ikSelectHandler subroutine. This subroutine is a custom command that allows the user to choose what end of the Limbi the inverse kinematics will be performed on. GlobalPhi1, globalTheta1, globalPhi2, and globalTheta2 are place holders that hold the angle last written to the middle1 servo, end1  servo, middle2 servo, and end2 servo respectively. These global variables are being used because the angles are used over several subroutines for the inverse kinematics. The last global variable is globalCount. This variable is used for the docking subroutines and is used to ensure that both docks aren’t undocked at the same time.

During the initialization of the code, all of the pins are set to their corresponding servo, or are set as Output/Inputs. All of the joint servos are set to 90 degrees, which will result in the arm being completely straight. Dock 1 servo is set to a docked position, while Dock 2 servo is set to an undocked position. Pins 9 and 10 are set as Outputs, and set to LOW. These two pins are the pins that will be used for the solenoids on the docking mechanism.

Dock1Handler and Dock2Handler Subroutines

The Dock 1 command and Dock 2 commands perform the same function just on opposite sides of the Limbi.The Dock 1 code and Dock 2 code are almost identical. These two subroutines are called whenever the user presses the ON/OFF button with the command address 0x40, and 0x41 for Dock 1 and Dock 2 respectively. When the user presses the button, the Arxterra App or Control Panel send a 0 for OFF and a 1 for ON.

The first thing that happens when this is performed, is that the respective servo is attached to it’s corresponding digital pin.

Then there is a conditional statement that depends on the parameter sent by the user through the app or control panel. If the user sent a parameter of 1 (Or pressed the ON button), then this causes the Dock Servo to turn and therefore Lock the Limbi to the module. The dock servo turns to an angle of 41 degrees at a speed of 40/255. If the conditional statement reads a 1, it also causes the globalCount variable to set to 1 when Dock 1 is docked, and it sets the globalCount to 2 when Dock 2 is docked. This is important to note, because this is what prevents both docks from being undocked at the same time.

The second half of the conditional statement (the else part), will run if the user sent a parameter of 0 (Or pressed the OFF button). This section controls the undocking (or unlocking) between the Limbi and the module. However, as was explained in the previous paragraph, the undocking is controlled and both docks can’t be undocked at the same time. This means that in order for one dock to undock, the other dock must have been connected previously. As stated earlier, when Dock 1 is ON, it saves a value of 1 in globalCount, and when Dock 2 is ON, it saves a value of 2 in globalCount. In order for the undocking to take place, Dock 1 has another conditional statement that takes the remaider of a division between globalCount, and 2. If the answer is 0 (no remainder) then the actual undocking occurs. If thats not the case, then it doesn’t undock. The reason this is set this way is because it’s set this way for both Dock 1 and Dock 2, and it helps prevent both docks from being disconnected at the same time which would result in a loss of power.

Vernier 1 and 2 Handler, and Middle 1 and 2 Handler

These four subroutines all work the same way, and essentially do the same thing. On the Arxterra App/Control Panel, these commands were made into sliders with a value between 0 and 180. When the user moves any of these sliders, the respective subroutine is called, and the value sent from the slider is received in the subroutine as the zeroth element in the vector “param” (param[0]). Param[0] is the angle that we want the servo to move to. The respective servo is attached to it’s corresponding pin, and the angle is written to the servo, with a certain speed using the slowmove() command from the VarSpeedServo library. The servo is then detached, and the angle that was written to the servo is then saved in it’s corresponding globalVariable.

Forward/Inverse Kinematics

This is perhaps the most complex section of the code, and it involves several subroutines working in unison. These subroutines allow the forward or inverse kinematics to be performed on either the middle 1 and end 1 joints or the middle 2 and end 2 joints. One thing to note about this section is that whenever Phi corresponds to the middle 1 or middle 2 joint, and Theta corresponds to the end 1 or end 2 servo joints. This is seen in the figure below.

Inverse Kinematics Spreadsheet to calculate angles

The first subroutine I will explain is the ikSelectHandler subroutine, as it is the simplest. This subroutine is a custom command that uses the Arxterra app/control panel. This is a select button that has an option between Side 1 and Side 2. When the subroutine is called, and the user selects an option, the vector element “param[0]” recieves either a 0 or a 1. This element value is saved in the global variable “globalIKSelector”. If globalIKSelector is a 0 that corresponds to side 1, and if it’s a 1 it corresponds to side 2. This is used later for determining what side to perform the Inverse Kinematics on.

MoveHandler Subroutine

The second subroutine that deals with the inverse kinematics is the MoveHandler subroutine. The overall goal of this subroutine is to move the limbi, with respect to either the middle 1 joint or middle 2 joint, in the forward, backwards, left or right direction. If globalIKSelector is a 0, then the reference point will be the middle 1 joint and the end point will be the end of the length on the end 1 joint. If the globalIKSelector is a 1, then the reference point will be the middle 2 joint, and the end point will be the end of the length on the end 2 joint.

I will only explain how the inverse Kinematics works on one side, as it is the same on both sides, and this way it will be easier to explain and understand. Using the middle 1 joint as our reference point, the location of this joint becomes (0,0) in an X,Y Plane, with the position of the end point on the end 1 joint being in the positive Y direction when fully extended.

The move command is called whenever the user presses either of the buttons on the joystick in the control panel/app. When this happens, the first thing that happens is that the MoveHandler calls the subroutines forwardKinematicsX and forwardKinematicsY. These subroutines use the globalPhi and globalTheta which are saved each time an angle is written to middle1 servo and end1 servo. Some arithmetic is performed on the angles to get the desired angle for the equation, and then the forward kinematics are calculated using these angles to determine what the current X,Y position of the end point is. The X and Y values are returned back to the MoveHandler.

Once the X and Y values are in the moveHandler, the code determines what position we wish to go to. The joystick in the app/control panel is meant for motors, and determines the direction and speed of the motor. The limbi code only utilizes the direction to determine what our desired X and Y position is. The moveHandler then has a conditional statement to determine whether we will be increasing or decreasing in the X or Y position. The vector param, has a length of 4, however the elements param[0] and param[2] contain the direction of the joysticks. Param[0] = 1 & param[2]  = 1 corresponds to forward, param[0] = 2 & param[2]  = 2 corresponds to backwards, param[0] = 1 & param[2]  = 2 corresponds to right, and param[0] = 2 & param[2]  = 1 corresponds to left. The conditional statement uses these parameters, and if it’s forward it will add only to the Y position, if it’s backwards, it will subtract only from the Y position, if it’s left it will subtract from the X position, and if it’s right it will add to the X position.

This X and Y position are now sent to the subroutines inverseKinematicsPhi and inverseKinematicsTheta. These subroutines perform inverse kinematics calculations on the X and Y position that was determined we desire the limbi to go to, and then calculate and angles for Phi and Theta that will move the endpoint to that location. These values are then returned to the Move Handler.

Once the MoveHandler has the final angles calculated by the inverse kinematics, it then writes the angles to the corresponding servos.