Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

inverse_kinematics.h@27:b247e78a4380, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 10:55:11 2018 +0000
Revision:
27:b247e78a4380
Parent:
26:a8f4a117cc0d
Child:
50:5d2176b93a65
+ Added limitation to r angle in inverse kinematics.

Who changed what in which revision?

UserRevisionLine numberNew 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 26:a8f4a117cc0d 15 float lambda_pre_value = Pe_y/(L6-Pe_x);
brass_phoenix 26:a8f4a117cc0d 16 float lambda = PI - atan(lambda_pre_value); // Entire angle between L0 and n
brass_phoenix 21:d541303a2ea6 17 dest_main_angle = lambda - theta;
brass_phoenix 21:d541303a2ea6 18 float J3x_0 = L6 + L4*cos(dest_main_angle); // x-coordinate of joint 3 in frame 0
brass_phoenix 21:d541303a2ea6 19 float J3y_0 = L4*sin(dest_main_angle); // y-coordinate of joint 3 in frame 0
brass_phoenix 21:d541303a2ea6 20
brass_phoenix 21:d541303a2ea6 21 // Calculation of the position of joint 2 in frame 0
brass_phoenix 26:a8f4a117cc0d 22 float S = J3y_0 - Pe_y; // Distance between height endeffector and joint 3
brass_phoenix 21:d541303a2ea6 23 float kappa = asin(S/L5); // Angle of L5
brass_phoenix 21:d541303a2ea6 24 float J2x_0 = (L3+L5)*cos(kappa) + Pe_x; // x-coordinate of joint 2 in frame 0
brass_phoenix 21:d541303a2ea6 25 float J2y_0 = (L3+L5)*sin(kappa) + Pe_y; // y-coordinate of joint 2 in frame 0
brass_phoenix 21:d541303a2ea6 26
brass_phoenix 21:d541303a2ea6 27 // Calculation of the position of joint 1 in frame 0
brass_phoenix 21:d541303a2ea6 28 float J2x_1 = J2x_0 - L0 - L6; // x-coordinate of joint 2 in frame 1
brass_phoenix 21:d541303a2ea6 29 float J2y_1 = J2y_0; // y-coordinate of joint 2 in frame 1
brass_phoenix 21:d541303a2ea6 30 float r = sqrt(pow(J2x_1,2) + pow(J2y_1,2)); // Radius between origin frame 1 and J2
brass_phoenix 21:d541303a2ea6 31 float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r
brass_phoenix 21:d541303a2ea6 32 float q2 = PI - alfa; // Angle between L1 and L2
brass_phoenix 21:d541303a2ea6 33
brass_phoenix 27:b247e78a4380 34 // Delta < 175 graden SAFETY LIMITATION
brass_phoenix 27:b247e78a4380 35 if (r > max_r_angle) // If delta is larger than 175 degrees than r is this
brass_phoenix 27:b247e78a4380 36 {
brass_phoenix 27:b247e78a4380 37 r = max_r_angle;
brass_phoenix 27:b247e78a4380 38 }
brass_phoenix 27:b247e78a4380 39
brass_phoenix 21:d541303a2ea6 40 // Calculation of motor_angle2
brass_phoenix 26:a8f4a117cc0d 41 float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r)); // Angle between r and L1
brass_phoenix 26:a8f4a117cc0d 42 float zeta = acos(J2x_1/r); // Angle between r and x-axis of frame 1
brass_phoenix 26:a8f4a117cc0d 43 dest_sec_angle = zeta - beta;
brass_phoenix 21:d541303a2ea6 44
brass_phoenix 21:d541303a2ea6 45
brass_phoenix 21:d541303a2ea6 46 // Determining angle delta for safety
brass_phoenix 21:d541303a2ea6 47 float J1x_0 = L0 + L6 + L1*cos(dest_sec_angle); // x-coordinate of joint 1 in frame 0
brass_phoenix 21:d541303a2ea6 48 float J1y_0 = L1*sin(dest_sec_angle); // y-coordinate of joint 1 in frame 0
brass_phoenix 21:d541303a2ea6 49
brass_phoenix 21:d541303a2ea6 50 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 51 float delta = acos(- (pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3)); // Angle between L2 and L3
brass_phoenix 21:d541303a2ea6 52
brass_phoenix 26:a8f4a117cc0d 53
brass_phoenix 21:d541303a2ea6 54 // Implementing stops for safety
brass_phoenix 21:d541303a2ea6 55 // 45 < Motor_angle1 < 70 graden
brass_phoenix 21:d541303a2ea6 56 if (dest_main_angle < main_arm_min_angle) // If motor_angle is smaller than 45 degrees
brass_phoenix 21:d541303a2ea6 57 {
brass_phoenix 21:d541303a2ea6 58 dest_main_angle = main_arm_min_angle;
brass_phoenix 21:d541303a2ea6 59 }
brass_phoenix 21:d541303a2ea6 60 else if (dest_main_angle > main_arm_max_angle) // If motor_angle is larger than 70 degrees
brass_phoenix 21:d541303a2ea6 61 {
brass_phoenix 21:d541303a2ea6 62 dest_main_angle = main_arm_max_angle;
brass_phoenix 21:d541303a2ea6 63 }
brass_phoenix 21:d541303a2ea6 64
brass_phoenix 21:d541303a2ea6 65 // -42 < Motor_angle2 < 85 graden
brass_phoenix 21:d541303a2ea6 66 if (dest_sec_angle < sec_arm_min_angle) // If motor_angle is smaller than -42 degrees
brass_phoenix 21:d541303a2ea6 67 {
brass_phoenix 21:d541303a2ea6 68 dest_sec_angle = sec_arm_min_angle;
brass_phoenix 21:d541303a2ea6 69 }
brass_phoenix 21:d541303a2ea6 70 else if (dest_sec_angle > sec_arm_max_angle) // If motor_angle is larger than 85 degrees
brass_phoenix 21:d541303a2ea6 71 {
brass_phoenix 21:d541303a2ea6 72 dest_sec_angle = sec_arm_max_angle;
brass_phoenix 21:d541303a2ea6 73 }
brass_phoenix 21:d541303a2ea6 74
brass_phoenix 21:d541303a2ea6 75
brass_phoenix 26:a8f4a117cc0d 76
brass_phoenix 21:d541303a2ea6 77 // Delta < 170 graden
brass_phoenix 21:d541303a2ea6 78 if (delta > 2.96706) // If delta is larger than 180 degrees
brass_phoenix 21:d541303a2ea6 79 {
brass_phoenix 21:d541303a2ea6 80 delta = 2.96706;
brass_phoenix 21:d541303a2ea6 81 }
brass_phoenix 21:d541303a2ea6 82 }