Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
main.cpp
- Committer:
- MAHCSnijders
- Date:
- 2018-10-30
- Revision:
- 0:6fa73e77d49c
- Child:
- 1:3dfde431f833
File content as of revision 0:6fa73e77d49c:
#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); } }