Forward Kinematics

Dependencies:   MODSERIAL Matrix mbed

Files at this revision

API Documentation at this revision

Comitter:
MAHCSnijders
Date:
Wed Oct 31 21:02:06 2018 +0000
Parent:
5:65a609067e14
Commit message:
Fixed bug

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 65a609067e14 -r fe8712b56eb9 main.cpp
--- a/main.cpp	Wed Oct 31 14:11:08 2018 +0000
+++ b/main.cpp	Wed Oct 31 21:02:06 2018 +0000
@@ -15,10 +15,10 @@
 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 double motor_angle1;                // Current angle of motor 1 (left) based on kinematics [rad]
-volatile double motor_angle2;                // Current angle of motor 2 (right) based on kinematics [rad]
+volatile double motor_angle1;               // Current angle of motor 1 (left) based on kinematics [rad]
+volatile double motor_angle2;               // Current angle of motor 2 (right) based on kinematics [rad]
 
-DigitalOut safetyLED(LED_GREEN);             // Safety check LED
+DigitalOut safetyLED(LED_GREEN);            // Safety check LED
 
 
 // Useful stuff
@@ -37,60 +37,60 @@
     // 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 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 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 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
+    float q2 = PI - mu - phi;                                               // Angle that L2 makes in frame 2
     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 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_x = J2x_0 - L6;     // I CHANGED THIS!!!!
     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 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 theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) );          // Angle between n and L4
-    float rho = PI - theta - motor_angle1;                              // Angle between n and L4
+    float theta = acos( (pow(L4,2) - pow(L5,2) + pow(n,2))/(2*n*L4) );      // Angle between n and L4
+    float rho = PI - theta - 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
+    float Pe_x = L6 - n*cos(rho);                                           // y-coordinate end-effector
+    float Pe_y = n*sin(rho);                                                // x-coordinate end-effector
 
 
     // Implementing stops for safety
     // 45 < Motor_angle1 < 70 graden
-    if (motor_angle1 < 0.785398)                                // If motor_angle is smaller than 45 degrees
+    if (motor_angle1 < 0.785398)                                            // If motor_angle is smaller than 45 degrees
     {
         motor_angle1 = 0.785398;
         safetyLED = 0;
     }
-    else if (motor_angle1 > 1.22173)                            // If motor_angle is larger than 70 degrees
+    else if (motor_angle1 > 1.22173)                                        // If motor_angle is larger than 70 degrees
     {
         motor_angle1 = 1.22173;
         safetyLED = 0;
     }
     
     // -42 < Motor_angle2 < 85 graden
-    if (motor_angle2 < -0.733038)                               // If motor_angle is smaller than -42 degrees
+    if (motor_angle2 < -0.733038)                                           // If motor_angle is smaller than -42 degrees
     {
         motor_angle2 = -0.733038;
         safetyLED = 0;
     }
-    else if (motor_angle2 > 1.48353)                            // If motor_angle is larger than 85 degrees
+    else if (motor_angle2 > 1.48353)                                        // If motor_angle is larger than 85 degrees
     {
         motor_angle2 = 1.48353;
         safetyLED = 0;
@@ -98,7 +98,7 @@
     
     
     // Delta < 170 graden
-    if (delta > 2.96706)                                        // If delta is larger than 180 degrees
+    if (delta > 2.96706)                                                    // If delta is larger than 180 degrees
     {
         delta = 2.96706;
         safetyLED = 0;