Biorobotics 7
/
Inverse_kinematic
Inverse kinematics
Diff: main.cpp
- Revision:
- 8:01eb609fe0c1
- Parent:
- 7:f3c4ecbf864e
- Child:
- 9:2944c792daa2
diff -r f3c4ecbf864e -r 01eb609fe0c1 main.cpp --- 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; } }