Diff: FK_Engine.cpp

Revision:

0:8a7c1e92d067

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FK_Engine.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,190 @@
+#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;
+}
\ No newline at end of file