locate.cpp

00001 #include <math.h>
00002 #include "mbed.h"
00003 #include "Encoder.h"
00004 #include "locate.h"
00005 
00006 
00007 /********以下グローバル変数宣言**************/
00008 Serial pc(SERIAL_TX, SERIAL_RX); 
00009 TIM_Encoder_InitTypeDef encoder1, encoder2;     //Encoderライブラリで使う
00010 TIM_HandleTypeDef  timer1,  timer2;             //Encoderライブラリで使う
00011 uint16_t count1=0,  count2=0;                   //Encoderライブラリで使う
00012 int8_t   dir1, dir2;                            //Encoderライブラリで使う
00013 
00014 
00015 DigitalOut enc_v(PC_7);             //エンコーダの電源
00016 
00017 int     r, l;                  //現在の回転数
00018 int     pr = 0, pl = 0;        //前回のステップ数
00019 int     v = 0;                 //ステップ速度
00020 float   xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし)
00021 float   theta = 0;             //機体角度、x軸正の向きを0とする
00022 float   d_theta = 0;           //erase()関数を使って、そのときのthetaをコピー
00023 float   virtual_xcount = 0 ,virtual_ycount = 0;
00024 int     *virtual_v;
00025 float   virtual_theta = 0;
00026 float   virtual_ptheta  = 0;
00027 int     teamcolor = -1;
00028 /*************変数宣言終了******************/
00029 
00030 
00031 
00032 
00033 void  setup(int team)
00034 {
00035     enc_v = 1;
00036     
00037     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00038     //use PB6 PB7 = Nucleo D7 D8
00039     EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI1);
00040 
00041     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00042     //use PA0 PA1 = Nucleo A0 A1
00043     EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI1);
00044     
00045     teamcolor = team;
00046 }
00047 
00048 void  setup(int tx, int ty)
00049 {
00050     enc_v = 1;
00051 
00052     xcount = tx / (LOCATE_STEP / 2);
00053     ycount = ty / (LOCATE_STEP / 2);
00054 
00055     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00056     //use PB6 PB7 = Nucleo D7 D8
00057     EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
00058 
00059     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00060     //use PA0 PA1 = Nucleo A0 A1
00061     EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
00062 }
00063 
00064 void  update()
00065 //位置情報を更新する。r,lはエンコーダから
00066 {
00067     count1=__HAL_TIM_GET_COUNTER(&timer1);
00068     dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
00069     count2=__HAL_TIM_GET_COUNTER(&timer2);
00070     dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00071 
00072     r = -convert_enc_count(count1, dir1);
00073     l = -convert_enc_count(count2, dir2);
00074 
00075     theta = teamcolor*(r - l) * ROUND;
00076     //theta = (r - l) * ROUND - d_theta;
00077     v = (r - pr + l - pl);
00078 
00079     xcount += v * cos(theta);
00080     ycount += v * sin(theta);
00081 
00082     pr = r;
00083     pl = l;
00084     //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
00085     pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f   d%d\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta(),r-l);
00086 }
00087 
00088 void update_np()
00089 {
00090     count1=__HAL_TIM_GET_COUNTER(&timer1);
00091     dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
00092     count2=__HAL_TIM_GET_COUNTER(&timer2);
00093     dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00094 
00095     r = -convert_enc_count(count1, dir1);
00096     l = -convert_enc_count(count2, dir2);
00097 
00098     theta = teamcolor * (r - l) * ROUND;
00099     v = (r - pr + l - pl);
00100 
00101     xcount += v * cos(theta);
00102     ycount += v * sin(theta);
00103 
00104     pr = r;
00105     pl = l;
00106     pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f   d%d\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta(),r-l);
00107 }
00108 
00109 int coordinateX()
00110 {
00111     return xcount * LOCATE_STEP / 2;
00112 }
00113 
00114 int coordinateY()
00115 {
00116     return ycount * LOCATE_STEP / 2;
00117 }
00118 
00119 float coordinateTheta()
00120 {
00121     return theta;
00122 }
00123 
00124 int convert_enc_count(int16_t pulse, int8_t direction)
00125 {
00126     if(direction == 0)
00127         pulse = pulse - 0xffff -1;
00128 
00129     return pulse;
00130 }
00131 
00132 void erase()
00133 {
00134     xcount = 0;
00135     ycount = 0;
00136     d_theta = theta;
00137 }
00138 
00139 void virtual_setup()
00140 {
00141     virtual_xcount = 0;
00142     virtual_ycount = 0;
00143 
00144     virtual_v = &v;
00145 
00146     virtual_theta = 0;
00147     virtual_ptheta = coordinateTheta();
00148 }
00149 
00150 void virtual_update()
00151 {
00152     virtual_theta = coordinateTheta() - virtual_ptheta;
00153 
00154     virtual_xcount += *virtual_v * cos(virtual_theta);
00155     virtual_ycount -= *virtual_v * sin(virtual_theta);
00156 }
00157 
00158 int virtual_coordinateX()
00159 {
00160     return virtual_xcount * LOCATE_STEP / 2;
00161 }
00162 
00163 int virtual_coordinateY()
00164 {
00165     return virtual_ycount * LOCATE_STEP / 2;
00166 }
00167 
00168 float virtual_coordinateTheta()
00169 {
00170     return virtual_theta;
00171 }
00172