fs
Dependents: ARAI45th 3servotest 1stcomp 3rdcompfixstart ... more
locate.cpp@1:3513a2fbd81f, 2016-09-03 (annotated)
- Committer:
- sakanakuuun
- Date:
- Sat Sep 03 10:53:09 2016 +0000
- Revision:
- 1:3513a2fbd81f
- Parent:
- 0:265d8ad19d2a
- Child:
- 4:ecbb45e84c08
hi;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sakanakuuun | 0:265d8ad19d2a | 1 | #include <math.h> |
sakanakuuun | 0:265d8ad19d2a | 2 | #include "mbed.h" |
sakanakuuun | 0:265d8ad19d2a | 3 | #include "Encoder.h" |
sakanakuuun | 0:265d8ad19d2a | 4 | #include "locate.h" |
sakanakuuun | 0:265d8ad19d2a | 5 | |
sakanakuuun | 0:265d8ad19d2a | 6 | |
sakanakuuun | 0:265d8ad19d2a | 7 | /********以下グローバル変数宣言**************/ |
sakanakuuun | 0:265d8ad19d2a | 8 | |
sakanakuuun | 0:265d8ad19d2a | 9 | TIM_Encoder_InitTypeDef encoder1, encoder2; //Encoderライブラリで使う |
sakanakuuun | 0:265d8ad19d2a | 10 | TIM_HandleTypeDef timer1, timer2; //Encoderライブラリで使う |
sakanakuuun | 0:265d8ad19d2a | 11 | uint16_t count1=0, count2=0; //Encoderライブラリで使う |
sakanakuuun | 0:265d8ad19d2a | 12 | int8_t dir1, dir2; //Encoderライブラリで使う |
sakanakuuun | 0:265d8ad19d2a | 13 | |
sakanakuuun | 0:265d8ad19d2a | 14 | Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信 |
sakanakuuun | 0:265d8ad19d2a | 15 | DigitalOut enc_v(PC_7); //エンコーダの電源 |
sakanakuuun | 0:265d8ad19d2a | 16 | |
sakanakuuun | 0:265d8ad19d2a | 17 | int r, l; //現在の回転数 |
sakanakuuun | 0:265d8ad19d2a | 18 | int pr = 0, pl = 0; //前回のステップ数 |
sakanakuuun | 0:265d8ad19d2a | 19 | short v = 0; //ステップ速度 |
sakanakuuun | 0:265d8ad19d2a | 20 | float x = 0, y = 0; //xy方向に進んだ距離(m換算なし) |
sakanakuuun | 0:265d8ad19d2a | 21 | float theta = 0; //機体角度、x軸正の向きを0とする |
sakanakuuun | 0:265d8ad19d2a | 22 | float d_theta = 0; //erase()関数を使って、そのときのthetaをコピー |
sakanakuuun | 0:265d8ad19d2a | 23 | |
sakanakuuun | 0:265d8ad19d2a | 24 | /*************変数宣言終了******************/ |
sakanakuuun | 0:265d8ad19d2a | 25 | |
sakanakuuun | 0:265d8ad19d2a | 26 | |
sakanakuuun | 1:3513a2fbd81f | 27 | |
sakanakuuun | 1:3513a2fbd81f | 28 | |
sakanakuuun | 1:3513a2fbd81f | 29 | |
sakanakuuun | 1:3513a2fbd81f | 30 | |
sakanakuuun | 1:3513a2fbd81f | 31 | |
sakanakuuun | 0:265d8ad19d2a | 32 | void update () |
sakanakuuun | 0:265d8ad19d2a | 33 | //位置情報を更新する。r,lはエンコーダから |
sakanakuuun | 0:265d8ad19d2a | 34 | { |
sakanakuuun | 0:265d8ad19d2a | 35 | count1=__HAL_TIM_GET_COUNTER(&timer1); |
sakanakuuun | 0:265d8ad19d2a | 36 | dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1); |
sakanakuuun | 0:265d8ad19d2a | 37 | count2=__HAL_TIM_GET_COUNTER(&timer2); |
sakanakuuun | 0:265d8ad19d2a | 38 | dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
sakanakuuun | 0:265d8ad19d2a | 39 | |
sakanakuuun | 0:265d8ad19d2a | 40 | r = -convert_enc_count(count1, dir1); |
sakanakuuun | 1:3513a2fbd81f | 41 | l = -convert_enc_count(count2, dir2); |
sakanakuuun | 0:265d8ad19d2a | 42 | |
sakanakuuun | 0:265d8ad19d2a | 43 | theta = (r - l) * ROUND - d_theta; |
sakanakuuun | 0:265d8ad19d2a | 44 | v = (r - pr + l - pl); |
sakanakuuun | 0:265d8ad19d2a | 45 | |
sakanakuuun | 0:265d8ad19d2a | 46 | x += v * cos(theta); |
sakanakuuun | 0:265d8ad19d2a | 47 | y += v * sin(theta); |
sakanakuuun | 0:265d8ad19d2a | 48 | |
sakanakuuun | 0:265d8ad19d2a | 49 | pr = r; |
sakanakuuun | 0:265d8ad19d2a | 50 | pl = l; |
sakanakuuun | 0:265d8ad19d2a | 51 | //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-"); |
sakanakuuun | 0:265d8ad19d2a | 52 | pc.printf("right:%d left:%d x:%d y:%d t:%f\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta()); |
sakanakuuun | 0:265d8ad19d2a | 53 | } |
sakanakuuun | 0:265d8ad19d2a | 54 | |
sakanakuuun | 0:265d8ad19d2a | 55 | void setup() |
sakanakuuun | 0:265d8ad19d2a | 56 | { |
sakanakuuun | 0:265d8ad19d2a | 57 | enc_v = 1; |
sakanakuuun | 0:265d8ad19d2a | 58 | |
sakanakuuun | 0:265d8ad19d2a | 59 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 0:265d8ad19d2a | 60 | //use PB6 PB7 = Nucleo D7 D8 |
sakanakuuun | 0:265d8ad19d2a | 61 | EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12); |
sakanakuuun | 0:265d8ad19d2a | 62 | |
sakanakuuun | 0:265d8ad19d2a | 63 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 0:265d8ad19d2a | 64 | //use PA0 PA1 = Nucleo A0 A1 |
sakanakuuun | 0:265d8ad19d2a | 65 | EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12); |
sakanakuuun | 0:265d8ad19d2a | 66 | } |
sakanakuuun | 0:265d8ad19d2a | 67 | |
sakanakuuun | 1:3513a2fbd81f | 68 | int coordinateX() |
sakanakuuun | 0:265d8ad19d2a | 69 | { |
sakanakuuun | 0:265d8ad19d2a | 70 | return x * LOCATE_STEP / 2; |
sakanakuuun | 0:265d8ad19d2a | 71 | } |
sakanakuuun | 0:265d8ad19d2a | 72 | |
sakanakuuun | 1:3513a2fbd81f | 73 | int coordinateY() |
sakanakuuun | 0:265d8ad19d2a | 74 | { |
sakanakuuun | 0:265d8ad19d2a | 75 | return y * LOCATE_STEP / 2; |
sakanakuuun | 0:265d8ad19d2a | 76 | } |
sakanakuuun | 0:265d8ad19d2a | 77 | |
sakanakuuun | 0:265d8ad19d2a | 78 | float coordinateTheta() |
sakanakuuun | 0:265d8ad19d2a | 79 | { |
sakanakuuun | 0:265d8ad19d2a | 80 | return theta; |
sakanakuuun | 0:265d8ad19d2a | 81 | } |
sakanakuuun | 0:265d8ad19d2a | 82 | |
sakanakuuun | 0:265d8ad19d2a | 83 | int convert_enc_count(int16_t pulse, int8_t direction) |
sakanakuuun | 0:265d8ad19d2a | 84 | { |
sakanakuuun | 0:265d8ad19d2a | 85 | if(direction == 0) |
sakanakuuun | 0:265d8ad19d2a | 86 | pulse = pulse - 0xffff -1; |
sakanakuuun | 0:265d8ad19d2a | 87 | |
sakanakuuun | 0:265d8ad19d2a | 88 | return pulse; |
sakanakuuun | 0:265d8ad19d2a | 89 | } |
sakanakuuun | 0:265d8ad19d2a | 90 | |
sakanakuuun | 0:265d8ad19d2a | 91 | void erase() |
sakanakuuun | 0:265d8ad19d2a | 92 | { |
sakanakuuun | 0:265d8ad19d2a | 93 | x = 0; |
sakanakuuun | 0:265d8ad19d2a | 94 | y = 0; |
sakanakuuun | 0:265d8ad19d2a | 95 | d_theta = theta; |
sakanakuuun | 0:265d8ad19d2a | 96 | } |