Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
Diff: main.cpp
- Revision:
- 0:6fa73e77d49c
- Child:
- 1:3dfde431f833
diff -r 000000000000 -r 6fa73e77d49c main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 30 15:19:17 2018 +0000 @@ -0,0 +1,114 @@ +#include "mbed.h" +#include "math.h" +#include "Matrix.h" +#include "MODSERIAL.h" + +MODSERIAL pc(USBTX, USBRX); + +// Stuff die waarschijnlijk weg kan?? +const float L0 = 0.15; // Length between two motors [meter] +const float L1 = 0.10; // Length first beam from right motor2 [meter] +const float L2 = 0.30; // Length second beam from right motor2 [meter] +const float L3 = 0.15; // Length beam between L2 and L4 [meter] +const float L4 = 0.30; // Length first beam from left motor1 [meter] +const float L5 = 0.35; // Length from L3 to end-effector [meter] +const double PI = 3.14159265359; + +// DEZE MOET ER NOG WEL IN!!! +const float L6 = 1.0; // Length beam between frame 0 and motor 1 [meter] +volatile static float Pe_x_cur; // Current x-coordinate of end-effector from frame 0 [meter] +volatile static float Pe_y_cur; // Current y-coordinate of end-effector from frame 0 [meter] +volatile float motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] +volatile float motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] + + +// Useful stuff +Matrix H(3,3); // 2x2 matrix +Matrix J2_2(3,1); // +Ticker ForwardKinematics_ticker; +float J2x_2; +float J2y_2; + +void ForwardKinematics() +{ + // Calculation of position joint 1 expressed in frame 0 + float J1x_0 = L6 + L0 + L1*cos(motor_angle2); + float J1y_0 = L1*sin(motor_angle2); + + // Calculation of position joint 3 expressed in frame 0 + float J3x_0 = L6 + L4*cos(motor_angle1); + float J3y_0 = L4*sin(motor_angle1); + + // Calculation of Joint 2 expressed in frame 2 + float m_y = J3y_0 - J1y_0; + float m_x = J1x_0 - J3x_0; + float m = sqrt(pow(m_y,2) + pow(m_x,2)); // Radius between Joint 1 and 3 + float delta = acos(- ( pow(m,2) - pow(L2,2) - pow(L3,2))/(2*L2*L3) ); + float mu = acos( (pow(L2,2) - pow(L3,2) + pow(m,2))/(2*m*L2) ); // Angle between L2 and m + + float t_y = J3y_0; + float t_x = (L0 + L6) - J3x_0; + float t = sqrt(pow(t_y,2) + pow(t_x,2)); // Radius between frame 1 and Joint 3 + float phi = acos( (pow(L1,2) - pow(t,2) + pow(m,2))/(2*m*L1) ); // Angle between L1 and m + + float q2 = PI - mu - phi; // Angle that L2 makes in frame 2 + J2x_2 = L2*cos(q2); + J2y_2 = L2*sin(q2); + + // Coordinate transformation for Joint 2 + + float J1x_1 = L1*cos(motor_angle2); // Joint 1 expressed in frame 1 + float J1y_1 = L1*sin(motor_angle2); + + + H; + //float J2_1 = H*J2_2; // Homogenous coordinates Joint 2 in frame 1 + //float J2x_0 = J2_1(1) + L0 + L6; // x-coordinate Joint 2 in frame 0 + //float J2y_0 = J2_1(2); + + // DEZE MATRIXMULTIPLICATIES MOETEN OOK IN EEN MATRIX FORMULE GEMAAKT WORDEN. MET STATIC VARIABLES KAN JE DAN NIEUWE MATRIX MAKEN + // DIE BESTAAT UIT DE COMPONENTEN VAN DE ANDERE MATRICES + + + +} + +Matrix ComputeH(void) // Making homogeneous matrix for frame 2 to 1 transformation +{ + double a = cos(motor_angle2); + double b = - sin(motor_angle2); + double c = L1*cos(motor_angle2); + double d = sin(motor_angle2); + double e = cos(motor_angle2); + double f = L1*sin(motor_angle2); + double g = 0; + double h = 0; + double i = 1; + + H << a << b << c + << d << e << f + << g << h << i; + return H; +} + +Matrix ComputeJ2_2(void) // Homogenous coordinates Joint 2 in frame 2 +{ + double a = J2x_2; + double b = J2y_2; + double c = 1; + + J2_2 << a + << b + << c; + return J2_2; +} + + +int main() +{ + pc.baud(115200); + while (true) { + ForwardKinematics_ticker.attach(ForwardKinematics,2); + pc.printf("%d\n",H); + } +} \ No newline at end of file