Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

inverse_kinematics.h@26:a8f4a117cc0d, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 10:12:35 2018 +0000
Revision:
26:a8f4a117cc0d
Parent:
21:d541303a2ea6
Child:
27:b247e78a4380
+ Inverse and forward kinematics works.

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 21:d541303a2ea6 34 // Calculation of motor_angle2
brass_phoenix 26:a8f4a117cc0d 35 float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r)); // Angle between r and L1
brass_phoenix 26:a8f4a117cc0d 36 float zeta = acos(J2x_1/r); // Angle between r and x-axis of frame 1
brass_phoenix 26:a8f4a117cc0d 37 dest_sec_angle = zeta - 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 26:a8f4a117cc0d 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 26:a8f4a117cc0d 70
brass_phoenix 21:d541303a2ea6 71 // Delta < 170 graden
brass_phoenix 21:d541303a2ea6 72 if (delta > 2.96706) // If delta is larger than 180 degrees
brass_phoenix 21:d541303a2ea6 73 {
brass_phoenix 21:d541303a2ea6 74 delta = 2.96706;
brass_phoenix 21:d541303a2ea6 75 }
brass_phoenix 21:d541303a2ea6 76 }