Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

Diff: move.cpp

Revision:
19:0b0ed6b22de9
Parent:
18:4c812a3c7411
Child:
20:908443fd2b09
diff -r 4c812a3c7411 -r 0b0ed6b22de9 move.cpp
--- a/move.cpp	Sat Sep 10 12:14:26 2016 +0000
+++ b/move.cpp	Sat Sep 10 12:25:58 2016 +0000
@@ -313,7 +313,7 @@
     float daikei;
 
 
-    length = sqrt((double)x*x + (double)y*y);
+    length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
     
     pc2.printf("length:%f", length);
     
@@ -323,7 +323,9 @@
     }
 
     ptheta = giveatan(x, y);
-
+    
+    ptheta += nearPi(coordinateTheta() - ptheta);
+    
     turnrad(ptheta);
 
     virtual_setup();