State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
inverse_kinematics.h@21:d541303a2ea6, 2018-10-31 (annotated)
- Committer:
- brass_phoenix
- Date:
- Wed Oct 31 18:54:07 2018 +0000
- Revision:
- 21:d541303a2ea6
- Child:
- 26:a8f4a117cc0d
+ Debugging forward and inverse kinematics.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
brass_phoenix | 21:d541303a2ea6 | 1 | #pragma once |
brass_phoenix | 21:d541303a2ea6 | 2 | |
brass_phoenix | 21:d541303a2ea6 | 3 | #include "mbed.h" |
brass_phoenix | 21:d541303a2ea6 | 4 | #include "constants.h" |
brass_phoenix | 21:d541303a2ea6 | 5 | |
brass_phoenix | 21:d541303a2ea6 | 6 | |
brass_phoenix | 21:d541303a2ea6 | 7 | // Pass the two values that you want the target angle outputs in, as 3rd and 4th arguments. |
brass_phoenix | 21:d541303a2ea6 | 8 | void inverse_kinematics(double Pe_x, double Pe_y, double &dest_main_angle, double &dest_sec_angle) |
brass_phoenix | 21:d541303a2ea6 | 9 | { |
brass_phoenix | 21:d541303a2ea6 | 10 | // Calculation of the position of joint 3 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 11 | float n = sqrt(pow((L6-Pe_x),2) + pow(Pe_y,2)); // Radius between motor 1 and endeffector [meter] |
brass_phoenix | 21:d541303a2ea6 | 12 | float omega = acos(-(pow(n,2) - pow(L4,2) - pow(L5,2))/(2*L4*L5)); // Angle between L4 and L5 [rad] |
brass_phoenix | 21:d541303a2ea6 | 13 | float q4 = PI - omega; // Angle of joint 3 between L3 and L4 |
brass_phoenix | 21:d541303a2ea6 | 14 | float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) ); // Angle between n and L4 |
brass_phoenix | 21:d541303a2ea6 | 15 | float lambda = PI - atan(Pe_y/(L6-Pe_x)); // Entire angle between L0 and n |
brass_phoenix | 21:d541303a2ea6 | 16 | dest_main_angle = lambda - theta; |
brass_phoenix | 21:d541303a2ea6 | 17 | float J3x_0 = L6 + L4*cos(dest_main_angle); // x-coordinate of joint 3 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 18 | float J3y_0 = L4*sin(dest_main_angle); // y-coordinate of joint 3 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 19 | |
brass_phoenix | 21:d541303a2ea6 | 20 | // Calculation of the position of joint 2 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 21 | float S = abs(J3y_0 - Pe_y); // Distance between height endeffector and joint 3 |
brass_phoenix | 21:d541303a2ea6 | 22 | float kappa = asin(S/L5); // Angle of L5 |
brass_phoenix | 21:d541303a2ea6 | 23 | float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 24 | float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 25 | |
brass_phoenix | 21:d541303a2ea6 | 26 | // Calculation of the position of joint 1 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 27 | float J2x_1 = J2x_0 - L0 - L6; // x-coordinate of joint 2 in frame 1 |
brass_phoenix | 21:d541303a2ea6 | 28 | float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1 |
brass_phoenix | 21:d541303a2ea6 | 29 | float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2)); // Radius between origin frame 1 and J2 |
brass_phoenix | 21:d541303a2ea6 | 30 | float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r |
brass_phoenix | 21:d541303a2ea6 | 31 | float q2 = PI - alfa; // Angle between L1 and L2 |
brass_phoenix | 21:d541303a2ea6 | 32 | |
brass_phoenix | 21:d541303a2ea6 | 33 | // Calculation of motor_angle2 |
brass_phoenix | 21:d541303a2ea6 | 34 | float beta = atan(L2*sin(q2)/(L1+L2*cos(q2))); // Angle between r and L1 |
brass_phoenix | 21:d541303a2ea6 | 35 | float gamma = PI - atan(abs(J2y_1/J2x_1)); // Angle between r and x-axis |
brass_phoenix | 21:d541303a2ea6 | 36 | // check if gamma works! |
brass_phoenix | 21:d541303a2ea6 | 37 | dest_sec_angle = gamma - beta; |
brass_phoenix | 21:d541303a2ea6 | 38 | |
brass_phoenix | 21:d541303a2ea6 | 39 | |
brass_phoenix | 21:d541303a2ea6 | 40 | // Determining angle delta for safety |
brass_phoenix | 21:d541303a2ea6 | 41 | float J1x_0 = L0 + L6 + L1*cos(dest_sec_angle); // x-coordinate of joint 1 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 42 | float J1y_0 = L1*sin(dest_sec_angle); // y-coordinate of joint 1 in frame 0 |
brass_phoenix | 21:d541303a2ea6 | 43 | |
brass_phoenix | 21:d541303a2ea6 | 44 | float m = sqrt(pow((J1x_0 - J3x_0),2) + pow((J3y_0 - J1y_0),2)); // Radius between Joint 1 and Joint 3 |
brass_phoenix | 21:d541303a2ea6 | 45 | float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3 |
brass_phoenix | 21:d541303a2ea6 | 46 | |
brass_phoenix | 21:d541303a2ea6 | 47 | |
brass_phoenix | 21:d541303a2ea6 | 48 | // Implementing stops for safety |
brass_phoenix | 21:d541303a2ea6 | 49 | // 45 < Motor_angle1 < 70 graden |
brass_phoenix | 21:d541303a2ea6 | 50 | if (dest_main_angle < main_arm_min_angle) // If motor_angle is smaller than 45 degrees |
brass_phoenix | 21:d541303a2ea6 | 51 | { |
brass_phoenix | 21:d541303a2ea6 | 52 | dest_main_angle = main_arm_min_angle; |
brass_phoenix | 21:d541303a2ea6 | 53 | } |
brass_phoenix | 21:d541303a2ea6 | 54 | else if (dest_main_angle > main_arm_max_angle) // If motor_angle is larger than 70 degrees |
brass_phoenix | 21:d541303a2ea6 | 55 | { |
brass_phoenix | 21:d541303a2ea6 | 56 | dest_main_angle = main_arm_max_angle; |
brass_phoenix | 21:d541303a2ea6 | 57 | } |
brass_phoenix | 21:d541303a2ea6 | 58 | |
brass_phoenix | 21:d541303a2ea6 | 59 | // -42 < Motor_angle2 < 85 graden |
brass_phoenix | 21:d541303a2ea6 | 60 | if (dest_sec_angle < sec_arm_min_angle) // If motor_angle is smaller than -42 degrees |
brass_phoenix | 21:d541303a2ea6 | 61 | { |
brass_phoenix | 21:d541303a2ea6 | 62 | dest_sec_angle = sec_arm_min_angle; |
brass_phoenix | 21:d541303a2ea6 | 63 | } |
brass_phoenix | 21:d541303a2ea6 | 64 | else if (dest_sec_angle > sec_arm_max_angle) // If motor_angle is larger than 85 degrees |
brass_phoenix | 21:d541303a2ea6 | 65 | { |
brass_phoenix | 21:d541303a2ea6 | 66 | dest_sec_angle = sec_arm_max_angle; |
brass_phoenix | 21:d541303a2ea6 | 67 | } |
brass_phoenix | 21:d541303a2ea6 | 68 | |
brass_phoenix | 21:d541303a2ea6 | 69 | |
brass_phoenix | 21:d541303a2ea6 | 70 | // Delta < 170 graden |
brass_phoenix | 21:d541303a2ea6 | 71 | if (delta > 2.96706) // If delta is larger than 180 degrees |
brass_phoenix | 21:d541303a2ea6 | 72 | { |
brass_phoenix | 21:d541303a2ea6 | 73 | delta = 2.96706; |
brass_phoenix | 21:d541303a2ea6 | 74 | } |
brass_phoenix | 21:d541303a2ea6 | 75 | } |