Diff: move.cpp

Revision:

16:140e758346ae

Parent:

15:403e9c57c1a1

Child:

17:c167f4ed9070

--- a/move.cpp	Sat Sep 10 04:37:04 2016 +0000
+++ b/move.cpp	Sat Sep 10 06:14:48 2016 +0000
@@ -301,7 +301,6 @@
     yellow = 0;
 }
 
-
 void back300()
 {
     red = 1;
@@ -319,8 +318,6 @@
 
     length = 300;
 
-    if(length == 0) return;
-
     turnrad(PI + nearPi(coordinateTheta() - PI));
 
     while(1) {
@@ -351,6 +348,56 @@
     red = 0;
 }
 
+
+
+void nxback300()
+{
+    red = 1;
+
+    float k = 0.9;
+    int   k_theta = 2;
+
+    int length, px, py, dx, dy;
+    float daikei;
+
+    update();
+
+    px = coordinateX();
+    py = coordinateY();
+
+    length = 300;
+
+    turnrad(nearPi(coordinateTheta()));
+
+    while(1) {
+        update_np();
+        dx = coordinateX() - px;
+        dy = coordinateY() - py;
+
+        if(dy>max_disorder) {
+            dy = max_disorder;
+        } else if(dy<-max_disorder) {
+            dy = -max_disorder;
+        }
+
+
+        move(-(30 - k*dy), -(32 + k*dy));
+
+
+        if(abs(dx)>length) {
+            move(0, 0);
+            break;
+        }
+
+        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
+    }
+
+    wait(0.5);
+
+    red = 0;
+}
+
+
 float nearPi(float rad)
 {
     float npi  = 0;