Diff: header.h

Revision:

2:b204cf2f9b60

Parent:

1:a1e592eca305

--- a/header.h	Tue Aug 30 06:51:41 2016 +0000
+++ b/header.h	Wed Aug 31 11:09:45 2016 +0000
@@ -12,6 +12,35 @@
 const int allowlength=30;
 
 
+void turn(int degree)
+{
+    update(-right.getPulses(), left.getPulses());
+    float pTheta = coordinateTheta();
+
+    float rad = degree * PI / 180;
+    if(degree>=0) {
+        move(30, -30);
+        while(1) {
+            update(-right.getPulses(), left.getPulses());
+            float nTheta = coordinateTheta();
+            if(pTheta-nTheta < -rad ){
+                move(0,0);
+            break;
+            }
+        }
+    } else if(degree<0) {
+        move(-30, 30);
+        while(1) {
+            update(-right.getPulses(), left.getPulses());
+            float nTheta = coordinateTheta();
+
+            if(pTheta-nTheta > rad ){
+                move(0,0);
+            break;
+            }
+        }
+    }
+}
 
 
 
@@ -21,6 +50,7 @@
     int onoff = 1;
 
     //よく使うかたまり
+        update(-right.getPulses(), left.getPulses());
         x = coordinateX();
         y = coordinateY();
         theta = coordinateTheta();//自己位置取得
@@ -29,6 +59,7 @@
 
     //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
     while (1){
+        update(-right.getPulses(), left.getPulses());
         x = coordinateX();
         y = coordinateY();
         theta = coordinateTheta();
@@ -40,6 +71,7 @@
 
     //角度を合わせながら直進
     while (onoff){
+        update(-right.getPulses(), left.getPulses());
         x = coordinateX();
         y = coordinateY();
         theta = coordinateTheta();
@@ -70,6 +102,7 @@
     move((-1)*speed, (-1) *speed);
 
     while (1){
+        update(-right.getPulses(), left.getPulses());
         x2 = coordinateX();
         y2 = coordinateY();
         distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);