Diff: main.cpp

Revision:

8:01eb609fe0c1

Parent:

7:f3c4ecbf864e

Child:

9:2944c792daa2

--- a/main.cpp	Thu Nov 01 10:01:25 2018 +0000
+++ b/main.cpp	Thu Nov 01 10:29:23 2018 +0000
@@ -26,7 +26,7 @@
     float q4 = PI - omega;                                                  // Angle of joint 3 between L3 and L4
     float theta = acos( -(pow(L5,2) - pow(n,2) - pow(L4,2))/(2*n*L4) );     // Angle between n and L4
     float lambda_stuff = Pe_y/(L6-Pe_x);
-    float lambda = PI - atan(lambda_stuff);                               // Entire angle between L0 and n
+    float lambda = PI - atan(lambda_stuff);                                 // Entire angle between L0 and n
     des_motor_angle1 = lambda - theta;
     float J3x_0 = L6 + L4*cos(des_motor_angle1);                            // x-coordinate of joint 3 in frame 0
     float J3y_0 = L4*sin(des_motor_angle1);                                 // y-coordinate of joint 3 in frame 0
@@ -88,7 +88,7 @@
     // Delta < 170 graden
     if (delta > 2.96706)                                        // If delta is larger than 180 degrees
     {
-        delta = 2.96706;
+        r = 0.2399;     // I CHANGED THIS
         safetyLED = 0;
     }
 }