Diff: move.cpp

Revision:

5:0e18cf25291a

Parent:

4:4c574be6325c

Child:

6:0aa97a99c9cb

--- a/move.cpp	Tue Sep 06 06:47:25 2016 +0000
+++ b/move.cpp	Tue Sep 06 09:31:07 2016 +0000
@@ -17,7 +17,7 @@
 
 //const int allowlength=5;
 //const float allowdegree=0.02;
-const int rightspeed=120;
+const int rightspeed=70;
 const int leftspeed=rightspeed + 2;
 const int turnspeed=30*2;
 const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
@@ -122,86 +122,85 @@
     wait(0.5);
 }
 
-
-void pmove_to_dir(int direction, int x, int y)
-{
-    float k = 0.9;
-    int   k_theta = 2;
-
+void pmove(int x, int y)
+{ 
+    float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
+    
+    int   k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
+    
     int length, dx, dy;
     int *d_length, *disorder;
+    int absd_length;
     float dtheta, ptheta;
     float daikei;
 
-    update();
+    char direction;
+    if(abs(x - coordinateX()) > abs(y - coordinateY())) {
+        y = coordinateY();
+        direction = X_PLUS;
+        length = abs(x - coordinateX());
+        d_length = &dx;
+        disorder = &dy;
+    }
+    else {
+        x = coordinateX();
+        direction = Y_PLUS;
+        length = abs(y - coordinateY());
+        d_length = &dy;
+        disorder = &dx;
+    }
+
+    if(*d_length < 0)
+        direction *= -1;
 
     switch(direction) {
         case X_PLUS:
-            length = x - coordinateX();
-            y = coordinateY();
-            d_length = &dx;
-            disorder = &dy;
-            turn_abs_rad(0);
-
             ptheta = 0;
             break;
         case Y_PLUS:
-            length = y - coordinateY();
-            x = coordinateX();
-            d_length = &dy;
-            disorder = &dx;
             k *= -1;
-            turn_abs_rad(PI/2);
             ptheta = PI/2;
             break;
         case X_MINUS:
-            length = x - coordinateX();
-            y = coordinateY();
-            d_length = &dx;
-            disorder = &dy;
             k *= -1;
-            turn_abs_rad(PI);
-            //pc.printf("finish_turn");
             ptheta = PI;
             break;
         case Y_MINUS:
-            length = y - coordinateY();
-            x = coordinateX();
-            d_length = &dy;
-            disorder = &dx;
-            turn_abs_rad(PI*3/2);
             ptheta = PI*3/2;
             break;
         default:
             return;
     }
 
+    turn_abs_rad(ptheta);
+
     if(length == 0) return;
 
     while(1) {
         update();
-        dx = coordinateX() - x;
-        dy = coordinateY() - y;
+        dx = x - coordinateX();
+        dy = y - coordinateY();
         dtheta = coordinateTheta() - ptheta;
 
-
         if(*disorder>1) {
             *disorder = 1;
         } else if(*disorder<-1) {
             *disorder = -1;
         }
 
+        absd_length = abs(*d_length);
 
-        if(abs(*d_length) > abs(length) -200) {
-            daikei = (float)(abs(length)-abs(*d_length)) / 200;
-        } else if(abs(*d_length) < 150) {
-            daikei = (float)(abs(*d_length)) / 150;
+        if(absd_length > length - 30) {
+            daikei = (length - absd_length) / 30.0;
+        } else if(absd_length < 150) {
+            daikei = absd_length / 150.0;
         } else
             daikei = 1;
 
-        move(daikei * (rightspeed*7/8.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/8.0, daikei * (leftspeed*7/8.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/8.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);
 
-        if((direction > 0 && *d_length >= 0) || (direction < 0 &&  *d_length <= 0)) {
+        if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
             move(0, 0);
             break;
         }
@@ -212,6 +211,7 @@
     wait(0.5);
 }
 
+
 void back(int x, int y)
 { 
     float k = 0.9;
@@ -239,23 +239,23 @@
         disorder = &dx;
     }
 
-    if(d_length < 0)
+    if(*d_length < 0)
         direction *= -1;
 
     switch(direction) {
         case X_PLUS:
-            ptheta = PI;
+            ptheta = 0;
             break;
         case Y_PLUS:
             k *= -1;
-            ptheta = PI*3/2;
+            ptheta = PI/2;
             break;
         case X_MINUS:
             k *= -1;
-            ptheta = 0;
+            ptheta = PI;
             break;
         case Y_MINUS:
-            ptheta = PI/2;
+            ptheta = PI*3/2;
             break;
         default:
             return;