Biorobotics 7 / Mbed 2 deprecated Forward_kinematics

Forward Kinematics

Dependencies:   MODSERIAL Matrix mbed

main.cpp@4:6db7291caa6d, 2018-10-31 (annotated)

Committer:
MAHCSnijders
Date:
Wed Oct 31 13:55:56 2018 +0000
Revision:
4:6db7291caa6d
Parent:
3:c006b3e9a41c
Child:
5:65a609067e14
Forward kinematics with safety breaks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MAHCSnijders 0:6fa73e77d49c 1 #include "mbed.h"
MAHCSnijders 0:6fa73e77d49c 2 #include "math.h"
MAHCSnijders 0:6fa73e77d49c 3 #include "Matrix.h"
MAHCSnijders 0:6fa73e77d49c 4
MAHCSnijders 0:6fa73e77d49c 5 // Stuff die waarschijnlijk weg kan??
MAHCSnijders 0:6fa73e77d49c 6 const float L0 = 0.15; // Length between two motors [meter]
MAHCSnijders 0:6fa73e77d49c 7 const float L1 = 0.10; // Length first beam from right motor2 [meter]
MAHCSnijders 0:6fa73e77d49c 8 const float L2 = 0.30; // Length second beam from right motor2 [meter]
MAHCSnijders 0:6fa73e77d49c 9 const float L3 = 0.15; // Length beam between L2 and L4 [meter]
MAHCSnijders 0:6fa73e77d49c 10 const float L4 = 0.30; // Length first beam from left motor1 [meter]
MAHCSnijders 0:6fa73e77d49c 11 const float L5 = 0.35; // Length from L3 to end-effector [meter]
MAHCSnijders 0:6fa73e77d49c 12 const double PI = 3.14159265359;
MAHCSnijders 0:6fa73e77d49c 13
MAHCSnijders 0:6fa73e77d49c 14 // DEZE MOET ER NOG WEL IN!!!
MAHCSnijders 0:6fa73e77d49c 15 const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter]
MAHCSnijders 0:6fa73e77d49c 16 volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter]
MAHCSnijders 0:6fa73e77d49c 17 volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter]
MAHCSnijders 0:6fa73e77d49c 18 volatile float motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad]
MAHCSnijders 0:6fa73e77d49c 19 volatile float motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad]
MAHCSnijders 0:6fa73e77d49c 20
MAHCSnijders 4:6db7291caa6d 21 DigitalOut safetyLED(LED_GREEN); // Safety check LED
MAHCSnijders 4:6db7291caa6d 22
MAHCSnijders 0:6fa73e77d49c 23
MAHCSnijders 0:6fa73e77d49c 24 // Useful stuff
MAHCSnijders 0:6fa73e77d49c 25 Ticker ForwardKinematics_ticker;
MAHCSnijders 0:6fa73e77d49c 26
MAHCSnijders 0:6fa73e77d49c 27 void ForwardKinematics()
MAHCSnijders 0:6fa73e77d49c 28 {
MAHCSnijders 0:6fa73e77d49c 29 // Calculation of position joint 1 expressed in frame 0
MAHCSnijders 0:6fa73e77d49c 30 float J1x_0 = L6 + L0 + L1*cos(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 31 float J1y_0 = L1*sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 32
MAHCSnijders 0:6fa73e77d49c 33 // Calculation of position joint 3 expressed in frame 0
MAHCSnijders 0:6fa73e77d49c 34 float J3x_0 = L6 + L4*cos(motor_angle1);
MAHCSnijders 0:6fa73e77d49c 35 float J3y_0 = L4*sin(motor_angle1);
MAHCSnijders 0:6fa73e77d49c 36
MAHCSnijders 0:6fa73e77d49c 37 // Calculation of Joint 2 expressed in frame 2
MAHCSnijders 0:6fa73e77d49c 38 float m_y = J3y_0 - J1y_0;
MAHCSnijders 0:6fa73e77d49c 39 float m_x = J1x_0 - J3x_0;
MAHCSnijders 0:6fa73e77d49c 40 float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3
MAHCSnijders 0:6fa73e77d49c 41 float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) );
MAHCSnijders 0:6fa73e77d49c 42 float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m
MAHCSnijders 0:6fa73e77d49c 43
MAHCSnijders 0:6fa73e77d49c 44 float t_y = J3y_0;
MAHCSnijders 0:6fa73e77d49c 45 float t_x = (L0 + L6) - J3x_0;
MAHCSnijders 0:6fa73e77d49c 46 float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3
MAHCSnijders 0:6fa73e77d49c 47 float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m
MAHCSnijders 0:6fa73e77d49c 48
MAHCSnijders 0:6fa73e77d49c 49 float q2 = PI - mu - phi; // Angle that L2 makes in frame 2
MAHCSnijders 2:92aeffa6b16e 50 float J2x_2 = L2*cos(q2);
MAHCSnijders 2:92aeffa6b16e 51 float J2y_2 = L2*sin(q2);
MAHCSnijders 0:6fa73e77d49c 52
MAHCSnijders 1:3dfde431f833 53 // Calculation of Joint 2 expressed in frame 0
MAHCSnijders 0:6fa73e77d49c 54 float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1
MAHCSnijders 0:6fa73e77d49c 55 float J1y_1 = L1*sin(motor_angle2);
MAHCSnijders 2:92aeffa6b16e 56 float J2x_0 = J2x_2*cos(motor_angle2) - J2y_2 * sin(motor_angle2) + J1x_1 + L0 + L6; // Joint 2 expressed in frame 0
MAHCSnijders 2:92aeffa6b16e 57 float J2y_0 = J2x_2*sin(motor_angle2) + J2y_2 * cos(motor_angle2) + J1y_1;
MAHCSnijders 2:92aeffa6b16e 58
MAHCSnijders 2:92aeffa6b16e 59 // Calculation of End-effector
MAHCSnijders 2:92aeffa6b16e 60 float f_x = J2x_0 - J3x_0;
MAHCSnijders 2:92aeffa6b16e 61 float f_y = J2y_0;
MAHCSnijders 2:92aeffa6b16e 62 float f = sqrt(pow(f_x,2) + pow(f_y,2)); // Radius between motor 1 and Joint 2
MAHCSnijders 2:92aeffa6b16e 63 float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) ); // Angle between L3 and L4
MAHCSnijders 2:92aeffa6b16e 64 float omega = PI - xhi; // Angle between L4 and L5
MAHCSnijders 2:92aeffa6b16e 65 float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega)); // Radius between end effector and motor 1
MAHCSnijders 0:6fa73e77d49c 66
MAHCSnijders 3:c006b3e9a41c 67 float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) ); // Angle between n and L4
MAHCSnijders 3:c006b3e9a41c 68 float rho = PI - theta - motor_angle1; // Angle between n and L4
MAHCSnijders 2:92aeffa6b16e 69
MAHCSnijders 2:92aeffa6b16e 70 float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector
MAHCSnijders 2:92aeffa6b16e 71 float Pe_y = n*sin(rho); // x-coordinate end-effector
MAHCSnijders 2:92aeffa6b16e 72
MAHCSnijders 4:6db7291caa6d 73
MAHCSnijders 4:6db7291caa6d 74 // Implementing stops for safety
MAHCSnijders 4:6db7291caa6d 75 // 45 < Motor_angle1 < 70 graden
MAHCSnijders 4:6db7291caa6d 76 if (0.785398 < motor_angle1 && motor_angle1 < 1.22173)
MAHCSnijders 4:6db7291caa6d 77 {
MAHCSnijders 4:6db7291caa6d 78 ForwardKinematics_ticker.detach();
MAHCSnijders 4:6db7291caa6d 79 safetyLED = 0;
MAHCSnijders 4:6db7291caa6d 80 }
MAHCSnijders 4:6db7291caa6d 81
MAHCSnijders 4:6db7291caa6d 82 // -42 < Motor_angle2 < 85 graden
MAHCSnijders 4:6db7291caa6d 83 if (-0.733038 < motor_angle2 && motor_angle2 < 1.48353)
MAHCSnijders 4:6db7291caa6d 84 {
MAHCSnijders 4:6db7291caa6d 85 ForwardKinematics_ticker.detach();
MAHCSnijders 4:6db7291caa6d 86 safetyLED = 0;
MAHCSnijders 4:6db7291caa6d 87 }
MAHCSnijders 4:6db7291caa6d 88
MAHCSnijders 4:6db7291caa6d 89 // Delta < 170 graden
MAHCSnijders 4:6db7291caa6d 90 if (delta < 2.96706)
MAHCSnijders 4:6db7291caa6d 91 {
MAHCSnijders 4:6db7291caa6d 92 ForwardKinematics_ticker.detach();
MAHCSnijders 4:6db7291caa6d 93 safetyLED = 0;
MAHCSnijders 4:6db7291caa6d 94 }
MAHCSnijders 4:6db7291caa6d 95
MAHCSnijders 2:92aeffa6b16e 96 // NEEDS TO RETURN END-EFFECTOR COORDINATES
MAHCSnijders 2:92aeffa6b16e 97 // return Pe_x;
MAHCSnijders 2:92aeffa6b16e 98 // return Pe_y;
MAHCSnijders 0:6fa73e77d49c 99 }
MAHCSnijders 0:6fa73e77d49c 100
MAHCSnijders 0:6fa73e77d49c 101
MAHCSnijders 0:6fa73e77d49c 102 int main()
MAHCSnijders 0:6fa73e77d49c 103 {
MAHCSnijders 4:6db7291caa6d 104 safetyLED = 1;
MAHCSnijders 0:6fa73e77d49c 105 while (true) {
MAHCSnijders 4:6db7291caa6d 106 ForwardKinematics_ticker.attach(ForwardKinematics,0.5);
MAHCSnijders 0:6fa73e77d49c 107 }
MAHCSnijders 0:6fa73e77d49c 108 }