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