inverse_kinematics.h

00001 #pragma once
00002 
00003 #include "mbed.h"
00004 #include "constants.h"
00005 
00006 
00007 // Pass the two values that you want the target angle outputs in, as 3rd and 4th arguments.
00008 void inverse_kinematics(double Pe_x, double Pe_y, double &dest_main_angle, double &dest_sec_angle)
00009 {
00010     // Calculation of the position of joint 3 in frame 0
00011     float n = sqrt(pow((L6-Pe_x),2) + pow(Pe_y,2));                         // Radius between motor 1 and endeffector [meter]
00012     float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5));      // Angle between L4 and L5 [rad]
00013     float q4 = PI - omega;                                                  // Angle of joint 3 between L3 and L4
00014     float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) );     // Angle between n and L4
00015     float lambda_pre_value = Pe_y/(L6-Pe_x);
00016     float lambda = PI - atan(lambda_pre_value);                               // Entire angle between L0 and n
00017     dest_main_angle = lambda - theta;
00018     float J3x_0 = L6 + L4*cos(dest_main_angle);                            // x-coordinate of joint 3 in frame 0
00019     float J3y_0 = L4*sin(dest_main_angle);                                 // y-coordinate of joint 3 in frame 0
00020     
00021     // Calculation of the position of joint 2 in frame 0 
00022     float S = J3y_0 - Pe_y;                                           // Distance between height endeffector and joint 3
00023     float kappa = asin(S/L5);                                               // Angle of L5  
00024     float J2x_0 = (L3+L5)*cos(kappa) + Pe_x;                                // x-coordinate of joint 2 in frame 0
00025     float J2y_0 = (L3+L5)*sin(kappa) + Pe_y;                                // y-coordinate of joint 2 in frame 0
00026     
00027     // Calculation of the position of joint 1 in frame 0
00028     float J2x_1 = J2x_0 - L0 - L6;                                          // x-coordinate of joint 2 in frame 1
00029     float J2y_1 = J2y_0;                                                    // y-coordinate of joint 2 in frame 1
00030     float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2));                            // Radius between origin frame 1 and J2
00031     float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) );     // Angle opposite of radius r
00032     float q2 = PI - alfa;                                                   // Angle between L1 and L2
00033     
00034     // Delta < 175 graden SAFETY LIMITATION
00035     if (r > max_r_length)                                        // If delta is larger than 175 degrees then r is this
00036     {
00037         r = max_r_length;
00038     }
00039     
00040     // Calculation of motor_angle2
00041     float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r));       // Angle between r and L1
00042     float zeta = acos(J2x_1/r);                                             // Angle between r and x-axis of frame 1
00043     dest_sec_angle = zeta - beta;
00044     
00045 
00046     // Determining angle delta for safety (not necessary for calculation, for debugging purposes)
00047     float J1x_0 = L0 + L6 + L1*cos(dest_sec_angle);                       // x-coordinate of joint 1 in frame 0
00048     float J1y_0 = L1*sin(dest_sec_angle);                                 // y-coordinate of joint 1 in frame 0   
00049     
00050     float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2));        // Radius between Joint 1 and Joint 3
00051     float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3));     // Angle between L2 and L3
00052 
00053     
00054     // Implementing stops for safety
00055     // 45 < Motor_angle1 < 160 graden
00056     if (dest_main_angle < main_arm_min_angle)                             // If motor_angle is smaller than 45 degrees
00057     {
00058         dest_main_angle = main_arm_min_angle;
00059     }
00060     else if (dest_main_angle > main_arm_max_angle)                         // If motor_angle is larger than 70 degrees
00061     {
00062         dest_main_angle = main_arm_max_angle;
00063     }
00064     
00065     // -42 < Motor_angle2 < 85 graden
00066     if (dest_sec_angle < sec_arm_min_angle)                            // If motor_angle is smaller than -42 degrees
00067     {
00068         dest_sec_angle = sec_arm_min_angle;
00069     }
00070     else if (dest_sec_angle > sec_arm_max_angle)                         // If motor_angle is larger than 85 degrees
00071     {
00072         dest_sec_angle = sec_arm_max_angle;
00073     }
00074 }