Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
main.cpp@0:6fa73e77d49c, 2018-10-30 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Tue Oct 30 15:19:17 2018 +0000
- Revision:
- 0:6fa73e77d49c
- Child:
- 1:3dfde431f833
Forward kinematics met MATRIX!! (fout)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MAHCSnijders | 0:6fa73e77d49c | 1 | #include "mbed.h" |
MAHCSnijders | 0:6fa73e77d49c | 2 | #include "math.h" |
MAHCSnijders | 0:6fa73e77d49c | 3 | #include "Matrix.h" |
MAHCSnijders | 0:6fa73e77d49c | 4 | #include "MODSERIAL.h" |
MAHCSnijders | 0:6fa73e77d49c | 5 | |
MAHCSnijders | 0:6fa73e77d49c | 6 | MODSERIAL pc(USBTX, USBRX); |
MAHCSnijders | 0:6fa73e77d49c | 7 | |
MAHCSnijders | 0:6fa73e77d49c | 8 | // Stuff die waarschijnlijk weg kan?? |
MAHCSnijders | 0:6fa73e77d49c | 9 | const float L0 = 0.15; // Length between two motors [meter] |
MAHCSnijders | 0:6fa73e77d49c | 10 | const float L1 = 0.10; // Length first beam from right motor2 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 11 | const float L2 = 0.30; // Length second beam from right motor2 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 12 | const float L3 = 0.15; // Length beam between L2 and L4 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 13 | const float L4 = 0.30; // Length first beam from left motor1 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 14 | const float L5 = 0.35; // Length from L3 to end-effector [meter] |
MAHCSnijders | 0:6fa73e77d49c | 15 | const double PI = 3.14159265359; |
MAHCSnijders | 0:6fa73e77d49c | 16 | |
MAHCSnijders | 0:6fa73e77d49c | 17 | // DEZE MOET ER NOG WEL IN!!! |
MAHCSnijders | 0:6fa73e77d49c | 18 | const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 19 | volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 20 | volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 21 | volatile float motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] |
MAHCSnijders | 0:6fa73e77d49c | 22 | volatile float motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] |
MAHCSnijders | 0:6fa73e77d49c | 23 | |
MAHCSnijders | 0:6fa73e77d49c | 24 | |
MAHCSnijders | 0:6fa73e77d49c | 25 | // Useful stuff |
MAHCSnijders | 0:6fa73e77d49c | 26 | Matrix H(3,3); // 2x2 matrix |
MAHCSnijders | 0:6fa73e77d49c | 27 | Matrix J2_2(3,1); // |
MAHCSnijders | 0:6fa73e77d49c | 28 | Ticker ForwardKinematics_ticker; |
MAHCSnijders | 0:6fa73e77d49c | 29 | float J2x_2; |
MAHCSnijders | 0:6fa73e77d49c | 30 | float J2y_2; |
MAHCSnijders | 0:6fa73e77d49c | 31 | |
MAHCSnijders | 0:6fa73e77d49c | 32 | void ForwardKinematics() |
MAHCSnijders | 0:6fa73e77d49c | 33 | { |
MAHCSnijders | 0:6fa73e77d49c | 34 | // Calculation of position joint 1 expressed in frame 0 |
MAHCSnijders | 0:6fa73e77d49c | 35 | float J1x_0 = L6 + L0 + L1*cos(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 36 | float J1y_0 = L1*sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 37 | |
MAHCSnijders | 0:6fa73e77d49c | 38 | // Calculation of position joint 3 expressed in frame 0 |
MAHCSnijders | 0:6fa73e77d49c | 39 | float J3x_0 = L6 + L4*cos(motor_angle1); |
MAHCSnijders | 0:6fa73e77d49c | 40 | float J3y_0 = L4*sin(motor_angle1); |
MAHCSnijders | 0:6fa73e77d49c | 41 | |
MAHCSnijders | 0:6fa73e77d49c | 42 | // Calculation of Joint 2 expressed in frame 2 |
MAHCSnijders | 0:6fa73e77d49c | 43 | float m_y = J3y_0 - J1y_0; |
MAHCSnijders | 0:6fa73e77d49c | 44 | float m_x = J1x_0 - J3x_0; |
MAHCSnijders | 0:6fa73e77d49c | 45 | float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3 |
MAHCSnijders | 0:6fa73e77d49c | 46 | float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) ); |
MAHCSnijders | 0:6fa73e77d49c | 47 | float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m |
MAHCSnijders | 0:6fa73e77d49c | 48 | |
MAHCSnijders | 0:6fa73e77d49c | 49 | float t_y = J3y_0; |
MAHCSnijders | 0:6fa73e77d49c | 50 | float t_x = (L0 + L6) - J3x_0; |
MAHCSnijders | 0:6fa73e77d49c | 51 | float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3 |
MAHCSnijders | 0:6fa73e77d49c | 52 | float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m |
MAHCSnijders | 0:6fa73e77d49c | 53 | |
MAHCSnijders | 0:6fa73e77d49c | 54 | float q2 = PI - mu - phi; // Angle that L2 makes in frame 2 |
MAHCSnijders | 0:6fa73e77d49c | 55 | J2x_2 = L2*cos(q2); |
MAHCSnijders | 0:6fa73e77d49c | 56 | J2y_2 = L2*sin(q2); |
MAHCSnijders | 0:6fa73e77d49c | 57 | |
MAHCSnijders | 0:6fa73e77d49c | 58 | // Coordinate transformation for Joint 2 |
MAHCSnijders | 0:6fa73e77d49c | 59 | |
MAHCSnijders | 0:6fa73e77d49c | 60 | float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1 |
MAHCSnijders | 0:6fa73e77d49c | 61 | float J1y_1 = L1*sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 62 | |
MAHCSnijders | 0:6fa73e77d49c | 63 | |
MAHCSnijders | 0:6fa73e77d49c | 64 | H; |
MAHCSnijders | 0:6fa73e77d49c | 65 | //float J2_1 = H*J2_2; // Homogenous coordinates Joint 2 in frame 1 |
MAHCSnijders | 0:6fa73e77d49c | 66 | //float J2x_0 = J2_1(1) + L0 + L6; // x-coordinate Joint 2 in frame 0 |
MAHCSnijders | 0:6fa73e77d49c | 67 | //float J2y_0 = J2_1(2); |
MAHCSnijders | 0:6fa73e77d49c | 68 | |
MAHCSnijders | 0:6fa73e77d49c | 69 | // DEZE MATRIXMULTIPLICATIES MOETEN OOK IN EEN MATRIX FORMULE GEMAAKT WORDEN. MET STATIC VARIABLES KAN JE DAN NIEUWE MATRIX MAKEN |
MAHCSnijders | 0:6fa73e77d49c | 70 | // DIE BESTAAT UIT DE COMPONENTEN VAN DE ANDERE MATRICES |
MAHCSnijders | 0:6fa73e77d49c | 71 | |
MAHCSnijders | 0:6fa73e77d49c | 72 | |
MAHCSnijders | 0:6fa73e77d49c | 73 | |
MAHCSnijders | 0:6fa73e77d49c | 74 | } |
MAHCSnijders | 0:6fa73e77d49c | 75 | |
MAHCSnijders | 0:6fa73e77d49c | 76 | Matrix ComputeH(void) // Making homogeneous matrix for frame 2 to 1 transformation |
MAHCSnijders | 0:6fa73e77d49c | 77 | { |
MAHCSnijders | 0:6fa73e77d49c | 78 | double a = cos(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 79 | double b = - sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 80 | double c = L1*cos(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 81 | double d = sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 82 | double e = cos(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 83 | double f = L1*sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 84 | double g = 0; |
MAHCSnijders | 0:6fa73e77d49c | 85 | double h = 0; |
MAHCSnijders | 0:6fa73e77d49c | 86 | double i = 1; |
MAHCSnijders | 0:6fa73e77d49c | 87 | |
MAHCSnijders | 0:6fa73e77d49c | 88 | H << a << b << c |
MAHCSnijders | 0:6fa73e77d49c | 89 | << d << e << f |
MAHCSnijders | 0:6fa73e77d49c | 90 | << g << h << i; |
MAHCSnijders | 0:6fa73e77d49c | 91 | return H; |
MAHCSnijders | 0:6fa73e77d49c | 92 | } |
MAHCSnijders | 0:6fa73e77d49c | 93 | |
MAHCSnijders | 0:6fa73e77d49c | 94 | Matrix ComputeJ2_2(void) // Homogenous coordinates Joint 2 in frame 2 |
MAHCSnijders | 0:6fa73e77d49c | 95 | { |
MAHCSnijders | 0:6fa73e77d49c | 96 | double a = J2x_2; |
MAHCSnijders | 0:6fa73e77d49c | 97 | double b = J2y_2; |
MAHCSnijders | 0:6fa73e77d49c | 98 | double c = 1; |
MAHCSnijders | 0:6fa73e77d49c | 99 | |
MAHCSnijders | 0:6fa73e77d49c | 100 | J2_2 << a |
MAHCSnijders | 0:6fa73e77d49c | 101 | << b |
MAHCSnijders | 0:6fa73e77d49c | 102 | << c; |
MAHCSnijders | 0:6fa73e77d49c | 103 | return J2_2; |
MAHCSnijders | 0:6fa73e77d49c | 104 | } |
MAHCSnijders | 0:6fa73e77d49c | 105 | |
MAHCSnijders | 0:6fa73e77d49c | 106 | |
MAHCSnijders | 0:6fa73e77d49c | 107 | int main() |
MAHCSnijders | 0:6fa73e77d49c | 108 | { |
MAHCSnijders | 0:6fa73e77d49c | 109 | pc.baud(115200); |
MAHCSnijders | 0:6fa73e77d49c | 110 | while (true) { |
MAHCSnijders | 0:6fa73e77d49c | 111 | ForwardKinematics_ticker.attach(ForwardKinematics,2); |
MAHCSnijders | 0:6fa73e77d49c | 112 | pc.printf("%d\n",H); |
MAHCSnijders | 0:6fa73e77d49c | 113 | } |
MAHCSnijders | 0:6fa73e77d49c | 114 | } |