Tk A
/
locate_by_hello_enc
aaaaaaaaaa
locate.h@1:10cc86cabdce, 2016-09-01 (annotated)
- Committer:
- sakanakuuun
- Date:
- Thu Sep 01 08:53:14 2016 +0000
- Revision:
- 1:10cc86cabdce
- Parent:
- 0:f12d257b587e
fnaklnsflksopeugjpv9shid; ; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakanakuuun | 0:f12d257b587e | 1 | #ifndef Locate_H |
sakanakuuun | 0:f12d257b587e | 2 | #define Locate_H |
sakanakuuun | 0:f12d257b587e | 3 | |
sakanakuuun | 0:f12d257b587e | 4 | #include <math.h> |
sakanakuuun | 0:f12d257b587e | 5 | #include"mbed.h" |
sakanakuuun | 0:f12d257b587e | 6 | #include "Encoder.h" |
sakanakuuun | 0:f12d257b587e | 7 | |
sakanakuuun | 0:f12d257b587e | 8 | |
sakanakuuun | 0:f12d257b587e | 9 | #define OUTERRING_D 140 //外輪間距離(mm) |
sakanakuuun | 0:f12d257b587e | 10 | #define INNERRING_D 136 //内輪間距離(mm) |
sakanakuuun | 0:f12d257b587e | 11 | #define PI 3.14159 //π |
sakanakuuun | 0:f12d257b587e | 12 | #define RESOLUSION 400 //P/R(分解能) |
sakanakuuun | 0:f12d257b587e | 13 | #define DIAMETER 31.8 //タイヤの直径(mm) |
sakanakuuun | 0:f12d257b587e | 14 | #define LOCATE_STEP (DIAMETER*PI / RESOLUSION) // エンコーダの1ステップあたりの距離(mm) |
sakanakuuun | 0:f12d257b587e | 15 | #define TIRE_DISTANCE ((OUTERRING_D + INNERRING_D) / 2) //タイヤ間距離(mm) |
sakanakuuun | 0:f12d257b587e | 16 | #define ROUND_HOSEI 1 //角度のズレを補正 |
sakanakuuun | 0:f12d257b587e | 17 | #define ROUND ((PI * DIAMETER / (RESOLUSION * TIRE_DISTANCE)) * ROUND_HOSEI) //機体が1回転するために必要なステップ数の”逆数” |
sakanakuuun | 0:f12d257b587e | 18 | |
sakanakuuun | 0:f12d257b587e | 19 | //STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h |
sakanakuuun | 0:f12d257b587e | 20 | #ifdef TARGET_STM32F3 |
sakanakuuun | 0:f12d257b587e | 21 | #define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT) |
sakanakuuun | 0:f12d257b587e | 22 | #define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR)) |
sakanakuuun | 0:f12d257b587e | 23 | #endif |
sakanakuuun | 0:f12d257b587e | 24 | |
sakanakuuun | 0:f12d257b587e | 25 | |
sakanakuuun | 0:f12d257b587e | 26 | //エンコーダから、現在のステップ数(=タイヤがどれだけ回ったか)を得られる |
sakanakuuun | 0:f12d257b587e | 27 | |
sakanakuuun | 0:f12d257b587e | 28 | |
sakanakuuun | 0:f12d257b587e | 29 | //グローバル変数宣言 |
sakanakuuun | 0:f12d257b587e | 30 | Serial pc(SERIAL_TX, SERIAL_RX); |
sakanakuuun | 0:f12d257b587e | 31 | TIM_Encoder_InitTypeDef encoder1, encoder2; |
sakanakuuun | 0:f12d257b587e | 32 | TIM_HandleTypeDef timer1, timer2; |
sakanakuuun | 1:10cc86cabdce | 33 | DigitalOut enc_v(PC_7); |
sakanakuuun | 0:f12d257b587e | 34 | |
sakanakuuun | 0:f12d257b587e | 35 | uint16_t count1=0, count2=0; |
sakanakuuun | 0:f12d257b587e | 36 | int8_t dir1, dir2; |
sakanakuuun | 0:f12d257b587e | 37 | int r, l; |
sakanakuuun | 0:f12d257b587e | 38 | int pr = 0, pl = 0; //前回のステップ数 |
sakanakuuun | 0:f12d257b587e | 39 | short v = 0; //ステップ速度 |
sakanakuuun | 0:f12d257b587e | 40 | float x = 0, y = 0; //xy方向に進んだ距離(m換算なし) |
sakanakuuun | 0:f12d257b587e | 41 | float theta = 0; //機体角度、x軸正の向きを0とする |
sakanakuuun | 0:f12d257b587e | 42 | //宣言終わり |
sakanakuuun | 0:f12d257b587e | 43 | |
sakanakuuun | 0:f12d257b587e | 44 | |
sakanakuuun | 0:f12d257b587e | 45 | |
sakanakuuun | 0:f12d257b587e | 46 | void setup() //エンコーダの初期のズレ(dr,dl)を出す、最初に一回だけ行う |
sakanakuuun | 0:f12d257b587e | 47 | { |
sakanakuuun | 0:f12d257b587e | 48 | enc_v = 1; |
sakanakuuun | 0:f12d257b587e | 49 | |
sakanakuuun | 0:f12d257b587e | 50 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 0:f12d257b587e | 51 | //use PA8 PA9 = Nucleo D7 D8 |
sakanakuuun | 0:f12d257b587e | 52 | EncoderInit(&encoder1, &timer1, TIM1, 0xffff, TIM_ENCODERMODE_TI12); |
sakanakuuun | 0:f12d257b587e | 53 | |
sakanakuuun | 0:f12d257b587e | 54 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 0:f12d257b587e | 55 | //use PA0 PA1 = Nucleo A0 A1 |
sakanakuuun | 0:f12d257b587e | 56 | EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12); |
sakanakuuun | 0:f12d257b587e | 57 | } |
sakanakuuun | 0:f12d257b587e | 58 | |
sakanakuuun | 0:f12d257b587e | 59 | int convert_enc_count(int16_t pulse, int8_t direction) |
sakanakuuun | 0:f12d257b587e | 60 | { |
sakanakuuun | 0:f12d257b587e | 61 | if(direction == 0) |
sakanakuuun | 0:f12d257b587e | 62 | pulse = pulse - 0xffff -1; |
sakanakuuun | 0:f12d257b587e | 63 | |
sakanakuuun | 0:f12d257b587e | 64 | return pulse; |
sakanakuuun | 0:f12d257b587e | 65 | } |
sakanakuuun | 0:f12d257b587e | 66 | |
sakanakuuun | 0:f12d257b587e | 67 | void update () |
sakanakuuun | 0:f12d257b587e | 68 | //位置情報を更新する。r,lはエンコーダから |
sakanakuuun | 0:f12d257b587e | 69 | { |
sakanakuuun | 0:f12d257b587e | 70 | count1=__HAL_TIM_GET_COUNTER(&timer1); |
sakanakuuun | 0:f12d257b587e | 71 | dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1); |
sakanakuuun | 0:f12d257b587e | 72 | count2=__HAL_TIM_GET_COUNTER(&timer2); |
sakanakuuun | 0:f12d257b587e | 73 | dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
sakanakuuun | 0:f12d257b587e | 74 | |
sakanakuuun | 1:10cc86cabdce | 75 | r = -convert_enc_count(count1, dir1); |
sakanakuuun | 0:f12d257b587e | 76 | l = convert_enc_count(count2, dir2); |
sakanakuuun | 0:f12d257b587e | 77 | |
sakanakuuun | 0:f12d257b587e | 78 | theta = (r - l) * ROUND; |
sakanakuuun | 0:f12d257b587e | 79 | v = (r - pr + l - pl); |
sakanakuuun | 0:f12d257b587e | 80 | |
sakanakuuun | 0:f12d257b587e | 81 | x += v * cos(theta); |
sakanakuuun | 0:f12d257b587e | 82 | y += v * sin(theta); |
sakanakuuun | 0:f12d257b587e | 83 | |
sakanakuuun | 0:f12d257b587e | 84 | pr = r; |
sakanakuuun | 0:f12d257b587e | 85 | pl = l; |
sakanakuuun | 1:10cc86cabdce | 86 | //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-"); |
sakanakuuun | 1:10cc86cabdce | 87 | pc.printf("right:%d left:%d ", r, l); |
sakanakuuun | 0:f12d257b587e | 88 | } |
sakanakuuun | 0:f12d257b587e | 89 | |
sakanakuuun | 0:f12d257b587e | 90 | short coordinateX() |
sakanakuuun | 0:f12d257b587e | 91 | //xをmm換算して整数値として返す |
sakanakuuun | 0:f12d257b587e | 92 | { |
sakanakuuun | 0:f12d257b587e | 93 | return x * LOCATE_STEP / 2; |
sakanakuuun | 0:f12d257b587e | 94 | } |
sakanakuuun | 0:f12d257b587e | 95 | |
sakanakuuun | 0:f12d257b587e | 96 | short coordinateY() |
sakanakuuun | 0:f12d257b587e | 97 | //yをmm換算して整数値として返す |
sakanakuuun | 0:f12d257b587e | 98 | { |
sakanakuuun | 0:f12d257b587e | 99 | return y * LOCATE_STEP / 2; |
sakanakuuun | 0:f12d257b587e | 100 | } |
sakanakuuun | 0:f12d257b587e | 101 | |
sakanakuuun | 0:f12d257b587e | 102 | float coordinateTheta() |
sakanakuuun | 0:f12d257b587e | 103 | //thetaを返す |
sakanakuuun | 0:f12d257b587e | 104 | { |
sakanakuuun | 0:f12d257b587e | 105 | return theta; |
sakanakuuun | 0:f12d257b587e | 106 | } |
sakanakuuun | 0:f12d257b587e | 107 | |
sakanakuuun | 0:f12d257b587e | 108 | |
sakanakuuun | 0:f12d257b587e | 109 | #endif |