Biorobotics 7 / Mbed 2 deprecated Forward_kinematics

Forward Kinematics

Dependencies:   MODSERIAL Matrix mbed

main.cpp@0:6fa73e77d49c, 2018-10-30 (annotated)

Committer:
MAHCSnijders
Date:
Tue Oct 30 15:19:17 2018 +0000
Revision:
0:6fa73e77d49c
Child:
1:3dfde431f833
Forward kinematics met MATRIX!! (fout)

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 #include "MODSERIAL.h"
MAHCSnijders 0:6fa73e77d49c 5
MAHCSnijders 0:6fa73e77d49c 6 MODSERIAL pc(USBTX, USBRX);
MAHCSnijders 0:6fa73e77d49c 7
MAHCSnijders 0:6fa73e77d49c 8 // Stuff die waarschijnlijk weg kan??
MAHCSnijders 0:6fa73e77d49c 9 const float L0 = 0.15; // Length between two motors [meter]
MAHCSnijders 0:6fa73e77d49c 10 const float L1 = 0.10; // Length first beam from right motor2 [meter]
MAHCSnijders 0:6fa73e77d49c 11 const float L2 = 0.30; // Length second beam from right motor2 [meter]
MAHCSnijders 0:6fa73e77d49c 12 const float L3 = 0.15; // Length beam between L2 and L4 [meter]
MAHCSnijders 0:6fa73e77d49c 13 const float L4 = 0.30; // Length first beam from left motor1 [meter]
MAHCSnijders 0:6fa73e77d49c 14 const float L5 = 0.35; // Length from L3 to end-effector [meter]
MAHCSnijders 0:6fa73e77d49c 15 const double PI = 3.14159265359;
MAHCSnijders 0:6fa73e77d49c 16
MAHCSnijders 0:6fa73e77d49c 17 // DEZE MOET ER NOG WEL IN!!!
MAHCSnijders 0:6fa73e77d49c 18 const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter]
MAHCSnijders 0:6fa73e77d49c 19 volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter]
MAHCSnijders 0:6fa73e77d49c 20 volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter]
MAHCSnijders 0:6fa73e77d49c 21 volatile float motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad]
MAHCSnijders 0:6fa73e77d49c 22 volatile float motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad]
MAHCSnijders 0:6fa73e77d49c 23
MAHCSnijders 0:6fa73e77d49c 24
MAHCSnijders 0:6fa73e77d49c 25 // Useful stuff
MAHCSnijders 0:6fa73e77d49c 26 Matrix H(3,3); // 2x2 matrix
MAHCSnijders 0:6fa73e77d49c 27 Matrix J2_2(3,1); //
MAHCSnijders 0:6fa73e77d49c 28 Ticker ForwardKinematics_ticker;
MAHCSnijders 0:6fa73e77d49c 29 float J2x_2;
MAHCSnijders 0:6fa73e77d49c 30 float J2y_2;
MAHCSnijders 0:6fa73e77d49c 31
MAHCSnijders 0:6fa73e77d49c 32 void ForwardKinematics()
MAHCSnijders 0:6fa73e77d49c 33 {
MAHCSnijders 0:6fa73e77d49c 34 // Calculation of position joint 1 expressed in frame 0
MAHCSnijders 0:6fa73e77d49c 35 float J1x_0 = L6 + L0 + L1*cos(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 36 float J1y_0 = L1*sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 37
MAHCSnijders 0:6fa73e77d49c 38 // Calculation of position joint 3 expressed in frame 0
MAHCSnijders 0:6fa73e77d49c 39 float J3x_0 = L6 + L4*cos(motor_angle1);
MAHCSnijders 0:6fa73e77d49c 40 float J3y_0 = L4*sin(motor_angle1);
MAHCSnijders 0:6fa73e77d49c 41
MAHCSnijders 0:6fa73e77d49c 42 // Calculation of Joint 2 expressed in frame 2
MAHCSnijders 0:6fa73e77d49c 43 float m_y = J3y_0 - J1y_0;
MAHCSnijders 0:6fa73e77d49c 44 float m_x = J1x_0 - J3x_0;
MAHCSnijders 0:6fa73e77d49c 45 float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3
MAHCSnijders 0:6fa73e77d49c 46 float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) );
MAHCSnijders 0:6fa73e77d49c 47 float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m
MAHCSnijders 0:6fa73e77d49c 48
MAHCSnijders 0:6fa73e77d49c 49 float t_y = J3y_0;
MAHCSnijders 0:6fa73e77d49c 50 float t_x = (L0 + L6) - J3x_0;
MAHCSnijders 0:6fa73e77d49c 51 float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3
MAHCSnijders 0:6fa73e77d49c 52 float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m
MAHCSnijders 0:6fa73e77d49c 53
MAHCSnijders 0:6fa73e77d49c 54 float q2 = PI - mu - phi; // Angle that L2 makes in frame 2
MAHCSnijders 0:6fa73e77d49c 55 J2x_2 = L2*cos(q2);
MAHCSnijders 0:6fa73e77d49c 56 J2y_2 = L2*sin(q2);
MAHCSnijders 0:6fa73e77d49c 57
MAHCSnijders 0:6fa73e77d49c 58 // Coordinate transformation for Joint 2
MAHCSnijders 0:6fa73e77d49c 59
MAHCSnijders 0:6fa73e77d49c 60 float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1
MAHCSnijders 0:6fa73e77d49c 61 float J1y_1 = L1*sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 62
MAHCSnijders 0:6fa73e77d49c 63
MAHCSnijders 0:6fa73e77d49c 64 H;
MAHCSnijders 0:6fa73e77d49c 65 //float J2_1 = H*J2_2; // Homogenous coordinates Joint 2 in frame 1
MAHCSnijders 0:6fa73e77d49c 66 //float J2x_0 = J2_1(1) + L0 + L6; // x-coordinate Joint 2 in frame 0
MAHCSnijders 0:6fa73e77d49c 67 //float J2y_0 = J2_1(2);
MAHCSnijders 0:6fa73e77d49c 68
MAHCSnijders 0:6fa73e77d49c 69 // DEZE MATRIXMULTIPLICATIES MOETEN OOK IN EEN MATRIX FORMULE GEMAAKT WORDEN. MET STATIC VARIABLES KAN JE DAN NIEUWE MATRIX MAKEN
MAHCSnijders 0:6fa73e77d49c 70 // DIE BESTAAT UIT DE COMPONENTEN VAN DE ANDERE MATRICES
MAHCSnijders 0:6fa73e77d49c 71
MAHCSnijders 0:6fa73e77d49c 72
MAHCSnijders 0:6fa73e77d49c 73
MAHCSnijders 0:6fa73e77d49c 74 }
MAHCSnijders 0:6fa73e77d49c 75
MAHCSnijders 0:6fa73e77d49c 76 Matrix ComputeH(void) // Making homogeneous matrix for frame 2 to 1 transformation
MAHCSnijders 0:6fa73e77d49c 77 {
MAHCSnijders 0:6fa73e77d49c 78 double a = cos(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 79 double b = - sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 80 double c = L1*cos(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 81 double d = sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 82 double e = cos(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 83 double f = L1*sin(motor_angle2);
MAHCSnijders 0:6fa73e77d49c 84 double g = 0;
MAHCSnijders 0:6fa73e77d49c 85 double h = 0;
MAHCSnijders 0:6fa73e77d49c 86 double i = 1;
MAHCSnijders 0:6fa73e77d49c 87
MAHCSnijders 0:6fa73e77d49c 88 H << a << b << c
MAHCSnijders 0:6fa73e77d49c 89 << d << e << f
MAHCSnijders 0:6fa73e77d49c 90 << g << h << i;
MAHCSnijders 0:6fa73e77d49c 91 return H;
MAHCSnijders 0:6fa73e77d49c 92 }
MAHCSnijders 0:6fa73e77d49c 93
MAHCSnijders 0:6fa73e77d49c 94 Matrix ComputeJ2_2(void) // Homogenous coordinates Joint 2 in frame 2
MAHCSnijders 0:6fa73e77d49c 95 {
MAHCSnijders 0:6fa73e77d49c 96 double a = J2x_2;
MAHCSnijders 0:6fa73e77d49c 97 double b = J2y_2;
MAHCSnijders 0:6fa73e77d49c 98 double c = 1;
MAHCSnijders 0:6fa73e77d49c 99
MAHCSnijders 0:6fa73e77d49c 100 J2_2 << a
MAHCSnijders 0:6fa73e77d49c 101 << b
MAHCSnijders 0:6fa73e77d49c 102 << c;
MAHCSnijders 0:6fa73e77d49c 103 return J2_2;
MAHCSnijders 0:6fa73e77d49c 104 }
MAHCSnijders 0:6fa73e77d49c 105
MAHCSnijders 0:6fa73e77d49c 106
MAHCSnijders 0:6fa73e77d49c 107 int main()
MAHCSnijders 0:6fa73e77d49c 108 {
MAHCSnijders 0:6fa73e77d49c 109 pc.baud(115200);
MAHCSnijders 0:6fa73e77d49c 110 while (true) {
MAHCSnijders 0:6fa73e77d49c 111 ForwardKinematics_ticker.attach(ForwardKinematics,2);
MAHCSnijders 0:6fa73e77d49c 112 pc.printf("%d\n",H);
MAHCSnijders 0:6fa73e77d49c 113 }
MAHCSnijders 0:6fa73e77d49c 114 }