Diff: move.cpp

Revision:

17:c167f4ed9070

Parent:

16:140e758346ae

Child:

18:4c812a3c7411

--- a/move.cpp	Sat Sep 10 06:14:48 2016 +0000
+++ b/move.cpp	Sat Sep 10 06:51:19 2016 +0000
@@ -24,8 +24,7 @@
 //const float allowdegree=0.02;
 const int rightspeed=70;
 const int leftspeed=rightspeed + 4;
-const int turnspeed=30*2;
-const int hosei_turnspeed = 13;
+const int hosei_ = 13;
 const int max_disorder = 4;
 const float ratio = 1.0/7.5;
 //const float PIfact=2.89;
@@ -86,9 +85,9 @@
         update_np();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
-            move(-hosei_turnspeed, hosei_turnspeed);
+            move(-hosei_, hosei_);
         } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
-            move(hosei_turnspeed, -hosei_turnspeed);
+            move(hosei_, -hosei_);
         } else {
             move(0,0);
             return;
@@ -108,7 +107,7 @@
     else if(coordinateTheta() < rad) np = -1;
     else return;
 
-    move((-np)*(turnspeed+2), (np)*turnspeed);
+    move((-np)*rightspeed, (np)*leftspeed);
 
     while(1) {
         update_np();
@@ -139,7 +138,7 @@
 
     update();
 
-    move((turnspeed+2), -turnspeed);
+    move(rightspeed, -leftspeed);
 
     while(1) {
         update();
@@ -169,7 +168,7 @@
 
     update();
 
-    move((-1)*(turnspeed+2), turnspeed);
+    move((-1)*rightspeed, leftspeed);
 
     while(1) {
         update();
@@ -349,7 +348,6 @@
 }
 
 
-
 void nxback300()
 {
     red = 1;
@@ -398,6 +396,54 @@
 }
 
 
+void pyback300()
+{
+    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(-PI/2 + nearPi(coordinateTheta() + PI/2));
+
+    while(1) {
+        update_np();
+        dx = coordinateX() - px;
+        dy = coordinateY() - py;
+
+        if(dx>max_disorder) {
+            dx = max_disorder;
+        } else if(dx<-max_disorder) {
+            dx = -max_disorder;
+        }
+
+
+        move(-(30 - k*dx), -(32 + k*dx));
+
+
+        if(dy>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;