Diff: locate.h

Revision:

3:56b034c41dc5

Parent:

2:4b2594dd86be

Child:

4:db36396371e0

--- a/locate.h	Thu Sep 01 09:19:34 2016 +0000
+++ b/locate.h	Fri Sep 02 06:50:14 2016 +0000
@@ -39,6 +39,7 @@
 short v = 0;        //ステップ速度
 float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
 float theta = 0;    //機体角度、x軸正の向きを0とする
+float erase_theta = 0;
 //宣言終わり
 
 
@@ -64,28 +65,6 @@
     return pulse;
 }
 
-void  update ()
-//位置情報を更新する。r,lはエンコーダから
-{
-    count1=__HAL_TIM_GET_COUNTER(&timer1);
-    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
-    count2=__HAL_TIM_GET_COUNTER(&timer2);
-    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);   
-    
-    r = -convert_enc_count(count1, dir1);
-    l = convert_enc_count(count2, dir2);
-        
-    theta = (r - l) * ROUND;
-    v = (r - pr + l - pl);
-
-    x += v * cos(theta);
-    y += v * sin(theta);
-
-    pr = r;
-    pl = l;
-    //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
-    pc.printf("right:%d     left:%d    ", r, l);
-}
 
 short coordinateX()
 //xをmm換算して整数値として返す
@@ -106,4 +85,34 @@
 }
 
 
+void  update ()
+//位置情報を更新する。r,lはエンコーダから
+{
+    count1=__HAL_TIM_GET_COUNTER(&timer1);
+    dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
+    count2=__HAL_TIM_GET_COUNTER(&timer2);
+    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);   
+    
+    r = -convert_enc_count(count1, dir1);
+    l = convert_enc_count(count2, dir2);
+        
+    theta = (r - l) * ROUND - erase_theta;
+    v = (r - pr + l - pl);
+
+    x += v * cos(theta);
+    y += v * sin(theta);
+
+    pr = r;
+    pl = l;
+    //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
+    pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
+}
+
+void erase()
+{
+    x = 0;
+    y = 0;
+    erase_theta = theta;
+}
+
 #endif