Martin Smith
FK_Engine.cpp@0:0965dacb3caf, 2011-08-21 (annotated)
- Committer:
- ms523
- Date:
- Sun Aug 21 08:25:00 2011 +0000
- Revision:
- 0:0965dacb3caf
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ms523 | 0:0965dacb3caf | 1 | /******************************************************************************** |
| ms523 | 0:0965dacb3caf | 2 | This function finds E.R.I.C.'s foot positions (XYZ co-ordinates) |
| ms523 | 0:0965dacb3caf | 3 | Notebook: http://mbed.org/users/ms523/notebook/robodog-leg-forward-kinematics/ |
| ms523 | 0:0965dacb3caf | 4 | ********************************************************************************/ |
| ms523 | 0:0965dacb3caf | 5 | #include "ERIC.h" |
| ms523 | 0:0965dacb3caf | 6 | |
| ms523 | 0:0965dacb3caf | 7 | Positions FK_Engine(){ |
| ms523 | 0:0965dacb3caf | 8 | |
| ms523 | 0:0965dacb3caf | 9 | // Read the ax-12+ Servos and translate into joint angles alpha, and phi for each leg |
| ms523 | 0:0965dacb3caf | 10 | float hind_right_alpha = 165 - Right_Hip_Lateral.GetPosition(); |
| ms523 | 0:0965dacb3caf | 11 | float hind_right_theta = 340 - Right_Hip.GetPosition(); |
| ms523 | 0:0965dacb3caf | 12 | float hind_right_phi = 320 - Right_Knee.GetPosition(); |
| ms523 | 0:0965dacb3caf | 13 | float hind_left_alpha = Left_Hip_Lateral.GetPosition() - 135; |
| ms523 | 0:0965dacb3caf | 14 | float hind_left_theta = Left_Hip.GetPosition() + 40; |
| ms523 | 0:0965dacb3caf | 15 | float hind_left_phi = Left_Knee.GetPosition() + 20; |
| ms523 | 0:0965dacb3caf | 16 | float temp; // used to store maths |
| ms523 | 0:0965dacb3caf | 17 | |
| ms523 | 0:0965dacb3caf | 18 | // Convert into radians for maths |
| ms523 | 0:0965dacb3caf | 19 | hind_right_alpha = hind_right_alpha * PI / 180; |
| ms523 | 0:0965dacb3caf | 20 | hind_right_theta = hind_right_theta * PI / 180; |
| ms523 | 0:0965dacb3caf | 21 | hind_right_phi = hind_right_phi * PI / 180; |
| ms523 | 0:0965dacb3caf | 22 | hind_left_alpha = hind_left_alpha * PI / 180; |
| ms523 | 0:0965dacb3caf | 23 | hind_left_theta = hind_left_theta * PI / 180; |
| ms523 | 0:0965dacb3caf | 24 | hind_left_phi = hind_left_phi * PI / 180; |
| ms523 | 0:0965dacb3caf | 25 | |
| ms523 | 0:0965dacb3caf | 26 | // Next calculate point 1 for both legs - start with the right leg... |
| ms523 | 0:0965dacb3caf | 27 | float x1_right = sin(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 28 | x1_right = x1_right * B; |
| ms523 | 0:0965dacb3caf | 29 | float y1_right = 0; |
| ms523 | 0:0965dacb3caf | 30 | float z1_right = cos(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 31 | z1_right = z1_right * B; |
| ms523 | 0:0965dacb3caf | 32 | // And for the left leg |
| ms523 | 0:0965dacb3caf | 33 | float x1_left = sin(hind_left_alpha + PI); //Add PI to invert to get the sign correct |
| ms523 | 0:0965dacb3caf | 34 | x1_left = x1_left * B; |
| ms523 | 0:0965dacb3caf | 35 | float y1_left = 0; |
| ms523 | 0:0965dacb3caf | 36 | float z1_left = cos(hind_left_alpha); |
| ms523 | 0:0965dacb3caf | 37 | z1_left = z1_left * B; |
| ms523 | 0:0965dacb3caf | 38 | |
| ms523 | 0:0965dacb3caf | 39 | |
| ms523 | 0:0965dacb3caf | 40 | // Now calculate point 2 - again start with the right leg.. |
| ms523 | 0:0965dacb3caf | 41 | float x2_right = cos(hind_right_theta + PI); |
| ms523 | 0:0965dacb3caf | 42 | x2_right = x2_right * E; |
| ms523 | 0:0965dacb3caf | 43 | temp = cos(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 44 | x2_right = x2_right * temp; |
| ms523 | 0:0965dacb3caf | 45 | x2_right = x1_right + x2_right; |
| ms523 | 0:0965dacb3caf | 46 | float y2_right = sin(hind_right_theta); |
| ms523 | 0:0965dacb3caf | 47 | y2_right = y2_right * E; |
| ms523 | 0:0965dacb3caf | 48 | y2_right = y2_right + y1_right; |
| ms523 | 0:0965dacb3caf | 49 | float z2_right = cos(hind_right_theta); |
| ms523 | 0:0965dacb3caf | 50 | z2_right = z2_right * E; |
| ms523 | 0:0965dacb3caf | 51 | temp = sin(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 52 | z2_right = z2_right * temp; |
| ms523 | 0:0965dacb3caf | 53 | z2_right = z1_right + z2_right; |
| ms523 | 0:0965dacb3caf | 54 | // And now the left leg... |
| ms523 | 0:0965dacb3caf | 55 | float x2_left = cos(hind_left_theta + PI); |
| ms523 | 0:0965dacb3caf | 56 | x2_left = x2_left * E; |
| ms523 | 0:0965dacb3caf | 57 | temp = cos(hind_left_alpha + PI); //Add PI to invert to get the sign correct |
| ms523 | 0:0965dacb3caf | 58 | x2_left = x2_left * temp; |
| ms523 | 0:0965dacb3caf | 59 | x2_left = x1_left + x2_left; |
| ms523 | 0:0965dacb3caf | 60 | float y2_left = sin(hind_left_theta); |
| ms523 | 0:0965dacb3caf | 61 | y2_left = y2_left * E; |
| ms523 | 0:0965dacb3caf | 62 | y2_left = y2_left + y1_left; |
| ms523 | 0:0965dacb3caf | 63 | float z2_left = cos(hind_left_theta); |
| ms523 | 0:0965dacb3caf | 64 | z2_left = z2_left * E; |
| ms523 | 0:0965dacb3caf | 65 | temp = sin(hind_left_alpha); |
| ms523 | 0:0965dacb3caf | 66 | z2_left = z2_left * temp; |
| ms523 | 0:0965dacb3caf | 67 | z2_left = z1_left + z2_left; |
| ms523 | 0:0965dacb3caf | 68 | |
| ms523 | 0:0965dacb3caf | 69 | // Finally calculate point 3 - start with the right leg.. |
| ms523 | 0:0965dacb3caf | 70 | float x3_right = cos(hind_right_theta - hind_right_phi); |
| ms523 | 0:0965dacb3caf | 71 | x3_right = x3_right * F; |
| ms523 | 0:0965dacb3caf | 72 | temp = cos(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 73 | x3_right = x3_right * temp; |
| ms523 | 0:0965dacb3caf | 74 | x3_right = x2_right + x3_right; |
| ms523 | 0:0965dacb3caf | 75 | float y3_right = sin(hind_right_theta + PI - hind_right_phi); |
| ms523 | 0:0965dacb3caf | 76 | y3_right = y3_right * F; |
| ms523 | 0:0965dacb3caf | 77 | y3_right = y2_right + y3_right; |
| ms523 | 0:0965dacb3caf | 78 | float z3_right = cos(hind_right_theta + PI - hind_right_phi); |
| ms523 | 0:0965dacb3caf | 79 | z3_right = z3_right * F; |
| ms523 | 0:0965dacb3caf | 80 | temp = sin(hind_right_alpha); |
| ms523 | 0:0965dacb3caf | 81 | z3_right = z3_right * temp; |
| ms523 | 0:0965dacb3caf | 82 | z3_right = z2_right + z3_right; |
| ms523 | 0:0965dacb3caf | 83 | // Now the left leg... |
| ms523 | 0:0965dacb3caf | 84 | float x3_left = cos(hind_left_theta - hind_left_phi); |
| ms523 | 0:0965dacb3caf | 85 | x3_left = x3_left * F; |
| ms523 | 0:0965dacb3caf | 86 | temp = cos(hind_left_alpha + PI); //Add PI to invert to get the sign correct |
| ms523 | 0:0965dacb3caf | 87 | x3_left = x3_left * temp; |
| ms523 | 0:0965dacb3caf | 88 | x3_left = x2_left + x3_left; |
| ms523 | 0:0965dacb3caf | 89 | float y3_left = sin(hind_left_theta + PI - hind_left_phi); |
| ms523 | 0:0965dacb3caf | 90 | y3_left = y3_left * F; |
| ms523 | 0:0965dacb3caf | 91 | y3_left = y2_left + y3_left; |
| ms523 | 0:0965dacb3caf | 92 | float z3_left = cos(hind_left_theta + PI - hind_left_phi); |
| ms523 | 0:0965dacb3caf | 93 | z3_left = z3_left * F; |
| ms523 | 0:0965dacb3caf | 94 | temp = sin(hind_left_alpha); |
| ms523 | 0:0965dacb3caf | 95 | z3_left = z3_left * temp; |
| ms523 | 0:0965dacb3caf | 96 | z3_left = z2_left + z3_left; |
| ms523 | 0:0965dacb3caf | 97 | |
| ms523 | 0:0965dacb3caf | 98 | // Now send the values back... |
| ms523 | 0:0965dacb3caf | 99 | Positions pos; // Struct to hold positions |
| ms523 | 0:0965dacb3caf | 100 | pos.X_right = (int) x3_right; |
| ms523 | 0:0965dacb3caf | 101 | pos.Y_right = (int) y3_right; |
| ms523 | 0:0965dacb3caf | 102 | pos.Z_right = (int) z3_right; |
| ms523 | 0:0965dacb3caf | 103 | pos.X_left = (int) x3_left; |
| ms523 | 0:0965dacb3caf | 104 | pos.Y_left = (int) y3_left; |
| ms523 | 0:0965dacb3caf | 105 | pos.Z_left = (int) z3_left; |
| ms523 | 0:0965dacb3caf | 106 | |
| ms523 | 0:0965dacb3caf | 107 | return(pos); // Return positions |
| ms523 | 0:0965dacb3caf | 108 | } |