Diff: main.cpp

Revision:

2:47913550ec8d

Parent:

1:dd8fca950fed

Child:

3:1e8cfc385b48

--- 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);