Diff: _hakaba/h_move.cpp

Revision:

6:931d51a70200

Parent:

4:1604d599d40f

--- a/_hakaba/h_move.cpp	Sat Sep 03 10:53:43 2016 +0000
+++ b/_hakaba/h_move.cpp	Mon Sep 05 10:39:03 2016 +0000
@@ -37,4 +37,105 @@
         }
     }
 }
+*/
+
+
+/*
+void pmovey_to(int x, int y)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    ptheta=coordinateTheta();
+    move(rightspeed,leftspeed);
+
+    while(1) {
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+        dtheta=coordinateTheta()-ptheta;
+
+        if(dx>7) {
+            dx=7;
+        } else if(dx<-7) {
+            dx=-7;
+        }
+        move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
+
+        if(dy>length) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void pmovex_minus_to(int x, int y)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    ptheta=coordinateTheta();
+    move(rightspeed,leftspeed);
+
+    while(1) {
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+        dtheta=coordinateTheta()-ptheta;
+
+        if(dy>7) {
+            dy=7;
+        }
+        if(dy<-7) {
+            dy=-7;
+        }
+        move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
+
+        if(dx < length) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void pmovey_minus_to(int x, int y)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    ptheta=coordinateTheta();
+    move(rightspeed,leftspeed);
+
+    while(1) {
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+        dtheta=coordinateTheta()-ptheta;
+
+        if(dx>7) {
+            dx=7;
+        }
+        if(dx<-7) {
+            dx=-7;
+        }
+        move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
+
+        if(dy<length) {
+            move(0,0);
+            break;
+        }
+    }
+}
 */
\ No newline at end of file