![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
Revision 6:fe8712b56eb9, committed 2018-10-31
- Comitter:
- MAHCSnijders
- Date:
- Wed Oct 31 21:02:06 2018 +0000
- Parent:
- 5:65a609067e14
- Commit message:
- Fixed bug
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 31 14:11:08 2018 +0000 +++ b/main.cpp Wed Oct 31 21:02:06 2018 +0000 @@ -15,10 +15,10 @@ const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter] volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter] volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter] -volatile double motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] -volatile double motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] +volatile double motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] +volatile double motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] -DigitalOut safetyLED(LED_GREEN); // Safety check LED +DigitalOut safetyLED(LED_GREEN); // Safety check LED // Useful stuff @@ -37,60 +37,60 @@ // Calculation of Joint 2 expressed in frame 2 float m_y = J3y_0 - J1y_0; float m_x = J1x_0 - J3x_0; - float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3 + float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3 float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) ); - float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m + float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m float t_y = J3y_0; float t_x = (L0 + L6) - J3x_0; - float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3 - float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m + float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3 + float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m - float q2 = PI - mu - phi; // Angle that L2 makes in frame 2 + float q2 = PI - mu - phi; // Angle that L2 makes in frame 2 float J2x_2 = L2*cos(q2); float J2y_2 = L2*sin(q2); // Calculation of Joint 2 expressed in frame 0 - float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1 + float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1 float J1y_1 = L1*sin(motor_angle2); float J2x_0 = J2x_2*cos(motor_angle2) - J2y_2 * sin(motor_angle2) + J1x_1 + L0 + L6; // Joint 2 expressed in frame 0 float J2y_0 = J2x_2*sin(motor_angle2) + J2y_2 * cos(motor_angle2) + J1y_1; // Calculation of End-effector - float f_x = J2x_0 - J3x_0; + float f_x = J2x_0 - L6; // I CHANGED THIS!!!! float f_y = J2y_0; - float f = sqrt(pow(f_x,2) + pow(f_y,2)); // Radius between motor 1 and Joint 2 - float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) ); // Angle between L3 and L4 - float omega = PI - xhi; // Angle between L4 and L5 - float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega)); // Radius between end effector and motor 1 + float f = sqrt(pow(f_x,2) + pow(f_y,2)); // Radius between motor 1 and Joint 2 + float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) ); // Angle between L3 and L4 + float omega = PI - xhi; // Angle between L4 and L5 + float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega)); // Radius between end effector and motor 1 - float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) ); // Angle between n and L4 - float rho = PI - theta - motor_angle1; // Angle between n and L4 + float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) ); // Angle between n and L4 + float rho = PI - theta - motor_angle1; // Angle between n and L4 - float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector - float Pe_y = n*sin(rho); // x-coordinate end-effector + float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector + float Pe_y = n*sin(rho); // x-coordinate end-effector // Implementing stops for safety // 45 < Motor_angle1 < 70 graden - if (motor_angle1 < 0.785398) // If motor_angle is smaller than 45 degrees + if (motor_angle1 < 0.785398) // If motor_angle is smaller than 45 degrees { motor_angle1 = 0.785398; safetyLED = 0; } - else if (motor_angle1 > 1.22173) // If motor_angle is larger than 70 degrees + else if (motor_angle1 > 1.22173) // If motor_angle is larger than 70 degrees { motor_angle1 = 1.22173; safetyLED = 0; } // -42 < Motor_angle2 < 85 graden - if (motor_angle2 < -0.733038) // If motor_angle is smaller than -42 degrees + if (motor_angle2 < -0.733038) // If motor_angle is smaller than -42 degrees { motor_angle2 = -0.733038; safetyLED = 0; } - else if (motor_angle2 > 1.48353) // If motor_angle is larger than 85 degrees + else if (motor_angle2 > 1.48353) // If motor_angle is larger than 85 degrees { motor_angle2 = 1.48353; safetyLED = 0; @@ -98,7 +98,7 @@ // Delta < 170 graden - if (delta > 2.96706) // If delta is larger than 180 degrees + if (delta > 2.96706) // If delta is larger than 180 degrees { delta = 2.96706; safetyLED = 0;