Diff: move.cpp

Revision:

9:7e99a1c80656

Parent:

8:3bf4addaaedd

Child:

10:6d38d1b6cad5

--- a/move.cpp	Wed Sep 07 07:36:49 2016 +0000
+++ b/move.cpp	Thu Sep 08 10:07:06 2016 +0000
@@ -85,9 +85,9 @@
         update();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
-            move(-12, 12);
+            move(-14, 14);
         } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
-            move(12, -12);
+            move(14, -14);
         } else {
             move(0,0);
             return;
@@ -194,7 +194,7 @@
     if(length == 0) return;
 
     while(1) {
-        update_np();
+        update();
         dx = x - coordinateX();
         dy = y - coordinateY();
         dtheta = coordinateTheta() - ptheta;
@@ -214,10 +214,10 @@
         } else
             daikei = 1;
 
-        move(daikei * (rightspeed*6/7.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/7.0,
-             daikei * (leftspeed*6/7.0  - k*(*disorder) + k_theta*dtheta) + leftspeed/7.0);
+        move(daikei * (rightspeed*4/5.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/5.0,
+             daikei * (leftspeed*4/5.0  - k*(*disorder) + k_theta*dtheta) + leftspeed/5.0);
 
-        pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
+        //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
         if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
             move(0, 0);
             break;