Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
Diff: main.cpp
- Revision:
- 3:c006b3e9a41c
- Parent:
- 2:92aeffa6b16e
- Child:
- 4:6db7291caa6d
diff -r 92aeffa6b16e -r c006b3e9a41c main.cpp --- a/main.cpp Tue Oct 30 15:33:51 2018 +0000 +++ b/main.cpp Wed Oct 31 08:28:57 2018 +0000 @@ -62,8 +62,8 @@ 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 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