Diff: move.cpp

Revision:

14:db58d3b0ecde

Parent:

13:176e543e6d64

Child:

15:403e9c57c1a1

diff -r 176e543e6d64 -r db58d3b0ecde move.cpp
--- a/move.cpp	Fri Sep 09 11:42:53 2016 +0000
+++ b/move.cpp	Sat Sep 10 03:15:19 2016 +0000
@@ -23,12 +23,11 @@
 //const int allowlength=5;
 //const float allowdegree=0.02;
 const int rightspeed=70;
-const int leftspeed=rightspeed + 3;
+const int leftspeed=rightspeed + 4;
 const int turnspeed=30*2;
 const int hosei_turnspeed = 13;
-const float k = 0.8;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
-const int k_theta = 2;
 const int max_disorder = 4;
+const float ratio = 1.0/7.5;
 //const float PIfact=2.89;
 
 
@@ -84,7 +83,7 @@
     if(cw) np = 1;
     else   np = -1;
     while(1) {
-        update();
+        update_np();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
             move(-hosei_turnspeed, hosei_turnspeed);
@@ -102,7 +101,7 @@
 {
     green = 1;
 
-    update();
+    update_np();
 
     int np;
     if(coordinateTheta() > rad) np = 1;
@@ -112,7 +111,7 @@
     move((-np)*(turnspeed+2), (np)*turnspeed);
 
     while(1) {
-        update();
+        update_np();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
             move(0,0);
@@ -137,9 +136,8 @@
 {
     yellow = 1;
 
-    float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
-
-    int   k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
+    float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
+    int   k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
 
     int length, dx, dy;
     int *d_length, *disorder;
@@ -196,9 +194,11 @@
     turn_abs_rad(ptheta);
 
     if(length == 0) return;
-
+    
+    int i = 0;
+    
     while(1) {
-        update();
+        update_np();
         dx = x - coordinateX();
         dy = y - coordinateY();
         dtheta = coordinateTheta() - ptheta;
@@ -210,16 +210,23 @@
         }
 
         absd_length = abs(*d_length);
-
-        if(absd_length < 300) {
+        
+        
+        if(i++ < 5) {
+            daikei = i/5;
+        }
+        else if(absd_length < 300) {
             daikei = absd_length / 300.0;
-        } else if(absd_length > length - 30) {
+        } 
+/*
+        else if(absd_length > length - 30) {
             daikei = abs(length - absd_length) / 30.0;
-        } else
+*/
+        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*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
+             daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
 
         //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)) {
@@ -254,10 +261,10 @@
 
     if(length == 0) return;
 
-    turn_abs_rad(PI);
+    turn_abs_rad(PI + nearPi(coordinateTheta() - PI));
 
     while(1) {
-        update();
+        update_np();
         dx = coordinateX() - px;
         dy = coordinateY() - py;