a
Dependents: 4thcomp 6th33222_copy
Fork of Locate by
locate.cpp@10:d88fe87720d2, 2016-09-17 (annotated)
- Committer:
- choutin
- Date:
- Sat Sep 17 23:20:49 2016 +0000
- Revision:
- 10:d88fe87720d2
- Parent:
- 9:69fe7b8350c6
?
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" |
choutin | 7:08dfba38d5d9 | 5 | |
choutin | 7:08dfba38d5d9 | 6 | |
sakanakuuun | 0:265d8ad19d2a | 7 | /********以下グローバル変数宣言**************/ |
choutin | 10:d88fe87720d2 | 8 | Serial pc(SERIAL_TX, SERIAL_RX); |
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ライブラリで使う |
choutin | 7:08dfba38d5d9 | 13 | |
choutin | 10:d88fe87720d2 | 14 | |
sakanakuuun | 0:265d8ad19d2a | 15 | DigitalOut enc_v(PC_7); //エンコーダの電源 |
choutin | 7:08dfba38d5d9 | 16 | |
choutin | 6:ba3ef4656f7a | 17 | int r, l; //現在の回転数 |
choutin | 6:ba3ef4656f7a | 18 | int pr = 0, pl = 0; //前回のステップ数 |
choutin | 6:ba3ef4656f7a | 19 | int v = 0; //ステップ速度 |
choutin | 6:ba3ef4656f7a | 20 | float xcount = 0, ycount = 0;//xy方向に進んだ距離(m換算なし) |
choutin | 6:ba3ef4656f7a | 21 | float theta = 0; //機体角度、x軸正の向きを0とする |
choutin | 6:ba3ef4656f7a | 22 | float d_theta = 0; //erase()関数を使って、そのときのthetaをコピー |
choutin | 6:ba3ef4656f7a | 23 | float virtual_xcount = 0 ,virtual_ycount = 0; |
choutin | 6:ba3ef4656f7a | 24 | int *virtual_v; |
choutin | 6:ba3ef4656f7a | 25 | float virtual_theta = 0; |
choutin | 6:ba3ef4656f7a | 26 | float virtual_ptheta = 0; |
choutin | 8:ecd49a9ee2ee | 27 | int teamcolor = -1; |
sakanakuuun | 0:265d8ad19d2a | 28 | /*************変数宣言終了******************/ |
choutin | 7:08dfba38d5d9 | 29 | |
choutin | 7:08dfba38d5d9 | 30 | |
choutin | 7:08dfba38d5d9 | 31 | |
choutin | 7:08dfba38d5d9 | 32 | |
choutin | 7:08dfba38d5d9 | 33 | void setup(int team) |
sakanakuuun | 4:ecbb45e84c08 | 34 | { |
sakanakuuun | 4:ecbb45e84c08 | 35 | enc_v = 1; |
choutin | 7:08dfba38d5d9 | 36 | |
sakanakuuun | 4:ecbb45e84c08 | 37 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 4:ecbb45e84c08 | 38 | //use PB6 PB7 = Nucleo D7 D8 |
choutin | 9:69fe7b8350c6 | 39 | EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI1); |
choutin | 7:08dfba38d5d9 | 40 | |
choutin | 7:08dfba38d5d9 | 41 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
choutin | 7:08dfba38d5d9 | 42 | //use PA0 PA1 = Nucleo A0 A1 |
choutin | 9:69fe7b8350c6 | 43 | EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI1); |
choutin | 7:08dfba38d5d9 | 44 | |
choutin | 7:08dfba38d5d9 | 45 | teamcolor = team; |
choutin | 7:08dfba38d5d9 | 46 | } |
choutin | 7:08dfba38d5d9 | 47 | |
choutin | 7:08dfba38d5d9 | 48 | void setup(int tx, int ty) |
choutin | 7:08dfba38d5d9 | 49 | { |
choutin | 7:08dfba38d5d9 | 50 | enc_v = 1; |
choutin | 7:08dfba38d5d9 | 51 | |
choutin | 7:08dfba38d5d9 | 52 | xcount = tx / (LOCATE_STEP / 2); |
choutin | 7:08dfba38d5d9 | 53 | ycount = ty / (LOCATE_STEP / 2); |
choutin | 7:08dfba38d5d9 | 54 | |
choutin | 7:08dfba38d5d9 | 55 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
choutin | 7:08dfba38d5d9 | 56 | //use PB6 PB7 = Nucleo D7 D8 |
choutin | 7:08dfba38d5d9 | 57 | EncoderInit(&encoder1, &timer1, TIM4, 0xffff, TIM_ENCODERMODE_TI12); |
choutin | 7:08dfba38d5d9 | 58 | |
sakanakuuun | 4:ecbb45e84c08 | 59 | //counting on both A&B inputs, 4 ticks per cycle, 16-bit count |
sakanakuuun | 4:ecbb45e84c08 | 60 | //use PA0 PA1 = Nucleo A0 A1 |
sakanakuuun | 4:ecbb45e84c08 | 61 | EncoderInit(&encoder2, &timer2, TIM2, 0xffff, TIM_ENCODERMODE_TI12); |
sakanakuuun | 4:ecbb45e84c08 | 62 | } |
choutin | 7:08dfba38d5d9 | 63 | |
choutin | 6:ba3ef4656f7a | 64 | void update() |
sakanakuuun | 0:265d8ad19d2a | 65 | //位置情報を更新する。r,lはエンコーダから |
sakanakuuun | 0:265d8ad19d2a | 66 | { |
sakanakuuun | 0:265d8ad19d2a | 67 | count1=__HAL_TIM_GET_COUNTER(&timer1); |
sakanakuuun | 0:265d8ad19d2a | 68 | dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1); |
sakanakuuun | 0:265d8ad19d2a | 69 | count2=__HAL_TIM_GET_COUNTER(&timer2); |
choutin | 6:ba3ef4656f7a | 70 | dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
choutin | 7:08dfba38d5d9 | 71 | |
sakanakuuun | 0:265d8ad19d2a | 72 | r = -convert_enc_count(count1, dir1); |
sakanakuuun | 1:3513a2fbd81f | 73 | l = -convert_enc_count(count2, dir2); |
choutin | 7:08dfba38d5d9 | 74 | |
choutin | 7:08dfba38d5d9 | 75 | theta = teamcolor*(r - l) * ROUND; |
choutin | 6:ba3ef4656f7a | 76 | //theta = (r - l) * ROUND - d_theta; |
sakanakuuun | 0:265d8ad19d2a | 77 | v = (r - pr + l - pl); |
choutin | 7:08dfba38d5d9 | 78 | |
sakanakuuun | 4:ecbb45e84c08 | 79 | xcount += v * cos(theta); |
choutin | 8:ecd49a9ee2ee | 80 | ycount += v * sin(theta); |
choutin | 7:08dfba38d5d9 | 81 | |
sakanakuuun | 0:265d8ad19d2a | 82 | pr = r; |
sakanakuuun | 0:265d8ad19d2a | 83 | pl = l; |
sakanakuuun | 0:265d8ad19d2a | 84 | //pc.printf("count1:%d%s count2:%d%s\r\n", count1, dir1==0 ? "+":"-",count2, dir2==0 ? "+":"-"); |
choutin | 10:d88fe87720d2 | 85 | pc.printf("right:%d left:%d x:%d y:%d t:%f d%d\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta(),r-l); |
sakanakuuun | 0:265d8ad19d2a | 86 | } |
choutin | 7:08dfba38d5d9 | 87 | |
sakanakuuun | 4:ecbb45e84c08 | 88 | void update_np() |
sakanakuuun | 0:265d8ad19d2a | 89 | { |
sakanakuuun | 4:ecbb45e84c08 | 90 | count1=__HAL_TIM_GET_COUNTER(&timer1); |
sakanakuuun | 4:ecbb45e84c08 | 91 | dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer1); |
sakanakuuun | 4:ecbb45e84c08 | 92 | count2=__HAL_TIM_GET_COUNTER(&timer2); |
choutin | 6:ba3ef4656f7a | 93 | dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
choutin | 7:08dfba38d5d9 | 94 | |
sakanakuuun | 4:ecbb45e84c08 | 95 | r = -convert_enc_count(count1, dir1); |
sakanakuuun | 4:ecbb45e84c08 | 96 | l = -convert_enc_count(count2, dir2); |
choutin | 7:08dfba38d5d9 | 97 | |
choutin | 8:ecd49a9ee2ee | 98 | theta = teamcolor * (r - l) * ROUND; |
sakanakuuun | 4:ecbb45e84c08 | 99 | v = (r - pr + l - pl); |
choutin | 7:08dfba38d5d9 | 100 | |
sakanakuuun | 4:ecbb45e84c08 | 101 | xcount += v * cos(theta); |
sakanakuuun | 4:ecbb45e84c08 | 102 | ycount += v * sin(theta); |
choutin | 7:08dfba38d5d9 | 103 | |
sakanakuuun | 4:ecbb45e84c08 | 104 | pr = r; |
sakanakuuun | 4:ecbb45e84c08 | 105 | pl = l; |
choutin | 10:d88fe87720d2 | 106 | pc.printf("right:%d left:%d x:%d y:%d t:%f d%d\n\r", r, l, coordinateX(), coordinateY(), coordinateTheta(),r-l); |
sakanakuuun | 0:265d8ad19d2a | 107 | } |
choutin | 7:08dfba38d5d9 | 108 | |
sakanakuuun | 1:3513a2fbd81f | 109 | int coordinateX() |
sakanakuuun | 0:265d8ad19d2a | 110 | { |
sakanakuuun | 4:ecbb45e84c08 | 111 | return xcount * LOCATE_STEP / 2; |
sakanakuuun | 0:265d8ad19d2a | 112 | } |
choutin | 7:08dfba38d5d9 | 113 | |
sakanakuuun | 1:3513a2fbd81f | 114 | int coordinateY() |
sakanakuuun | 0:265d8ad19d2a | 115 | { |
choutin | 8:ecd49a9ee2ee | 116 | return ycount * LOCATE_STEP / 2; |
sakanakuuun | 0:265d8ad19d2a | 117 | } |
choutin | 7:08dfba38d5d9 | 118 | |
sakanakuuun | 0:265d8ad19d2a | 119 | float coordinateTheta() |
sakanakuuun | 0:265d8ad19d2a | 120 | { |
sakanakuuun | 0:265d8ad19d2a | 121 | return theta; |
sakanakuuun | 0:265d8ad19d2a | 122 | } |
choutin | 7:08dfba38d5d9 | 123 | |
sakanakuuun | 0:265d8ad19d2a | 124 | int convert_enc_count(int16_t pulse, int8_t direction) |
sakanakuuun | 0:265d8ad19d2a | 125 | { |
sakanakuuun | 0:265d8ad19d2a | 126 | if(direction == 0) |
sakanakuuun | 0:265d8ad19d2a | 127 | pulse = pulse - 0xffff -1; |
choutin | 7:08dfba38d5d9 | 128 | |
sakanakuuun | 0:265d8ad19d2a | 129 | return pulse; |
sakanakuuun | 0:265d8ad19d2a | 130 | } |
choutin | 7:08dfba38d5d9 | 131 | |
sakanakuuun | 0:265d8ad19d2a | 132 | void erase() |
sakanakuuun | 0:265d8ad19d2a | 133 | { |
sakanakuuun | 4:ecbb45e84c08 | 134 | xcount = 0; |
sakanakuuun | 4:ecbb45e84c08 | 135 | ycount = 0; |
sakanakuuun | 0:265d8ad19d2a | 136 | d_theta = theta; |
sakanakuuun | 0:265d8ad19d2a | 137 | } |
choutin | 7:08dfba38d5d9 | 138 | |
choutin | 6:ba3ef4656f7a | 139 | void virtual_setup() |
choutin | 6:ba3ef4656f7a | 140 | { |
choutin | 6:ba3ef4656f7a | 141 | virtual_xcount = 0; |
choutin | 6:ba3ef4656f7a | 142 | virtual_ycount = 0; |
choutin | 7:08dfba38d5d9 | 143 | |
choutin | 6:ba3ef4656f7a | 144 | virtual_v = &v; |
choutin | 7:08dfba38d5d9 | 145 | |
choutin | 6:ba3ef4656f7a | 146 | virtual_theta = 0; |
choutin | 6:ba3ef4656f7a | 147 | virtual_ptheta = coordinateTheta(); |
choutin | 6:ba3ef4656f7a | 148 | } |
choutin | 7:08dfba38d5d9 | 149 | |
choutin | 6:ba3ef4656f7a | 150 | void virtual_update() |
choutin | 6:ba3ef4656f7a | 151 | { |
choutin | 6:ba3ef4656f7a | 152 | virtual_theta = coordinateTheta() - virtual_ptheta; |
choutin | 7:08dfba38d5d9 | 153 | |
choutin | 6:ba3ef4656f7a | 154 | virtual_xcount += *virtual_v * cos(virtual_theta); |
choutin | 7:08dfba38d5d9 | 155 | virtual_ycount -= *virtual_v * sin(virtual_theta); |
choutin | 6:ba3ef4656f7a | 156 | } |
choutin | 7:08dfba38d5d9 | 157 | |
choutin | 6:ba3ef4656f7a | 158 | int virtual_coordinateX() |
choutin | 6:ba3ef4656f7a | 159 | { |
choutin | 6:ba3ef4656f7a | 160 | return virtual_xcount * LOCATE_STEP / 2; |
choutin | 6:ba3ef4656f7a | 161 | } |
choutin | 7:08dfba38d5d9 | 162 | |
choutin | 6:ba3ef4656f7a | 163 | int virtual_coordinateY() |
choutin | 6:ba3ef4656f7a | 164 | { |
choutin | 6:ba3ef4656f7a | 165 | return virtual_ycount * LOCATE_STEP / 2; |
choutin | 6:ba3ef4656f7a | 166 | } |
choutin | 7:08dfba38d5d9 | 167 | |
choutin | 6:ba3ef4656f7a | 168 | float virtual_coordinateTheta() |
choutin | 6:ba3ef4656f7a | 169 | { |
choutin | 6:ba3ef4656f7a | 170 | return virtual_theta; |
choutin | 6:ba3ef4656f7a | 171 | } |
choutin | 7:08dfba38d5d9 | 172 |