Diff: move.cpp

Revision:

13:176e543e6d64

Parent:

12:f41918f71131

Child:

14:db58d3b0ecde

--- a/move.cpp	Fri Sep 09 08:55:56 2016 +0000
+++ b/move.cpp	Fri Sep 09 11:42:53 2016 +0000
@@ -23,12 +23,12 @@
 //const int allowlength=5;
 //const float allowdegree=0.02;
 const int rightspeed=70;
-const int leftspeed=rightspeed + 2;
+const int leftspeed=rightspeed + 3;
 const int turnspeed=30*2;
-const int hosei_turnspeed = 25;
-const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+const int hosei_turnspeed = 13;
+const float k = 0.8;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
 const int k_theta = 2;
-const int max_disorder = 3;
+const int max_disorder = 4;
 //const float PIfact=2.89;
 
 
@@ -191,7 +191,7 @@
             return;
     }
     
-    ptheta = nearPi(coordinateTheta() - ptheta);
+    ptheta += nearPi(coordinateTheta() - ptheta);
     
     turn_abs_rad(ptheta);
 
@@ -211,15 +211,15 @@
 
         absd_length = abs(*d_length);
 
-        if(absd_length > length - 30) {
+        if(absd_length < 300) {
+            daikei = absd_length / 300.0;
+        } else if(absd_length > length - 30) {
             daikei = abs(length - absd_length) / 30.0;
-        } else if(absd_length < 150) {
-            daikei = absd_length / 150.0;
         } else
             daikei = 1;
 
-        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);
+        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);
 
         //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)) {