NITICRobotClub 2018Team-B / Mbed 2 deprecated Lets_move_Seki2018_null

自動投射のみです。（手動用にチューニングしました）

Dependencies:   HCSR04 PID QEI mbed

Fork of Lets_move_Seki2018_ver2 by INCTRC Information Sharing Test

main.cpp@3:1223cab367d9, 2018-08-27 (annotated)

Committer:
yuron
Date:
Mon Aug 27 08:15:36 2018 +0000
Revision:
3:1223cab367d9
Parent:
2:c32991ba628f
Child:
4:df334779a69e
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuron 0:f73c1b076ae4 1 /* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
yuron 2:c32991ba628f 2 /* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
yuron 0:f73c1b076ae4 3 /* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
yuron 0:f73c1b076ae4 4 /* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
yuron 0:f73c1b076ae4 5 /* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
yuron 0:f73c1b076ae4 6 /* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
yuron 0:f73c1b076ae4 7 /* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 8 /* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 9 /* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
yuron 0:f73c1b076ae4 10 /* 2018/6/9追記 */
yuron 0:f73c1b076ae4 11 /* ついに回転数の！PI制御を実現できました。 */
yuron 0:f73c1b076ae4 12 /* タイマ割込みで角速度を取得してみようと思います。 */
yuron 0:f73c1b076ae4 13
yuron 0:f73c1b076ae4 14 #include "mbed.h"
yuron 0:f73c1b076ae4 15 #include "math.h"
yuron 0:f73c1b076ae4 16 #include "QEI.h"
yuron 0:f73c1b076ae4 17 #include "PID.h"
yuron 0:f73c1b076ae4 18 #define PI 3.14159265359
yuron 0:f73c1b076ae4 19
yuron 0:f73c1b076ae4 20 PID controller(0.7, 0.7, 0.0, 0.001);
yuron 2:c32991ba628f 21 PID RF(0.5, 0.4, 0.0, 0.001);
yuron 0:f73c1b076ae4 22 PID RB(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 23 PID LF(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 24 PID LB(0.5, 0.3, 0.0, 0.001);
yuron 0:f73c1b076ae4 25 I2C i2c(PB_3, PB_10); //SDA, SCL
yuron 1:62b321f6c9c3 26 Serial pc(USBTX, USBRX);
yuron 2:c32991ba628f 27 Serial photo(D8, D2);
yuron 0:f73c1b076ae4 28 DigitalOut emergency(PA_13);
yuron 1:62b321f6c9c3 29 DigitalOut myled(D13);
yuron 0:f73c1b076ae4 30
yuron 0:f73c1b076ae4 31 /* 以下446基板用 */
yuron 0:f73c1b076ae4 32 QEI wheel_LB(PA_1, PA_0, NC, 624);
yuron 0:f73c1b076ae4 33 QEI wheel_RB(PB_6, PB_7, NC, 624);
yuron 0:f73c1b076ae4 34 QEI wheel_LF(PB_4, PB_5, NC, 624);
yuron 0:f73c1b076ae4 35 QEI wheel_RF(PC_8, PC_9, NC, 624);
yuron 0:f73c1b076ae4 36
yuron 0:f73c1b076ae4 37 Ticker get_RPS;
yuron 0:f73c1b076ae4 38 Ticker print_RPS;
yuron 1:62b321f6c9c3 39 Timer timer;
yuron 0:f73c1b076ae4 40
yuron 0:f73c1b076ae4 41 int rps_RF;
yuron 0:f73c1b076ae4 42 int rps_RB;
yuron 0:f73c1b076ae4 43 int rps_LF;
yuron 0:f73c1b076ae4 44 int rps_LB;
yuron 0:f73c1b076ae4 45
yuron 0:f73c1b076ae4 46 int rpm_RF;
yuron 0:f73c1b076ae4 47 int rpm_RB;
yuron 0:f73c1b076ae4 48 int rpm_LF;
yuron 0:f73c1b076ae4 49 int rpm_LB;
yuron 0:f73c1b076ae4 50
yuron 0:f73c1b076ae4 51 int pulse_RF;
yuron 0:f73c1b076ae4 52 int pulse_RB;
yuron 0:f73c1b076ae4 53 int pulse_LF;
yuron 0:f73c1b076ae4 54 int pulse_LB;
yuron 0:f73c1b076ae4 55
yuron 0:f73c1b076ae4 56 int counter_RF;
yuron 0:f73c1b076ae4 57 int counter_RB;
yuron 0:f73c1b076ae4 58 int counter_LF;
yuron 0:f73c1b076ae4 59 int counter_LB;
yuron 0:f73c1b076ae4 60
yuron 0:f73c1b076ae4 61 double a_v; /* 角速度 */
yuron 0:f73c1b076ae4 62 double n_v; /* 速度(秒速) */
yuron 0:f73c1b076ae4 63 double h_v; /* 速度(時速) */
yuron 0:f73c1b076ae4 64 double n_a; /* 加速度 */
yuron 0:f73c1b076ae4 65
yuron 0:f73c1b076ae4 66 char send_data[1];
yuron 0:f73c1b076ae4 67 char true_send_data[1];
yuron 0:f73c1b076ae4 68
yuron 0:f73c1b076ae4 69 char RF_data[1];
yuron 0:f73c1b076ae4 70 char RB_data[1];
yuron 0:f73c1b076ae4 71 char LF_data[1];
yuron 0:f73c1b076ae4 72 char LB_data[1];
yuron 0:f73c1b076ae4 73 char true_RF_data[1];
yuron 0:f73c1b076ae4 74 char true_RB_data[1];
yuron 0:f73c1b076ae4 75 char true_LF_data[1];
yuron 0:f73c1b076ae4 76 char true_LB_data[1];
yuron 0:f73c1b076ae4 77
yuron 0:f73c1b076ae4 78 /* ロリコン処理関数 */
yuron 0:f73c1b076ae4 79 void flip();
yuron 0:f73c1b076ae4 80 /* RPS表示関数 */
yuron 0:f73c1b076ae4 81 void flip2();
yuron 0:f73c1b076ae4 82
yuron 0:f73c1b076ae4 83 void flip(){
yuron 0:f73c1b076ae4 84
yuron 0:f73c1b076ae4 85 pulse_RF = wheel_RF.getPulses();
yuron 0:f73c1b076ae4 86 pulse_RB = wheel_RB.getPulses();
yuron 0:f73c1b076ae4 87 pulse_LF = wheel_LF.getPulses();
yuron 0:f73c1b076ae4 88 pulse_LB = wheel_LB.getPulses();
yuron 0:f73c1b076ae4 89
yuron 0:f73c1b076ae4 90 /* *rps変換 */
yuron 0:f73c1b076ae4 91 /*10ms*100 = 1s
yuron 0:f73c1b076ae4 92 counter_RB = pulse_RB * 100;
yuron 0:f73c1b076ae4 93 counter_LB = pulse_LB * 100;
yuron 0:f73c1b076ae4 94 */
yuron 0:f73c1b076ae4 95
yuron 0:f73c1b076ae4 96 //40ms*25 = 1s
yuron 0:f73c1b076ae4 97 counter_RF = pulse_RF * 25;
yuron 0:f73c1b076ae4 98 counter_RB = pulse_RB * 25;
yuron 0:f73c1b076ae4 99 counter_LF = pulse_LF * 25;
yuron 0:f73c1b076ae4 100 counter_LB = pulse_LB * 25;
yuron 0:f73c1b076ae4 101
yuron 0:f73c1b076ae4 102 /*
yuron 0:f73c1b076ae4 103 //100ms*10 = 1s
yuron 0:f73c1b076ae4 104 counter_RB = pulse_RB * 10;
yuron 0:f73c1b076ae4 105 counter_LB = pulse_LB * 10;
yuron 0:f73c1b076ae4 106 */
yuron 0:f73c1b076ae4 107
yuron 0:f73c1b076ae4 108 /* /分解能 */
yuron 0:f73c1b076ae4 109 rps_RF = counter_RF / 100;
yuron 0:f73c1b076ae4 110 rps_RB = counter_RB / 100;
yuron 0:f73c1b076ae4 111 rps_LF = counter_LF / 100;
yuron 0:f73c1b076ae4 112 rps_LB = counter_LB / 100;
yuron 0:f73c1b076ae4 113
yuron 0:f73c1b076ae4 114 rpm_RF = pulse_RF * 25 * 60 / 100;
yuron 0:f73c1b076ae4 115 rpm_RB = pulse_RB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 116 rpm_LF = pulse_LF * 25 * 60 / 100;
yuron 0:f73c1b076ae4 117 rpm_LB = pulse_LB * 25 * 60 / 100;
yuron 0:f73c1b076ae4 118
yuron 0:f73c1b076ae4 119
yuron 0:f73c1b076ae4 120 /* RPMから角速度へ変換 */
yuron 0:f73c1b076ae4 121 a_v = rpm_LB * PI / 30;
yuron 0:f73c1b076ae4 122
yuron 0:f73c1b076ae4 123 /* RPMから速度(秒速)へ変換 */
yuron 0:f73c1b076ae4 124 /* タイヤ半径を0.05[m]とする */
yuron 0:f73c1b076ae4 125 n_v = 0.05 * 2 * PI * rpm_LB / 60;
yuron 0:f73c1b076ae4 126
yuron 0:f73c1b076ae4 127 /* 速度(秒速)から速度(時速)へ変換 */
yuron 0:f73c1b076ae4 128 h_v = n_v * 3600 / 1000;
yuron 0:f73c1b076ae4 129
yuron 0:f73c1b076ae4 130 //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
yuron 3:1223cab367d9 131 pc.printf("前: %d, 後: %d\n", rpm_LB, rpm_LF);
yuron 3:1223cab367d9 132 //pc.printf("%d\n", rpm_LB);
yuron 0:f73c1b076ae4 133
yuron 0:f73c1b076ae4 134 wheel_RF.reset();
yuron 0:f73c1b076ae4 135 wheel_RB.reset();
yuron 0:f73c1b076ae4 136 wheel_LF.reset();
yuron 0:f73c1b076ae4 137 wheel_LB.reset();
yuron 0:f73c1b076ae4 138 }
yuron 2:c32991ba628f 139 int front_PID(int setpoint)
yuron 0:f73c1b076ae4 140 {
yuron 0:f73c1b076ae4 141 // センサ出力値の最小、最大
yuron 2:c32991ba628f 142 RF.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 143 RB.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 144 LF.setInputLimits(-2000, 2000);
yuron 2:c32991ba628f 145 LB.setInputLimits(-2000, 2000);
yuron 0:f73c1b076ae4 146
yuron 0:f73c1b076ae4 147 // 制御量の最小、最大
yuron 2:c32991ba628f 148 RF.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 149 RB.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 150 LF.setOutputLimits(0x0C, 0x7C);
yuron 2:c32991ba628f 151 LB.setOutputLimits(0x0C, 0x7C);
yuron 0:f73c1b076ae4 152
yuron 0:f73c1b076ae4 153 // よくわからんやつ
yuron 0:f73c1b076ae4 154 RF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 155 RB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 156 LF.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 157 LB.setMode(AUTO_MODE);
yuron 0:f73c1b076ae4 158
yuron 0:f73c1b076ae4 159 // 目標値
yuron 2:c32991ba628f 160 RF.setSetPoint(setpoint);
yuron 2:c32991ba628f 161 RB.setSetPoint(setpoint);
yuron 2:c32991ba628f 162 LF.setSetPoint(setpoint);
yuron 2:c32991ba628f 163 LB.setSetPoint(setpoint);
yuron 0:f73c1b076ae4 164
yuron 0:f73c1b076ae4 165 // センサ出力
yuron 1:62b321f6c9c3 166 RF.setProcessValue(rpm_RF);
yuron 1:62b321f6c9c3 167 RB.setProcessValue(rpm_RB);
yuron 1:62b321f6c9c3 168 LF.setProcessValue(rpm_LF);
yuron 1:62b321f6c9c3 169 LB.setProcessValue(rpm_LB);
yuron 0:f73c1b076ae4 170
yuron 0:f73c1b076ae4 171 // 制御量(計算結果)
yuron 0:f73c1b076ae4 172 RF_data[0] = RF.compute();
yuron 0:f73c1b076ae4 173 RB_data[0] = RB.compute();
yuron 0:f73c1b076ae4 174 LF_data[0] = LF.compute();
yuron 0:f73c1b076ae4 175 LB_data[0] = LB.compute();
yuron 0:f73c1b076ae4 176
yuron 0:f73c1b076ae4 177 // 制御量をPWM値に変換
yuron 2:c32991ba628f 178 true_RF_data[0] = 0x7D - RF_data[0];
yuron 2:c32991ba628f 179 true_RB_data[0] = 0x7D - RB_data[0];
yuron 2:c32991ba628f 180 true_LF_data[0] = 0x7D - LF_data[0];
yuron 2:c32991ba628f 181 true_LB_data[0] = 0x7D - LB_data[0];
yuron 2:c32991ba628f 182
yuron 2:c32991ba628f 183 return 0;
yuron 0:f73c1b076ae4 184 }
yuron 3:1223cab367d9 185 int back_PID(int setpoint1, int setpoint2)
yuron 1:62b321f6c9c3 186 {
yuron 1:62b321f6c9c3 187 // センサ出力値の最小、最大
yuron 2:c32991ba628f 188 RF.setInputLimits(0, 2000);
yuron 2:c32991ba628f 189 RB.setInputLimits(0, 2000);
yuron 2:c32991ba628f 190 LF.setInputLimits(0, 2000);
yuron 2:c32991ba628f 191 LB.setInputLimits(0, 2000);
yuron 1:62b321f6c9c3 192
yuron 1:62b321f6c9c3 193 // 制御量の最小、最大
yuron 2:c32991ba628f 194 RF.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 195 RB.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 196 LF.setOutputLimits(0x84, 0xFF);
yuron 2:c32991ba628f 197 LB.setOutputLimits(0x84, 0xFF);
yuron 1:62b321f6c9c3 198
yuron 1:62b321f6c9c3 199 // よくわからんやつ
yuron 1:62b321f6c9c3 200 RF.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 201 RB.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 202 LF.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 203 LB.setMode(AUTO_MODE);
yuron 1:62b321f6c9c3 204
yuron 1:62b321f6c9c3 205 // 目標値
yuron 3:1223cab367d9 206 RF.setSetPoint(setpoint1);
yuron 3:1223cab367d9 207 RB.setSetPoint(setpoint1);
yuron 3:1223cab367d9 208 LF.setSetPoint(setpoint1);
yuron 3:1223cab367d9 209 LB.setSetPoint(setpoint2);
yuron 1:62b321f6c9c3 210
yuron 1:62b321f6c9c3 211 // センサ出力
yuron 2:c32991ba628f 212 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron 2:c32991ba628f 213 RF.setProcessValue(abs(rpm_RF));
yuron 2:c32991ba628f 214 RB.setProcessValue(abs(rpm_RB));
yuron 2:c32991ba628f 215 LF.setProcessValue(abs(rpm_LF));
yuron 2:c32991ba628f 216 LB.setProcessValue(abs(rpm_LB));
yuron 1:62b321f6c9c3 217
yuron 1:62b321f6c9c3 218 // 制御量(計算結果)
yuron 1:62b321f6c9c3 219 RF_data[0] = RF.compute();
yuron 1:62b321f6c9c3 220 RB_data[0] = RB.compute();
yuron 1:62b321f6c9c3 221 LF_data[0] = LF.compute();
yuron 1:62b321f6c9c3 222 LB_data[0] = LB.compute();
yuron 1:62b321f6c9c3 223
yuron 2:c32991ba628f 224 return 0;
yuron 1:62b321f6c9c3 225 }
yuron 0:f73c1b076ae4 226 int main(void)
yuron 0:f73c1b076ae4 227 {
yuron 0:f73c1b076ae4 228 emergency = 0;
yuron 2:c32991ba628f 229 send_data[0] = 0x80;
yuron 3:1223cab367d9 230 //i2c.write(0x10, send_data, 1);
yuron 3:1223cab367d9 231 //i2c.write(0x12, send_data, 1);
yuron 3:1223cab367d9 232 //i2c.write(0x14, send_data, 1);
yuron 3:1223cab367d9 233 //i2c.write(0x16, send_data, 1);
yuron 3:1223cab367d9 234 i2c.write(0x20, send_data, 1);
yuron 3:1223cab367d9 235 i2c.write(0x22, send_data, 1);
yuron 0:f73c1b076ae4 236 wait(0.1);
yuron 0:f73c1b076ae4 237
yuron 0:f73c1b076ae4 238 /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
yuron 0:f73c1b076ae4 239 get_RPS.attach_us(&flip, 40000);
yuron 0:f73c1b076ae4 240 //get_RPS.attach_us(&flip, 100000);
yuron 3:1223cab367d9 241
yuron 0:f73c1b076ae4 242 while(1)
yuron 0:f73c1b076ae4 243 {
yuron 2:c32991ba628f 244 /*
yuron 3:1223cab367d9 245 front_PID(300);
yuron 3:1223cab367d9 246 i2c.write(0x20, true_LB_data, 1, false);
yuron 3:1223cab367d9 247 i2c.write(0x22, true_LF_data, 1, false);
yuron 2:c32991ba628f 248 wait_us(20);
yuron 2:c32991ba628f 249 */
yuron 0:f73c1b076ae4 250
yuron 3:1223cab367d9 251
yuron 3:1223cab367d9 252 //back_PID(430, 730);
yuron 3:1223cab367d9 253 //back_PID(300, 500);
yuron 3:1223cab367d9 254 back_PID(325, 650);
yuron 3:1223cab367d9 255 //back_PID(250, 900);
yuron 3:1223cab367d9 256 //LB_data[0] = 0x0C;
yuron 3:1223cab367d9 257 //LF_data[0] = 0x0C;
yuron 3:1223cab367d9 258 i2c.write(0x10, LB_data, 1, false);
yuron 3:1223cab367d9 259 i2c.write(0x22, LF_data, 1, false);
yuron 3:1223cab367d9 260 wait_us(20);
yuron 3:1223cab367d9 261
yuron 3:1223cab367d9 262 /*
yuron 2:c32991ba628f 263 timer.start();
yuron 1:62b321f6c9c3 264 while(timer.read() <= 5.0f) {
yuron 2:c32991ba628f 265 myled = 1;
yuron 2:c32991ba628f 266 front_PID(200);
yuron 2:c32991ba628f 267 i2c.write(0x20, true_LF_data, 1, false);
yuron 1:62b321f6c9c3 268 wait_us(20);
yuron 1:62b321f6c9c3 269 }
yuron 1:62b321f6c9c3 270 LF.reset();
yuron 2:c32991ba628f 271 while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
yuron 2:c32991ba628f 272 myled = 0;
yuron 2:c32991ba628f 273 true_LF_data[0] = 0x80;
yuron 2:c32991ba628f 274 i2c.write(0x20, true_LF_data, 1, false);
yuron 2:c32991ba628f 275 wait_us(20);
yuron 2:c32991ba628f 276 }
yuron 2:c32991ba628f 277 while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
yuron 2:c32991ba628f 278 myled = 1;
yuron 2:c32991ba628f 279 back_PID(200);
yuron 2:c32991ba628f 280 i2c.write(0x20, LF_data, 1, false);
yuron 2:c32991ba628f 281 wait_us(20);
yuron 2:c32991ba628f 282 }
yuron 2:c32991ba628f 283 LF.reset();
yuron 2:c32991ba628f 284 while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
yuron 2:c32991ba628f 285 myled = 0;
yuron 2:c32991ba628f 286 true_LF_data[0] = 0x80;
yuron 2:c32991ba628f 287 i2c.write(0x20, true_LF_data, 1, false);
yuron 2:c32991ba628f 288 wait_us(20);
yuron 1:62b321f6c9c3 289 }
yuron 1:62b321f6c9c3 290 timer.reset();
yuron 0:f73c1b076ae4 291 */
yuron 0:f73c1b076ae4 292
yuron 0:f73c1b076ae4 293 /*
yuron 2:c32991ba628f 294 //どんどん加速(逆転)
yuron 2:c32991ba628f 295 for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){
yuron 0:f73c1b076ae4 296 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 297 //ice(0xA2, send_data);
yuron 2:c32991ba628f 298
yuron 0:f73c1b076ae4 299 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 300 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 301 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 302 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 303
yuron 2:c32991ba628f 304 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 305 wait(0.1);
yuron 0:f73c1b076ae4 306 }
yuron 2:c32991ba628f 307 //だんだん減速(逆転)
yuron 2:c32991ba628f 308 for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){
yuron 0:f73c1b076ae4 309 //ice(0x88, send_data);
yuron 0:f73c1b076ae4 310 //ice(0xA2, send_data);
yuron 2:c32991ba628f 311
yuron 0:f73c1b076ae4 312 i2c.write(0xA0, send_data, 1);
yuron 0:f73c1b076ae4 313 i2c.write(0xA2, send_data, 1);
yuron 0:f73c1b076ae4 314 i2c.write(0xA4, send_data, 1);
yuron 0:f73c1b076ae4 315 i2c.write(0xA6, send_data, 1);
yuron 2:c32991ba628f 316
yuron 2:c32991ba628f 317 i2c.write(0x20, send_data, 1);
yuron 0:f73c1b076ae4 318 wait(0.1);
yuron 0:f73c1b076ae4 319 }
yuron 2:c32991ba628f 320 send_data[0] = 0x80;
yuron 2:c32991ba628f 321 i2c.write(0x20, send_data, 1);
yuron 2:c32991ba628f 322 wait(5);
yuron 0:f73c1b076ae4 323 */
yuron 0:f73c1b076ae4 324 }
yuron 0:f73c1b076ae4 325 }