Diff: locate.cpp

Revision:

4:ecbb45e84c08

Parent:

1:3513a2fbd81f

Child:

6:27d0384052d2

--- a/locate.cpp	Tue Sep 06 09:31:00 2016 +0000
+++ b/locate.cpp	Wed Sep 07 04:57:39 2016 +0000
@@ -17,7 +17,7 @@
 int      r, l;                  //現在の回転数
 int      pr = 0, pl = 0;        //前回のステップ数
 short    v = 0;                 //ステップ速度
-float    x = 0, y = 0;          //xy方向に進んだ距離(m換算なし)
+float    xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
 float    theta = 0;             //機体角度、x軸正の向きを0とする
 float    d_theta = 0;           //erase()関数を使って、そのときのthetaをコピー
 
@@ -26,8 +26,34 @@
 
 
 
+void  setup()
+{
+    enc_v = 1;
+    
+    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
+    //use PB6 PB7 = Nucleo D7 D8
+    EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
 
+    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
+    //use PA0 PA1 = Nucleo A0 A1
+    EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
+}
 
+void  setup(int tx, int ty)
+{
+    enc_v = 1;
+    
+    xcount = tx / (LOCATE_STEP / 2);
+    ycount = ty / (LOCATE_STEP / 2);
+
+    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
+    //use PB6 PB7 = Nucleo D7 D8
+    EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
+
+    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
+    //use PA0 PA1 = Nucleo A0 A1
+    EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
+}
 
 void  update ()
 //位置情報を更新する。r,lはエンコーダから
@@ -43,8 +69,8 @@
     theta = (r - l) * ROUND - d_theta;
     v = (r - pr + l - pl);
 
-    x += v * cos(theta);
-    y += v * sin(theta);
+    xcount += v * cos(theta);
+    ycount += v * sin(theta);
 
     pr = r;
     pl = l;
@@ -52,27 +78,34 @@
     pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
 }
 
-void  setup()
+void update_np()
 {
-    enc_v = 1;
+    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);   
     
-    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
-    //use PB6 PB7 = Nucleo D7 D8
-    EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
+    r = -convert_enc_count(count1, dir1);
+    l = -convert_enc_count(count2, dir2);
+        
+    theta = (r - l) * ROUND - d_theta;
+    v = (r - pr + l - pl);
 
-    //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
-    //use PA0 PA1 = Nucleo A0 A1
-    EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
+    xcount += v * cos(theta);
+    ycount += v * sin(theta);
+
+    pr = r;
+    pl = l;
 }
 
 int coordinateX()
 {
-    return x * LOCATE_STEP / 2;
+    return xcount * LOCATE_STEP / 2;
 }
 
 int coordinateY()
 {
-    return y * LOCATE_STEP / 2;
+    return ycount * LOCATE_STEP / 2;
 }
 
 float coordinateTheta()
@@ -90,7 +123,7 @@
 
 void erase()
 {
-    x = 0;
-    y = 0;
+    xcount = 0;
+    ycount = 0;
     d_theta = theta;
 }