Diff: move.cpp

Revision:

4:4c574be6325c

Parent:

3:cecaa0154f92

Child:

5:0e18cf25291a

--- a/move.cpp	Mon Sep 05 11:08:19 2016 +0000
+++ b/move.cpp	Tue Sep 06 06:47:25 2016 +0000
@@ -19,7 +19,7 @@
 //const float allowdegree=0.02;
 const int rightspeed=120;
 const int leftspeed=rightspeed + 2;
-const int turnspeed=15*2;
+const int turnspeed=30*2;
 const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
 const int k_theta = 2;
 //const float PIfact=2.89;
@@ -80,9 +80,9 @@
         update();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
-            move(-10, 10);
+            move(-15, 15);
         } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
-            move(10, -10);
+            move(15, -15);
         } else {
             move(0,0);
             return;
@@ -122,17 +122,15 @@
     wait(0.5);
 }
 
-        
+
 void pmove_to_dir(int direction, int x, int y)
 {
     float k = 0.9;
     int   k_theta = 2;
 
-    int length;
-    int dx, dy;
+    int length, dx, dy;
     int *d_length, *disorder;
-    float dtheta;
-    float ptheta;
+    float dtheta, ptheta;
     float daikei;
 
     update();
@@ -196,15 +194,13 @@
 
         if(abs(*d_length) > abs(length) -200) {
             daikei = (float)(abs(length)-abs(*d_length)) / 200;
-        }
-        else if(abs(*d_length) < 150) {
+        } else if(abs(*d_length) < 150) {
             daikei = (float)(abs(*d_length)) / 150;
-        }
-        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);  
-        
+        } 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);
+
         if((direction > 0 && *d_length >= 0) || (direction < 0 &&  *d_length <= 0)) {
             move(0, 0);
             break;
@@ -216,9 +212,91 @@
     wait(0.5);
 }
 
-void back()
-{
-    move(-30, -32);
-    wait(1);
-    move(0,0);   
-}
\ No newline at end of file
+void back(int x, int y)
+{ 
+    float k = 0.9;
+    int   k_theta = 2;
+
+    int length, dx, dy;
+    int *d_length, *disorder;
+    int absd_length;
+    float dtheta, ptheta;
+    float daikei;
+
+    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:
+            ptheta = PI;
+            break;
+        case Y_PLUS:
+            k *= -1;
+            ptheta = PI*3/2;
+            break;
+        case X_MINUS:
+            k *= -1;
+            ptheta = 0;
+            break;
+        case Y_MINUS:
+            ptheta = PI/2;
+            break;
+        default:
+            return;
+    }
+
+    turn_abs_rad(ptheta);
+
+    if(length == 0) return;
+
+    while(1) {
+        update();
+        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(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*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)) {
+            move(0, 0);
+            break;
+        }
+
+        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
+    }
+
+    wait(0.5);
+    
+}