Diff: _hakaba/h_move.cpp

Revision:

4:1604d599d40f

Child:

6:931d51a70200

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/_hakaba/h_move.cpp	Fri Sep 02 12:41:13 2016 +0000
@@ -0,0 +1,40 @@
+/*
+void pmovex2(int length)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=5;//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;
+        }
+        else if(dy<-7) {
+            dy=-7;
+        }
+        
+        if(-3 < dy && dy < 3){
+            move(rightspeed - k_y*dy - k_theta*dtheta, leftspeed + k_y*dy + k_theta*dtheta);
+        }
+        else{
+            move(rightspeed -50 - k_y*dy*dy - k_theta*dtheta, leftspeed -50 + k_y*dy*dy + k_theta*dtheta);
+       }     
+        if(dx>length) {
+            move(0,0);
+            break;
+        }
+    }
+}
+*/
\ No newline at end of file