Biorobotics
/
frdm_kinematics
Direct and inverse kinematics of a scara robotic arm, based on trigonometry
Diff: main.cpp
- Revision:
- 2:47913550ec8d
- Parent:
- 1:dd8fca950fed
- Child:
- 3:1e8cfc385b48
diff -r dd8fca950fed -r 47913550ec8d main.cpp --- a/main.cpp Mon Oct 19 12:40:29 2015 +0000 +++ b/main.cpp Mon Oct 19 13:05:40 2015 +0000 @@ -25,9 +25,8 @@ { double L1 = 0.34; double L2 = 0.26; - double para2 = (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2); - double theta_2 = acos(para2)* 180 / PI; + double theta_2 = acos( (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2) )* 180 / PI; double theta_total = atan2 (y,x) * 180 / PI; double theta_Q = acos( (x*x+y*y+L1*L1-L2*L2) / (2*L1*sqrt(x*x+y*y)) )* 180 / PI; double theta_1 = theta_total - theta_Q; @@ -41,8 +40,8 @@ { pc.baud(115200); - double theta_1 = 1.0; - double theta_2 = 1.0; + double theta_1 = 0.0; + double theta_2 = 0.0; double xy[2] = {0.0,0.0}; @@ -51,7 +50,7 @@ while (true) { wait(0.5f); DirectKin(xy[0],xy[1], theta_1, theta_2); - // pc.printf("%4.4f %4.4f \n",xy[0], xy[1]); + pc.printf("%4.4f %4.4f \n",xy[0], xy[1]); IndirectKin(theta_1, theta_2, xy[0], xy[1]); pc.printf("%4.4f %4.4f \n",theta_1, theta_2);