Revision 6:27d0384052d2, committed 2016-09-10

Comitter:

sakanakuuun

Date:

Sat Sep 10 12:16:57 2016 +0000

Parent:

5:d9857c701310

Commit message:

aas

Changed in this revision

diff -r d9857c701310 -r 27d0384052d2 locate.cpp
--- 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

diff -r d9857c701310 -r 27d0384052d2 locate.h
--- a/locate.h	Wed Sep 07 07:36:42 2016 +0000
+++ b/locate.h	Sat Sep 10 12:16:57 2016 +0000
@@ -11,7 +11,7 @@
 #define PI              3.14159         //π
 #define RESOLUSION      400             //P/R(分解能)
 #define DIAMETER        31.8            //タイヤの直径(mm)
-#define ROUND_HOSEI     1.04            //角度のズレを補正
+#define ROUND_HOSEI     1.038            //角度のズレを補正
 
 // エンコーダの１ステップあたりの距離(mm)
 #define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                 
@@ -54,6 +54,16 @@
 //x, y, thetaを0にする
 void erase();
 
+//仮想
+void virtual_setup();
+
+void virtual_update();
+
+int virtual_coordinateX();
+
+int virtual_coordinateY();
+
+float virtual_coordinateTheta();
 /**********************************/
 #endif