locate.h

00001 #ifndef Locate_H
00002 #define Locate_H
00003 
00004 #include <math.h>
00005 #include"mbed.h"
00006 #include "Encoder.h"
00007 
00008 
00009 #define OUTERRING_D     140             //外輪間距離(mm)
00010 #define INNERRING_D     136             //内輪間距離(mm)
00011 #define PI              3.14159         //π
00012 #define RESOLUSION      400             //P/R(分解能)
00013 #define DIAMETER        31.8            //タイヤの直径(mm)
00014 #define LOCATE_STEP     (DIAMETER*PI / RESOLUSION)                  // エンコーダの１ステップあたりの距離(mm)
00015 #define TIRE_DISTANCE   ((OUTERRING_D + INNERRING_D) / 2)           //タイヤ間距離(mm)
00016 #define ROUND_HOSEI     1               //角度のズレを補正
00017 #define ROUND           ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI)   //機体が１回転するために必要なステップ数の”逆数”
00018 
00019 //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
00020 #ifdef TARGET_STM32F3
00021 #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
00022 #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__)            (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
00023 #endif
00024 
00025 
00026 //エンコーダから、現在のステップ数（＝タイヤがどれだけ回ったか）を得られる
00027 
00028 
00029 //グローバル変数宣言
00030 Serial pc(SERIAL_TX, SERIAL_RX);
00031 TIM_Encoder_InitTypeDef encoder1, encoder2;
00032 TIM_HandleTypeDef  timer1,  timer2;
00033 DigitalOut enc_v(PC_7);
00034 
00035 uint16_t count1=0,  count2=0;
00036 int8_t dir1, dir2;
00037 int r, l;
00038 int pr = 0, pl = 0;     //前回のステップ数
00039 short v = 0;        //ステップ速度
00040 float x = 0, y = 0;     //xy方向に進んだ距離(m換算なし)
00041 float theta = 0;    //機体角度、x軸正の向きを0とする
00042 float erase_theta = 0;
00043 //宣言終わり
00044 
00045 
00046 
00047 void  setup()      //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う
00048 {
00049     enc_v = 1;
00050     
00051     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00052     //use PB6 PB7 = Nucleo D7 D8
00053     EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12);
00054 
00055     //counting on both A&B inputs, 4 ticks per cycle,  16-bit count
00056     //use PA0 PA1 = Nucleo A0 A1
00057     EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12);
00058 }
00059 
00060 int convert_enc_count(int16_t pulse, int8_t direction)
00061 {
00062     if(direction == 0)
00063         pulse = pulse - 0xffff -1;
00064     
00065     return pulse;
00066 }
00067 
00068 
00069 short coordinateX()
00070 //xをmm換算して整数値として返す
00071 {
00072     return x * LOCATE_STEP / 2;
00073 }
00074 
00075 short coordinateY()
00076 //yをmm換算して整数値として返す
00077 {
00078     return y * LOCATE_STEP / 2;
00079 }
00080 
00081 float coordinateTheta()
00082 //thetaを返す
00083 {
00084     return theta;
00085 }
00086 
00087 
00088 void  update ()
00089 //位置情報を更新する。r,lはエンコーダから
00090 {
00091     count1=__HAL_TIM_GET_COUNTER(&timer1);
00092     dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1);
00093     count2=__HAL_TIM_GET_COUNTER(&timer2);
00094     dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);   
00095     
00096     r = -convert_enc_count(count1, dir1);
00097     l = -convert_enc_count(count2, dir2);
00098         
00099     theta = (r - l) * ROUND - erase_theta;
00100     v = (r - pr + l - pl);
00101 
00102     x += v * cos(theta);
00103     y += v * sin(theta);
00104 
00105     pr = r;
00106     pl = l;
00107     //pc.printf("count1:%d%s  count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-");
00108     pc.printf("right:%d     left:%d     x:%d    y:%d    t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta());
00109 }
00110 
00111 void erase()
00112 {
00113     x = 0;
00114     y = 0;
00115     erase_theta = theta;
00116 }
00117 
00118 #endif