File content as of revision 0:8a7c1e92d067:

#include "FK.h"

void FK_Engine(float *pos){

// Read the ax-12+ Servos and translate into joint angles α, θ and φ for each leg
    float fore_right_alpha = Fore_Right_Hip_Lateral.GetPosition() - 135;
    float fore_right_theta = 355 - Fore_Right_Hip.GetPosition();
    float fore_right_phi = 305 - Fore_Right_Knee.GetPosition();
    float fore_left_alpha = 165 - Fore_Left_Hip_Lateral.GetPosition();
    float fore_left_theta = Fore_Left_Hip.GetPosition() + 55;
    float fore_left_phi = Fore_Left_Knee.GetPosition() + 5;   
    float hind_right_alpha = 165 - Hind_Right_Hip_Lateral.GetPosition();
    float hind_right_theta = 340 - Hind_Right_Hip.GetPosition();
    float hind_right_phi = 320 - Hind_Right_Knee.GetPosition();
    float hind_left_alpha = Hind_Left_Hip_Lateral.GetPosition() - 135;
    float hind_left_theta = Hind_Left_Hip.GetPosition() + 40;
    float hind_left_phi = Hind_Left_Knee.GetPosition() + 20;      
    float temp;                             // used to store maths
    
// Convert into radians for maths
    hind_right_alpha = hind_right_alpha * PI / 180;
    hind_right_theta = hind_right_theta  * PI / 180;
    hind_right_phi = hind_right_phi * PI / 180;
    hind_left_alpha = hind_left_alpha * PI / 180;
    hind_left_theta = hind_left_theta  * PI / 180;
    hind_left_phi = hind_left_phi * PI / 180;
    fore_right_alpha = fore_right_alpha * PI / 180;
    fore_right_theta = fore_right_theta  * PI / 180;
    fore_right_phi = fore_right_phi * PI / 180;
    fore_left_alpha = fore_left_alpha * PI / 180;
    fore_left_theta = fore_left_theta  * PI / 180;
    fore_left_phi = fore_left_phi * PI / 180;

// Next calculate point 1 for all legs
    // Start with right fore leg
    float x1_fore_right = sin(fore_right_alpha);
    x1_fore_right = x1_fore_right * B;
    float y1_fore_right = 0;
    float z1_fore_right = cos(fore_right_alpha);
    z1_fore_right = z1_fore_right * B;
    // Then left fore leg
    float x1_fore_left = sin(fore_left_alpha + PI);  //Add PI to invert to get the sign correct
    x1_fore_left = x1_fore_left * B;            
    float y1_fore_left = 0;
    float z1_fore_left = cos(fore_left_alpha);
    z1_fore_left = z1_fore_left * B;
    // Then hind right leg
    float x1_hind_right = sin(hind_right_alpha);
    x1_hind_right = x1_hind_right * B;
    float y1_hind_right = 0;
    float z1_hind_right = cos(hind_right_alpha);
    z1_hind_right = z1_hind_right * B;
    // Finally hind left leg
    float x1_hind_left = sin(hind_left_alpha + PI);  //Add PI to invert to get the sign correct
    x1_hind_left = x1_hind_left * B;            
    float y1_hind_left = 0;
    float z1_hind_left = cos(hind_left_alpha);
    z1_hind_left = z1_hind_left * B;

// Now calculate point 2 
    // Aagain start with the fore right leg..
    float x2_fore_right = cos(fore_right_theta + PI);
    x2_fore_right = x2_fore_right * ULNA;
    temp = cos(fore_right_alpha);
    x2_fore_right = x2_fore_right * temp;
    x2_fore_right = x1_fore_right + x2_fore_right;
    float y2_fore_right = sin(fore_right_theta);
    y2_fore_right = y2_fore_right * ULNA;
    y2_fore_right = y2_fore_right + y1_fore_right;
    float z2_fore_right = cos(fore_right_theta);
    z2_fore_right = z2_fore_right * ULNA;
    temp = sin(fore_right_alpha);
    z2_fore_right = z2_fore_right * temp;
    z2_fore_right = z1_fore_right + z2_fore_right;
    // Then the fore left leg
    float x2_fore_left = cos(fore_left_theta + PI);  
    x2_fore_left = x2_fore_left * ULNA;
    temp = cos(fore_left_alpha + PI);       //Add PI to invert to get the sign correct
    x2_fore_left = x2_fore_left * temp;
    x2_fore_left = x1_fore_left + x2_fore_left;
    float y2_fore_left = sin(fore_left_theta);
    y2_fore_left = y2_fore_left * ULNA;
    y2_fore_left = y2_fore_left + y1_fore_left;
    float z2_fore_left = cos(fore_left_theta);
    z2_fore_left = z2_fore_left * ULNA;
    temp = sin(fore_left_alpha);
    z2_fore_left = z2_fore_left * temp;
    z2_fore_left = z1_fore_left + z2_fore_left;
    // Then the hind right leg
    float x2_hind_right = cos(hind_right_theta + PI);
    x2_hind_right = x2_hind_right * FEMUR;
    temp = cos(hind_right_alpha);
    x2_hind_right = x2_hind_right * temp;
    x2_hind_right = x1_hind_right + x2_hind_right;
    float y2_hind_right = sin(hind_right_theta);
    y2_hind_right = y2_hind_right * FEMUR;
    y2_hind_right = y2_hind_right + y1_hind_right;
    float z2_hind_right = cos(hind_right_theta);
    z2_hind_right = z2_hind_right * FEMUR;
    temp = sin(hind_right_alpha);
    z2_hind_right = z2_hind_right * temp;
    z2_hind_right = z1_hind_right + z2_hind_right;
    // Finally the hind left leg...
    float x2_hind_left = cos(hind_left_theta + PI);  
    x2_hind_left = x2_hind_left * FEMUR;
    temp = cos(hind_left_alpha + PI);       //Add PI to invert to get the sign correct
    x2_hind_left = x2_hind_left * temp;
    x2_hind_left = x1_hind_left + x2_hind_left;
    float y2_hind_left = sin(hind_left_theta);
    y2_hind_left = y2_hind_left * FEMUR;
    y2_hind_left = y2_hind_left + y1_hind_left;
    float z2_hind_left = cos(hind_left_theta);
    z2_hind_left = z2_hind_left * FEMUR;
    temp = sin(hind_left_alpha);
    z2_hind_left = z2_hind_left * temp;
    z2_hind_left = z1_hind_left + z2_hind_left;

// Finally calculate point 3 
    // Again start with the fore right leg
    float x3_fore_right = cos(fore_right_theta - fore_right_phi);
    x3_fore_right = x3_fore_right * RADIUS;
    temp = cos(fore_right_alpha);
    x3_fore_right = x3_fore_right * temp;
    x3_fore_right = x2_fore_right + x3_fore_right;
    float y3_fore_right = sin(fore_right_theta + PI - fore_right_phi);
    y3_fore_right = y3_fore_right * RADIUS;
    y3_fore_right = y2_fore_right + y3_fore_right;
    float z3_fore_right = cos(fore_right_theta + PI - fore_right_phi);
    z3_fore_right = z3_fore_right * RADIUS;
    temp = sin(fore_right_alpha);
    z3_fore_right = z3_fore_right * temp;
    z3_fore_right = z2_fore_right + z3_fore_right;
    // Then the fore left leg
    float x3_fore_left = cos(fore_left_theta - fore_left_phi);
    x3_fore_left = x3_fore_left * RADIUS;
    temp = cos(fore_left_alpha + PI);           //Add PI to invert to get the sign correct
    x3_fore_left = x3_fore_left * temp;
    x3_fore_left = x2_fore_left + x3_fore_left;
    float y3_fore_left = sin(fore_left_theta + PI - fore_left_phi);
    y3_fore_left = y3_fore_left * RADIUS;
    y3_fore_left = y2_fore_left + y3_fore_left;
    float z3_fore_left = cos(fore_left_theta + PI - fore_left_phi);
    z3_fore_left = z3_fore_left * RADIUS;
    temp = sin(fore_left_alpha);
    z3_fore_left = z3_fore_left * temp;
    z3_fore_left = z2_fore_left + z3_fore_left;    
    // Then the hind right leg..
    float x3_hind_right = cos(hind_right_theta - hind_right_phi);
    x3_hind_right = x3_hind_right * TIBIA;
    temp = cos(hind_right_alpha);
    x3_hind_right = x3_hind_right * temp;
    x3_hind_right = x2_fore_right + x3_hind_right;
    float y3_hind_right = sin(hind_right_theta + PI - hind_right_phi);
    y3_hind_right = y3_hind_right * TIBIA;
    y3_hind_right = y2_hind_right + y3_hind_right;
    float z3_hind_right = cos(hind_right_theta + PI - hind_right_phi);
    z3_hind_right = z3_hind_right * TIBIA;
    temp = sin(hind_right_alpha);
    z3_hind_right = z3_hind_right * temp;
    z3_hind_right = z2_hind_right + z3_hind_right;
    // Finally the hind left leg
    float x3_hind_left = cos(hind_left_theta - hind_left_phi);
    x3_hind_left = x3_hind_left * TIBIA;
    temp = cos(hind_left_alpha + PI);           //Add PI to invert to get the sign correct
    x3_hind_left = x3_hind_left * temp;
    x3_hind_left = x2_hind_left + x3_hind_left;
    float y3_hind_left = sin(hind_left_theta + PI - hind_left_phi);
    y3_hind_left = y3_hind_left * TIBIA;
    y3_hind_left = y2_hind_left + y3_hind_left;
    float z3_hind_left = cos(hind_left_theta + PI - hind_left_phi);
    z3_hind_left = z3_hind_left * TIBIA;
    temp = sin(hind_left_alpha);
    z3_hind_left = z3_hind_left * temp;
    z3_hind_left = z2_hind_left + z3_hind_left;  
    
    
    // Now updtae the array...   
    pos[0] = x3_fore_right;
    pos[1] = y3_fore_right;
    pos[2] = z3_fore_right;
    pos[3] = x3_fore_left;
    pos[4] = y3_fore_left;
    pos[5] = z3_fore_left;
    pos[6] = x3_hind_right;
    pos[7] = y3_hind_right;
    pos[8] = z3_hind_right;
    pos[9] = x3_hind_left;
    pos[10] = y3_hind_left;
    pos[11] = z3_hind_left;
}