Biorobotics
/
frdm_kinematics
Direct and inverse kinematics of a scara robotic arm, based on trigonometry
Diff: main.cpp
- Revision:
- 3:1e8cfc385b48
- Parent:
- 2:47913550ec8d
- Child:
- 4:022b69da83f8
--- a/main.cpp Mon Oct 19 13:05:40 2015 +0000 +++ b/main.cpp Mon Oct 19 13:23:58 2015 +0000 @@ -9,7 +9,7 @@ double L1 = 0.34; double L2 = 0.26; double theta1_0 = 0; - double theta2_0 = 0; + double theta2_0 = 180; double r0_1[2] = {L1*cos((theta1_0+theta1) * PI / 180.0) , L1*sin((theta1_0+theta1) * PI / 180.0)}; double r0_2[2] = {L1*cos((theta1_0+theta1) * PI / 180.0)+L2*cos((theta1_0+theta1+theta2+theta2_0)* PI / 180.0), L1*sin((theta1_0+theta1) * PI / 180.0)+L2*sin((theta1_0+theta1+theta2+theta2_0)* PI / 180.0)}; @@ -24,15 +24,17 @@ void IndirectKin(double&a, double& b, double x, double y) { double L1 = 0.34; - double L2 = 0.26; + double L2 = 0.26; + double theta1_0 = 30; + double theta2_0 = 46; 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; - - a = theta_1; - b = theta_2; + + a = theta_1 - theta1_0; + b = theta_2 - theta2_0; } @@ -40,8 +42,8 @@ { pc.baud(115200); - double theta_1 = 0.0; - double theta_2 = 0.0; + double theta_1 = 0.000001; + double theta_2 = 0.000001; double xy[2] = {0.0,0.0}; @@ -50,11 +52,11 @@ 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); - + pc.printf("%4.4f %4.4f \n\n",theta_1, theta_2); + theta_1 = theta_1 + 90.0; } } \ No newline at end of file