![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Direct and inverse kinematics of a scara robotic arm, based on trigonometry
Diff: main.cpp
- Revision:
- 0:8d4438eabc82
- Child:
- 1:dd8fca950fed
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 19 12:33:06 2015 +0000 @@ -0,0 +1,67 @@ +#include "mbed.h" +#include "MODSERIAL.h" +#define PI 3.14159265 + +MODSERIAL pc(USBTX, USBRX); + +void DirectKin(double& a, double& b, double theta1, double theta2) +{ + double L1 = 0.34; + double L2 = 0.26; + double theta1_0 = 0; + double theta2_0 = 0; + + 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)}; + + double x = r0_2[0]; + double y = r0_2[1]; + + a = x; + b = y; +} + +void IndirectKin(double&a, double& b, double x, double y) +{ + double L1 = 0.34; + double L2 = 0.26; + double para2 = (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2); + + //double theta_1 = 0.0; + double theta_2 = 0.0; + double theta_Q = 0.0; + + //double theta_2 = acos(para2)* 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; + //b = theta_2; +} + + +int main() +{ + pc.baud(115200); + + double theta_1 = 0.0; + double theta_2 = 0.0; + + double xy[2] = {0.0,0.0}; + + + + 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]); + + IndirectKin(theta_1, theta_2, xy[0], xy[1]); + pc.printf("%4.4f %4.4f \n",theta_1, theta_2); + + theta_1 = theta_1 + 90.0; + } +} \ No newline at end of file