涼太郎 中村 / Locate

a

Dependents:   4thcomp 6th33222_copy

Fork of Locate by Tk A

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?

UserRevisionLine numberNew 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