Diff: move.cpp

Revision:

6:0aa97a99c9cb

Parent:

5:0e18cf25291a

Child:

7:7f1721542753

--- a/move.cpp	Tue Sep 06 09:31:07 2016 +0000
+++ b/move.cpp	Tue Sep 06 10:18:17 2016 +0000
@@ -212,84 +212,40 @@
 }
 
 
-void back(int x, int y)
+void back300()
 { 
     float k = 0.9;
     int   k_theta = 2;
 
-    int length, dx, dy;
-    int *d_length, *disorder;
-    int absd_length;
-    float dtheta, ptheta;
+    int length, px, py, dx, dy;
     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 = 0;
-            break;
-        case Y_PLUS:
-            k *= -1;
-            ptheta = PI/2;
-            break;
-        case X_MINUS:
-            k *= -1;
-            ptheta = PI;
-            break;
-        case Y_MINUS:
-            ptheta = PI*3/2;
-            break;
-        default:
-            return;
-    }
-
-    turn_abs_rad(ptheta);
-
+    
+    update();
+    
+    px = coordinateX(); py = coordinateY();
+    
+    length = 300;
+    
     if(length == 0) return;
 
+    turn_abs_rad(PI);
+    
     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);
+        dx = coordinateX() - px;
+        dy = coordinateY() - py;
 
-        if(absd_length > length - 30) {
-            daikei = -(length - absd_length) / 30.0;
-        } else if(absd_length < 150) {
-            daikei = -absd_length / 150.0;
-        } else
-            daikei = -1;
+        if(dy>2) {
+            dy = 2;
+        } else if(dy<-2) {
+            dy = -2;
+        }
+    
 
-        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(-30*6/7.0 - k*dy, -33 + k*dy);
 
-        if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
+        
+        if(dx>length) {
             move(0, 0);
             break;
         }