Diff: locate.cpp

Revision:

6:27d0384052d2

Parent:

4:ecbb45e84c08

--- a/locate.cpp	Wed Sep 07 07:36:42 2016 +0000
+++ b/locate.cpp	Sat Sep 10 12:16:57 2016 +0000
@@ -14,13 +14,16 @@
 Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信
 DigitalOut enc_v(PC_7);             //エンコーダの電源
 
-int      r, l;                  //現在の回転数
-int      pr = 0, pl = 0;        //前回のステップ数
-short    v = 0;                 //ステップ速度
-float    xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
-float    theta = 0;             //機体角度、x軸正の向きを0とする
-float    d_theta = 0;           //erase()関数を使って、そのときのthetaをコピー
-
+int     r, l;                  //現在の回転数
+int     pr = 0, pl = 0;        //前回のステップ数
+int     v = 0;                 //ステップ速度
+float   xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
+float   theta = 0;             //機体角度、x軸正の向きを0とする
+float   d_theta = 0;           //erase()関数を使って、そのときのthetaをコピー
+float   virtual_xcount = 0 ,virtual_ycount = 0;
+int     *virtual_v;
+float   virtual_theta = 0;
+float   virtual_ptheta  = 0;
 /*************変数宣言終了******************/
 
 
@@ -29,7 +32,7 @@
 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);
@@ -42,7 +45,7 @@
 void  setup(int tx, int ty)
 {
     enc_v = 1;
-    
+
     xcount = tx / (LOCATE_STEP / 2);
     ycount = ty / (LOCATE_STEP / 2);
 
@@ -55,18 +58,19 @@
     EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
 }
 
-void  update ()
+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);   
-    
+    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
+
     r = -convert_enc_count(count1, dir1);
     l = -convert_enc_count(count2, dir2);
-        
-    theta = (r - l) * ROUND - d_theta;
+    
+    theta = (r - l) * ROUND;
+    //theta = (r - l) * ROUND - d_theta;
     v = (r - pr + l - pl);
 
     xcount += v * cos(theta);
@@ -83,11 +87,11 @@
     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);   
-    
+    dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
+
     r = -convert_enc_count(count1, dir1);
     l = -convert_enc_count(count2, dir2);
-        
+
     theta = (r - l) * ROUND - d_theta;
     v = (r - pr + l - pl);
 
@@ -117,7 +121,7 @@
 {
     if(direction == 0)
         pulse = pulse - 0xffff -1;
-    
+
     return pulse;
 }
 
@@ -127,3 +131,37 @@
     ycount = 0;
     d_theta = theta;
 }
+
+void virtual_setup()
+{
+    virtual_xcount = 0;
+    virtual_ycount = 0;
+    
+    virtual_v = &v;
+    
+    virtual_theta = 0;
+    virtual_ptheta = coordinateTheta();
+}
+
+void virtual_update()
+{
+    virtual_theta = coordinateTheta() - virtual_ptheta;
+    
+    virtual_xcount += *virtual_v * cos(virtual_theta);
+    virtual_ycount += *virtual_v * sin(virtual_theta);
+}
+
+int virtual_coordinateX()
+{
+    return virtual_xcount * LOCATE_STEP / 2;
+}
+
+int virtual_coordinateY()
+{
+    return virtual_ycount * LOCATE_STEP / 2;
+}
+
+float virtual_coordinateTheta()
+{
+    return virtual_theta;
+}
\ No newline at end of file