Diff: move.cpp

Revision:

12:f41918f71131

Parent:

10:6d38d1b6cad5

Child:

13:176e543e6d64

--- a/move.cpp	Fri Sep 09 05:41:17 2016 +0000
+++ b/move.cpp	Fri Sep 09 08:55:56 2016 +0000
@@ -25,8 +25,10 @@
 const int rightspeed=70;
 const int leftspeed=rightspeed + 2;
 const int turnspeed=30*2;
+const int hosei_turnspeed = 25;
 const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
 const int k_theta = 2;
+const int max_disorder = 3;
 //const float PIfact=2.89;
 
 
@@ -85,9 +87,9 @@
         update();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
-            move(-20, 20);
+            move(-hosei_turnspeed, hosei_turnspeed);
         } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
-            move(20, -20);
+            move(hosei_turnspeed, -hosei_turnspeed);
         } else {
             move(0,0);
             return;
@@ -164,7 +166,7 @@
     update();
     dx = x - coordinateX();
     dy = y - coordinateY();
-
+    
     if(*d_length < 0)   //x,y減少方向なら、*d_length<0
         direction *= -1;
 
@@ -188,7 +190,9 @@
         default:
             return;
     }
-
+    
+    ptheta = nearPi(coordinateTheta() - ptheta);
+    
     turn_abs_rad(ptheta);
 
     if(length == 0) return;
@@ -199,10 +203,10 @@
         dy = y - coordinateY();
         dtheta = coordinateTheta() - ptheta;
 
-        if(*disorder>2) {
-            *disorder = 2;
-        } else if(*disorder<-2) {
-            *disorder = -2;
+        if(*disorder>max_disorder) {
+            *disorder = max_disorder;
+        } else if(*disorder<-max_disorder) {
+            *disorder = -max_disorder;
         }
 
         absd_length = abs(*d_length);
@@ -257,10 +261,10 @@
         dx = coordinateX() - px;
         dy = coordinateY() - py;
 
-        if(dy>2) {
-            dy = 2;
-        } else if(dy<-2) {
-            dy = -2;
+        if(dy>max_disorder) {
+            dy = max_disorder;
+        } else if(dy<-max_disorder) {
+            dy = -max_disorder;
         }
 
 
@@ -279,3 +283,18 @@
 
     red = 0;
 }
+
+float nearPi(float rad)
+{
+    float npi  = 0;
+    
+    while(1)
+    {
+        if(rad > npi + PI)
+            npi += 2*PI;
+        else if(rad < npi - PI)
+            npi -= 2*PI;
+        else
+            return npi;
+    }
+}