Diff: main.cpp

Revision:

0:8d4438eabc82

Child:

1:dd8fca950fed

diff -r 000000000000 -r 8d4438eabc82 main.cpp
--- /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