![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Inverse kinematics
Diff: main.cpp
- Revision:
- 9:2944c792daa2
- Parent:
- 8:01eb609fe0c1
- Child:
- 10:c35d16ad1b6b
diff -r 01eb609fe0c1 -r 2944c792daa2 main.cpp --- a/main.cpp Thu Nov 01 10:29:23 2018 +0000 +++ b/main.cpp Thu Nov 01 10:37:07 2018 +0000 @@ -45,6 +45,13 @@ float alfa = acos( -(pow(r,2) - pow(L1,2) - pow(L2,2))/(2*L1*L2) ); // Angle opposite of radius r float q2 = PI - alfa; // Angle between L1 and L2 + // Delta < 170 graden SAFETY LIMITATION + if (r > 2.96706) // If delta is larger than 175 degrees than r is this + { + r = 0.2399; // I CHANGED THIS + safetyLED = 0; + } + // Calculation of motor_angle2 I CHANGED THIS!!! float beta = acos(- (pow(L2,2) - pow(r,2) - pow(L1,2))/(2*L1*r)); // Angle between r and L1 float zeta = acos(J2x_1/r); // Angle between r and x-axis of frame 1 @@ -83,14 +90,6 @@ des_motor_angle2 = 1.48353; safetyLED = 0; } - - - // Delta < 170 graden - if (delta > 2.96706) // If delta is larger than 180 degrees - { - r = 0.2399; // I CHANGED THIS - safetyLED = 0; - } }