Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
main.cpp@6:fe8712b56eb9, 2018-10-31 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Wed Oct 31 21:02:06 2018 +0000
- Revision:
- 6:fe8712b56eb9
- Parent:
- 5:65a609067e14
Fixed bug
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 | |
MAHCSnijders | 0:6fa73e77d49c | 5 | // Stuff die waarschijnlijk weg kan?? |
MAHCSnijders | 0:6fa73e77d49c | 6 | const float L0 = 0.15; // Length between two motors [meter] |
MAHCSnijders | 0:6fa73e77d49c | 7 | const float L1 = 0.10; // Length first beam from right motor2 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 8 | const float L2 = 0.30; // Length second beam from right motor2 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 9 | const float L3 = 0.15; // Length beam between L2 and L4 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 10 | const float L4 = 0.30; // Length first beam from left motor1 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 11 | const float L5 = 0.35; // Length from L3 to end-effector [meter] |
MAHCSnijders | 0:6fa73e77d49c | 12 | const double PI = 3.14159265359; |
MAHCSnijders | 0:6fa73e77d49c | 13 | |
MAHCSnijders | 0:6fa73e77d49c | 14 | // DEZE MOET ER NOG WEL IN!!! |
MAHCSnijders | 0:6fa73e77d49c | 15 | const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 16 | volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 0:6fa73e77d49c | 17 | volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter] |
MAHCSnijders | 6:fe8712b56eb9 | 18 | volatile double motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] |
MAHCSnijders | 6:fe8712b56eb9 | 19 | volatile double motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] |
MAHCSnijders | 0:6fa73e77d49c | 20 | |
MAHCSnijders | 6:fe8712b56eb9 | 21 | DigitalOut safetyLED(LED_GREEN); // Safety check LED |
MAHCSnijders | 4:6db7291caa6d | 22 | |
MAHCSnijders | 0:6fa73e77d49c | 23 | |
MAHCSnijders | 0:6fa73e77d49c | 24 | // Useful stuff |
MAHCSnijders | 0:6fa73e77d49c | 25 | Ticker ForwardKinematics_ticker; |
MAHCSnijders | 0:6fa73e77d49c | 26 | |
MAHCSnijders | 0:6fa73e77d49c | 27 | void ForwardKinematics() |
MAHCSnijders | 0:6fa73e77d49c | 28 | { |
MAHCSnijders | 0:6fa73e77d49c | 29 | // Calculation of position joint 1 expressed in frame 0 |
MAHCSnijders | 0:6fa73e77d49c | 30 | float J1x_0 = L6 + L0 + L1*cos(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 31 | float J1y_0 = L1*sin(motor_angle2); |
MAHCSnijders | 0:6fa73e77d49c | 32 | |
MAHCSnijders | 0:6fa73e77d49c | 33 | // Calculation of position joint 3 expressed in frame 0 |
MAHCSnijders | 0:6fa73e77d49c | 34 | float J3x_0 = L6 + L4*cos(motor_angle1); |
MAHCSnijders | 0:6fa73e77d49c | 35 | float J3y_0 = L4*sin(motor_angle1); |
MAHCSnijders | 0:6fa73e77d49c | 36 | |
MAHCSnijders | 0:6fa73e77d49c | 37 | // Calculation of Joint 2 expressed in frame 2 |
MAHCSnijders | 0:6fa73e77d49c | 38 | float m_y = J3y_0 - J1y_0; |
MAHCSnijders | 0:6fa73e77d49c | 39 | float m_x = J1x_0 - J3x_0; |
MAHCSnijders | 6:fe8712b56eb9 | 40 | float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3 |
MAHCSnijders | 0:6fa73e77d49c | 41 | float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) ); |
MAHCSnijders | 6:fe8712b56eb9 | 42 | float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m |
MAHCSnijders | 0:6fa73e77d49c | 43 | |
MAHCSnijders | 0:6fa73e77d49c | 44 | float t_y = J3y_0; |
MAHCSnijders | 0:6fa73e77d49c | 45 | float t_x = (L0 + L6) - J3x_0; |
MAHCSnijders | 6:fe8712b56eb9 | 46 | float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3 |
MAHCSnijders | 6:fe8712b56eb9 | 47 | float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m |
MAHCSnijders | 0:6fa73e77d49c | 48 | |
MAHCSnijders | 6:fe8712b56eb9 | 49 | float q2 = PI - mu - phi; // Angle that L2 makes in frame 2 |
MAHCSnijders | 2:92aeffa6b16e | 50 | float J2x_2 = L2*cos(q2); |
MAHCSnijders | 2:92aeffa6b16e | 51 | float J2y_2 = L2*sin(q2); |
MAHCSnijders | 0:6fa73e77d49c | 52 | |
MAHCSnijders | 1:3dfde431f833 | 53 | // Calculation of Joint 2 expressed in frame 0 |
MAHCSnijders | 6:fe8712b56eb9 | 54 | float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1 |
MAHCSnijders | 0:6fa73e77d49c | 55 | float J1y_1 = L1*sin(motor_angle2); |
MAHCSnijders | 2:92aeffa6b16e | 56 | float J2x_0 = J2x_2*cos(motor_angle2) - J2y_2 * sin(motor_angle2) + J1x_1 + L0 + L6; // Joint 2 expressed in frame 0 |
MAHCSnijders | 2:92aeffa6b16e | 57 | float J2y_0 = J2x_2*sin(motor_angle2) + J2y_2 * cos(motor_angle2) + J1y_1; |
MAHCSnijders | 2:92aeffa6b16e | 58 | |
MAHCSnijders | 2:92aeffa6b16e | 59 | // Calculation of End-effector |
MAHCSnijders | 6:fe8712b56eb9 | 60 | float f_x = J2x_0 - L6; // I CHANGED THIS!!!! |
MAHCSnijders | 2:92aeffa6b16e | 61 | float f_y = J2y_0; |
MAHCSnijders | 6:fe8712b56eb9 | 62 | float f = sqrt(pow(f_x,2) + pow(f_y,2)); // Radius between motor 1 and Joint 2 |
MAHCSnijders | 6:fe8712b56eb9 | 63 | float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) ); // Angle between L3 and L4 |
MAHCSnijders | 6:fe8712b56eb9 | 64 | float omega = PI - xhi; // Angle between L4 and L5 |
MAHCSnijders | 6:fe8712b56eb9 | 65 | float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega)); // Radius between end effector and motor 1 |
MAHCSnijders | 0:6fa73e77d49c | 66 | |
MAHCSnijders | 6:fe8712b56eb9 | 67 | float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) ); // Angle between n and L4 |
MAHCSnijders | 6:fe8712b56eb9 | 68 | float rho = PI - theta - motor_angle1; // Angle between n and L4 |
MAHCSnijders | 2:92aeffa6b16e | 69 | |
MAHCSnijders | 6:fe8712b56eb9 | 70 | float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector |
MAHCSnijders | 6:fe8712b56eb9 | 71 | float Pe_y = n*sin(rho); // x-coordinate end-effector |
MAHCSnijders | 2:92aeffa6b16e | 72 | |
MAHCSnijders | 4:6db7291caa6d | 73 | |
MAHCSnijders | 4:6db7291caa6d | 74 | // Implementing stops for safety |
MAHCSnijders | 4:6db7291caa6d | 75 | // 45 < Motor_angle1 < 70 graden |
MAHCSnijders | 6:fe8712b56eb9 | 76 | if (motor_angle1 < 0.785398) // If motor_angle is smaller than 45 degrees |
MAHCSnijders | 4:6db7291caa6d | 77 | { |
MAHCSnijders | 5:65a609067e14 | 78 | motor_angle1 = 0.785398; |
MAHCSnijders | 4:6db7291caa6d | 79 | safetyLED = 0; |
MAHCSnijders | 5:65a609067e14 | 80 | } |
MAHCSnijders | 6:fe8712b56eb9 | 81 | else if (motor_angle1 > 1.22173) // If motor_angle is larger than 70 degrees |
MAHCSnijders | 4:6db7291caa6d | 82 | { |
MAHCSnijders | 5:65a609067e14 | 83 | motor_angle1 = 1.22173; |
MAHCSnijders | 4:6db7291caa6d | 84 | safetyLED = 0; |
MAHCSnijders | 4:6db7291caa6d | 85 | } |
MAHCSnijders | 5:65a609067e14 | 86 | |
MAHCSnijders | 5:65a609067e14 | 87 | // -42 < Motor_angle2 < 85 graden |
MAHCSnijders | 6:fe8712b56eb9 | 88 | if (motor_angle2 < -0.733038) // If motor_angle is smaller than -42 degrees |
MAHCSnijders | 5:65a609067e14 | 89 | { |
MAHCSnijders | 5:65a609067e14 | 90 | motor_angle2 = -0.733038; |
MAHCSnijders | 5:65a609067e14 | 91 | safetyLED = 0; |
MAHCSnijders | 5:65a609067e14 | 92 | } |
MAHCSnijders | 6:fe8712b56eb9 | 93 | else if (motor_angle2 > 1.48353) // If motor_angle is larger than 85 degrees |
MAHCSnijders | 5:65a609067e14 | 94 | { |
MAHCSnijders | 5:65a609067e14 | 95 | motor_angle2 = 1.48353; |
MAHCSnijders | 5:65a609067e14 | 96 | safetyLED = 0; |
MAHCSnijders | 5:65a609067e14 | 97 | } |
MAHCSnijders | 5:65a609067e14 | 98 | |
MAHCSnijders | 5:65a609067e14 | 99 | |
MAHCSnijders | 4:6db7291caa6d | 100 | // Delta < 170 graden |
MAHCSnijders | 6:fe8712b56eb9 | 101 | if (delta > 2.96706) // If delta is larger than 180 degrees |
MAHCSnijders | 4:6db7291caa6d | 102 | { |
MAHCSnijders | 5:65a609067e14 | 103 | delta = 2.96706; |
MAHCSnijders | 4:6db7291caa6d | 104 | safetyLED = 0; |
MAHCSnijders | 4:6db7291caa6d | 105 | } |
MAHCSnijders | 4:6db7291caa6d | 106 | |
MAHCSnijders | 2:92aeffa6b16e | 107 | // NEEDS TO RETURN END-EFFECTOR COORDINATES |
MAHCSnijders | 2:92aeffa6b16e | 108 | // return Pe_x; |
MAHCSnijders | 2:92aeffa6b16e | 109 | // return Pe_y; |
MAHCSnijders | 0:6fa73e77d49c | 110 | } |
MAHCSnijders | 0:6fa73e77d49c | 111 | |
MAHCSnijders | 0:6fa73e77d49c | 112 | |
MAHCSnijders | 0:6fa73e77d49c | 113 | int main() |
MAHCSnijders | 0:6fa73e77d49c | 114 | { |
MAHCSnijders | 4:6db7291caa6d | 115 | safetyLED = 1; |
MAHCSnijders | 0:6fa73e77d49c | 116 | while (true) { |
MAHCSnijders | 4:6db7291caa6d | 117 | ForwardKinematics_ticker.attach(ForwardKinematics,0.5); |
MAHCSnijders | 0:6fa73e77d49c | 118 | } |
MAHCSnijders | 0:6fa73e77d49c | 119 | } |