Revision 2:92aeffa6b16e, committed 2018-10-30

Comitter:

MAHCSnijders

Date:

Tue Oct 30 15:33:51 2018 +0000

Parent:

1:3dfde431f833

Child:

3:c006b3e9a41c

Commit message:

Forward Kinematics final

Changed in this revision

--- a/main.cpp	Tue Oct 30 15:23:59 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:33:51 2018 +0000
@@ -1,9 +1,6 @@
 #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]
@@ -48,22 +45,38 @@
     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);
+    float J2x_2 = L2*cos(q2);
+    float J2y_2 = L2*sin(q2);
 
     // Calculation of Joint 2 expressed in frame 0
     float J1x_1 = L1*cos(motor_angle2);                              // Joint 1 expressed in frame 1
     float J1y_1 = L1*sin(motor_angle2); 
+    float J2x_0 = J2x_2*cos(motor_angle2) - J2y_2 * sin(motor_angle2) + J1x_1 + L0 + L6;    // Joint 2 expressed in frame 0
+    float J2y_0 = J2x_2*sin(motor_angle2) + J2y_2 * cos(motor_angle2) + J1y_1;
+ 
+    // Calculation of End-effector
+    float f_x = J2x_0 - J3x_0;
+    float f_y = J2y_0;
+    float f = sqrt(pow(f_x,2) + pow(f_y,2));                              // Radius between motor 1 and Joint 2
+    float xhi = acos( -(pow(f,2) - pow(L3,2) - pow(L4,2))/(2*L3*L4) );         // Angle between L3 and L4
+    float omega = PI - xhi;                                       // Angle between L4 and L5
+    float n = sqrt(pow(L4,2) + pow(L5,2) - 2*L4*L5*cos(omega));          // Radius between end effector and motor 1
 
-        
+    float eta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) );          // Angle between n and L4
+    float rho = PI - eta - motor_angle1;                              // Angle between n and L4
+
+    float Pe_x = L6 - n*cos(rho);                                 // y-coordinate end-effector
+    float Pe_y = n*sin(rho);                                      // x-coordinate end-effector
+
+    // NEEDS TO RETURN END-EFFECTOR COORDINATES
+//    return Pe_x;
+//    return Pe_y;    
 }
 
 
 int main()
 {
-    pc.baud(115200);
     while (true) {
     ForwardKinematics_ticker.attach(ForwardKinematics,2);
-    pc.printf("%d\n",H);
     }
 }
\ No newline at end of file