Diff: main.cpp

Revision:

0:6fa73e77d49c

Child:

1:3dfde431f833

--- /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