Diff: move.cpp

Revision:

32:e462489e2418

Parent:

31:427bd43429e3

--- a/move.cpp	Sun Sep 11 03:57:47 2016 +0000
+++ b/move.cpp	Sun Sep 11 04:04:55 2016 +0000
@@ -272,7 +272,6 @@
 }
 
 void pmove2(int x, int y)
-{
     yellow = 1;
     red=0;
     float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
@@ -339,6 +338,74 @@
     red = 0;
 }
 
+void pmove2_fast()
+{
+    yellow = 1;
+    red=0;
+    float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
+    int   k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
+
+    double length;
+    int d_length, disorder;
+    float dtheta, ptheta;
+    float daikei;
+
+    length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
+
+    if(length == 0) {
+        red=1;
+        return;
+    }
+
+    ptheta = giveatan(x, y);
+
+    ptheta += nearPi(coordinateTheta() - ptheta);
+
+    turnrad(ptheta);
+
+    virtual_setup();
+
+    int i = 0;
+
+    while(1) {
+        update_np();
+        virtual_update();
+
+        d_length = length - virtual_coordinateX();
+        disorder = virtual_coordinateY();
+        dtheta = virtual_coordinateTheta();
+
+        if(disorder>max_disorder) {
+            disorder = max_disorder;
+        } else if(disorder<-max_disorder) {
+            disorder = -max_disorder;
+        }
+
+        if(i++ < 5) {
+            daikei = i/5;
+        } else if(d_length < 300) {
+            daikei = d_length / 300.0;
+        } else
+            daikei = 1;
+
+        move(daikei * (rightspeed*2*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
+             daikei * (leftspeed*2*(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed*ratio);
+
+
+        //pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
+        if(d_length <= 0) {
+            move(0, 0);
+            break;
+        }
+
+    }
+
+    wait(0.2);
+
+    yellow = 0;
+    red = 0;    
+}
+
 void back300()
 {
     red = 1;