INCTRC Information Sharing Test
2018/09/05現在のBチーム自動機の制御プログラム(バックアップも兼ねて)
Dependencies: HCSR04 PID QEI mbed
Fork of
Lets_move_Seki2018
by 
INCTRC Information Sharing Test
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* ------------------------------------------------------------------- */ 00002 /* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */ 00003 /* ------------------------------------------------------------------- */ 00004 /* このプログラムは上記のロボット専用の制御プログラムである。 */ 00005 /* ------------------------------------------------------------------- */ 00006 /* 対応機種: NUCLEO-F446RE */ 00007 /* ------------------------------------------------------------------- */ 00008 /* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */ 00009 /* ------------------------------------------------------------------- */ 00010 /* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */ 00011 /* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */ 00012 /* ------------------------------------------------------------------- */ 00013 #include "mbed.h" 00014 #include "math.h" 00015 #include "QEI.h" 00016 #include "PID.h" 00017 #include "hcsr04.h" 00018 #define PI 3.14159265359 00019 #define Kp 20.0 00020 #define Ki 0.02 00021 #define Kd 0.0 00022 //#define roller_Kp 0.5 00023 //#define roller_Ki 0.3 00024 #define roller_Kp 2.5 00025 #define roller_Ki 0.01 00026 00027 //右前オムニ 00028 PID migimae(Kp, Ki, Kd, 0.001); 00029 //右後オムニ 00030 PID migiusiro(Kp, Ki, Kd, 0.001); 00031 //左前オムニ 00032 PID hidarimae(Kp, Ki, Kd, 0.001); 00033 //左後オムニ 00034 PID hidariusiro(Kp, Ki, Kd, 0.001); 00035 //前ローラー 00036 PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001); 00037 //後ローラー 00038 PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001); 00039 00040 //MDとの通信ポート 00041 I2C i2c(PB_9, PB_8); //SDA, SCL 00042 //PCとの通信ポート 00043 Serial pc(USBTX, USBRX); //TX, RX 00044 //フォトインタラプタ制御基板からの受信ポート 00045 Serial photo(NC, PC_11); //TX, RX 00046 //TWE-Liteからの受信ポート 00047 Serial twe(PC_12, PD_2); //TX, RX 00048 00049 //超音波センサ1 00050 HCSR04 sonic(PB_3, PB_10); //Trig, Echo 00051 //超音波センサ2 00052 //HCSR04 sonic2(PC_13, PA_15); //Trig, Echo 00053 //超音波センサ3 00054 //HCSR04 sonic3(PC_15, PC_14); //Trig, Echo 00055 //超音波センサ4 00056 //HCSR04 sonic4(PH_1 , PH_0 ); //Trig, Echo 00057 00058 //赤、青ゾーン切り替え用スイッチ 00059 DigitalIn side(PC_1); 00060 //スタートボタン 00061 DigitalIn start_button(PC_4); 00062 //スイッチ1 00063 DigitalIn user_switch1(PB_0); 00064 //スイッチ2 00065 DigitalIn user_switch2(PA_4); 00066 //スイッチ3 00067 DigitalIn user_switch3(PA_3); 00068 //スイッチ4 00069 //以下の文を入れるとロリコンが読めなくなる 00070 //DigitalIn user_switch4(PA_2); 00071 //スイッチ5 00072 DigitalIn user_switch5(PA_10); 00073 00074 //フォトインタラプタ 00075 DigitalIn photo_interrupter(PA_15); 00076 00077 //12V停止信号ピン 00078 DigitalOut emergency(PA_13); 00079 00080 //赤ゾーンLED 00081 DigitalOut blue_side(PC_0); 00082 //青ゾーンLED 00083 DigitalOut red_side(PC_3); 00084 //スタートLED 00085 DigitalOut start_LED(PA_8); 00086 //LED1 00087 DigitalOut myled1(PC_6); 00088 //LED2 00089 DigitalOut myled2(PC_5); 00090 //LED3 00091 DigitalOut myled3(PA_12); 00092 //LED4 00093 DigitalOut myled4(PB_1); 00094 //LED5 00095 DigitalOut myled5(PA_5); 00096 00097 //前ローラー 00098 QEI front_roller_wheel(PA_0, PA_1, NC, 624); 00099 //後ローラー 00100 QEI back_roller_wheel(PB_5, PB_4, NC, 624); 00101 //計測オムニ1 00102 //QEI measure1_wheel(PC_9, PC_8, NC, 624); 00103 //計測オムニ2(使用不可) 00104 //QEI measure2_wheel(PB_3, PB_10, NC, 624); 00105 //右前オムニ 00106 QEI migimae_wheel(PB_6 , PA_7 , NC, 624); 00107 //右後オムニ 00108 QEI migiusiro_wheel(PA_11, PB_12, NC, 624); 00109 //左前オムニ 00110 QEI hidarimae_wheel(PB_2, PB_15, NC, 624); 00111 //左後オムニ 00112 QEI hidariusiro_wheel(PB_14, PB_13, NC, 624); 00113 00114 Ticker get_rps; 00115 Ticker get_distance; 00116 Timer timer; 00117 00118 int roller_flag = 0; 00119 int loading_state = 0; 00120 00121 int migimae_rpm; 00122 int migiusiro_rpm; 00123 int hidarimae_rpm; 00124 int hidariusiro_rpm; 00125 int measure1_rpm; 00126 //int measure2_rpm; 00127 int front_roller_rpm; 00128 int back_roller_rpm; 00129 00130 int migimae_pulse; 00131 int migiusiro_pulse; 00132 int hidarimae_pulse; 00133 int hidariusiro_pulse; 00134 int measure1_pulse; 00135 //int measure2_pulse; 00136 int front_roller_pulse; 00137 int back_roller_pulse; 00138 00139 int ave_migimae_pulse[10]; 00140 int ave_migiusiro_pulse[10]; 00141 int ave_hidarimae_pulse[10]; 00142 int ave_hidariusiro_pulse[10]; 00143 int ave_measure1_pulse[10]; 00144 //int ave_measure2_pulse[10]; 00145 int ave_front_roller_pulse[10]; 00146 int ave_back_roller_pulse[10]; 00147 00148 00149 int migimae_counter; 00150 int migiusiro_counter; 00151 int hidarimae_counter; 00152 int hidariusiro_counter; 00153 int measure1_counter; 00154 //int measure2_counter; 00155 int front_roller_counter; 00156 int back_roller_counter; 00157 00158 char send_data[1]; 00159 char true_send_data[1]; 00160 00161 char migimae_data[1]; 00162 char migiusiro_data[1]; 00163 char hidarimae_data[1]; 00164 char hidariusiro_data[1]; 00165 char front_roller_data[1]; 00166 char back_roller_data[1]; 00167 char loading_data[1]; 00168 char cylinder_data[1]; 00169 00170 char true_migimae_data[1]; 00171 char true_migiusiro_data[1]; 00172 char true_hidarimae_data[1]; 00173 char true_hidariusiro_data[1]; 00174 00175 int line_state = 0; 00176 00177 unsigned int distance; 00178 00179 /* ロリコン処理関数 */ 00180 void flip(); 00181 int front_PID(int RF, int RB, int LF, int LB); 00182 int back_PID(int RF, int RB, int LF, int LB); 00183 int rihgt_PID(int RF, int RB, int LF, int LB); 00184 int left_PID(int RF, int RB, int LF, int LB); 00185 int right_front_PID(int RB, int LF); 00186 int right_back_PID(int RF, int LB); 00187 int left_front_PID(int RF, int LB); 00188 int left_back_PID(int RB, int RF); 00189 int turn_right_PID(int RF, int RB, int LF, int LB); 00190 int turn_left_PID(int RF, int RB, int LF, int LB); 00191 int roller_PID(int front, int back); 00192 void linetrace(); 00193 void ultrasonic(); 00194 00195 void flip() { 00196 migimae_pulse = migimae_wheel.getPulses(); 00197 migiusiro_pulse = migiusiro_wheel.getPulses(); 00198 hidarimae_pulse = hidarimae_wheel.getPulses(); 00199 hidariusiro_pulse = hidariusiro_wheel.getPulses(); 00200 //measure1_pulse = measure1_wheel.getPulses(); 00201 //measure2_pulse = measure2_wheel.getPulses(); 00202 front_roller_pulse = front_roller_wheel.getPulses(); 00203 back_roller_pulse = back_roller_wheel.getPulses(); 00204 00205 //((40ms*25 = 1s)* 60 = 1min) / 分解能 00206 migimae_rpm = migimae_pulse * 25 * 60 / 1200; 00207 migiusiro_rpm = migiusiro_pulse * 25 * 60 / 1200; 00208 hidarimae_rpm = hidarimae_pulse * 25 * 60 / 1200; 00209 hidariusiro_rpm = hidariusiro_pulse * 25 * 60 / 1200; 00210 //measure1_rpm = measure1_pulse * 25 * 60 / 1200; 00211 //measure2_rpm = measure2_pulse * 25 * 60 / 1200; 00212 front_roller_rpm = front_roller_pulse * 25 * 60 / 1200; 00213 back_roller_rpm = back_roller_pulse * 25 * 60 / 1200; 00214 00215 //pc.printf("RF: %d RB: %d LF: %d LB: %d\n", migimae_rpm, migiusiro_rpm, hidarimae_rpm, hidariusiro_rpm); 00216 //pc.printf("前: %d, 後: %d, %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance); 00217 //pc.printf("%d\n", abs(back_roller_rpm)); 00218 //pc.printf("RF: %d, RB: %d\n", migimae_rpm, migiusiro_rpm); 00219 00220 migimae_wheel.reset(); 00221 migiusiro_wheel.reset(); 00222 hidarimae_wheel.reset(); 00223 hidariusiro_wheel.reset(); 00224 //measure1_wheel.reset(); 00225 //measure2_wheel.reset(); 00226 front_roller_wheel.reset(); 00227 back_roller_wheel.reset(); 00228 } 00229 int front_PID(int RF, int RB, int LF, int LB) { 00230 // センサ出力値の最小、最大 00231 migimae.setInputLimits(-400, 400); 00232 migiusiro.setInputLimits(-400, 400); 00233 hidarimae.setInputLimits(-400, 400); 00234 hidariusiro.setInputLimits(-400, 400); 00235 00236 // 制御量の最小、最大 00237 migimae.setOutputLimits(0x0C, 0x7C); 00238 migiusiro.setOutputLimits(0x0C, 0x7C); 00239 hidarimae.setOutputLimits(0x0C, 0x7C); 00240 hidariusiro.setOutputLimits(0x0C, 0x7C); 00241 00242 // よくわからんやつ 00243 migimae.setMode(AUTO_MODE); 00244 migiusiro.setMode(AUTO_MODE); 00245 hidarimae.setMode(AUTO_MODE); 00246 hidariusiro.setMode(AUTO_MODE); 00247 00248 // 目標値 00249 migimae.setSetPoint(RF); 00250 migiusiro.setSetPoint(RB); 00251 hidarimae.setSetPoint(LF); 00252 hidariusiro.setSetPoint(LB); 00253 00254 // センサ出力 00255 migimae.setProcessValue(abs(migimae_rpm)); 00256 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00257 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00258 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00259 00260 // 制御量(計算結果) 00261 migimae_data[0] = migimae.compute(); 00262 migiusiro_data[0] = migiusiro.compute(); 00263 hidarimae_data[0] = hidarimae.compute(); 00264 hidariusiro_data[0] = hidariusiro.compute(); 00265 00266 // 制御量をPWM値に変換 00267 true_migimae_data[0] = 0x7D - migimae_data[0]; 00268 true_migiusiro_data[0] = 0x7D - migiusiro_data[0]; 00269 true_hidarimae_data[0] = 0x7D - hidarimae_data[0]; 00270 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0]; 00271 00272 return 0; 00273 } 00274 int back_PID(int RF, int RB, int LF, int LB) { 00275 // センサ出力値の最小、最大 00276 migimae.setInputLimits(-400, 400); 00277 migiusiro.setInputLimits(-400, 400); 00278 hidarimae.setInputLimits(-400, 400); 00279 hidariusiro.setInputLimits(-400, 400); 00280 00281 // 制御量の最小、最大 00282 migimae.setOutputLimits(0x84, 0xFF); 00283 migiusiro.setOutputLimits(0x84, 0xFF); 00284 hidarimae.setOutputLimits(0x84, 0xFF); 00285 hidariusiro.setOutputLimits(0x84, 0xFF); 00286 00287 // よくわからんやつ 00288 migimae.setMode(AUTO_MODE); 00289 migiusiro.setMode(AUTO_MODE); 00290 hidarimae.setMode(AUTO_MODE); 00291 hidariusiro.setMode(AUTO_MODE); 00292 00293 // 目標値 00294 migimae.setSetPoint(RF); 00295 migiusiro.setSetPoint(RB); 00296 hidarimae.setSetPoint(LF); 00297 hidariusiro.setSetPoint(LB); 00298 00299 // センサ出力 00300 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。 00301 migimae.setProcessValue(abs(migimae_rpm)); 00302 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00303 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00304 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00305 00306 // 制御量(計算結果) 00307 migimae_data[0] = migimae.compute(); 00308 migiusiro_data[0] = migiusiro.compute(); 00309 hidarimae_data[0] = hidarimae.compute(); 00310 hidariusiro_data[0] = hidariusiro.compute(); 00311 00312 true_migimae_data[0] = migimae_data[0]; 00313 true_migiusiro_data[0] = migiusiro_data[0]; 00314 true_hidarimae_data[0] = hidarimae_data[0]; 00315 true_hidariusiro_data[0] = hidariusiro_data[0]; 00316 00317 return 0; 00318 } 00319 int right_PID(int RF, int RB, int LF, int LB) { 00320 // センサ出力値の最小、最大 00321 migimae.setInputLimits(-400, 400); 00322 migiusiro.setInputLimits(-400, 400); 00323 hidarimae.setInputLimits(-400, 400); 00324 hidariusiro.setInputLimits(-400, 400); 00325 00326 // 制御量の最小、最大 00327 migimae.setOutputLimits(0x84, 0xFF); 00328 migiusiro.setOutputLimits(0x0C, 0x7C); 00329 hidarimae.setOutputLimits(0x0C, 0x7C); 00330 hidariusiro.setOutputLimits(0x84, 0xFF); 00331 00332 // よくわからんやつ 00333 migimae.setMode(AUTO_MODE); 00334 migiusiro.setMode(AUTO_MODE); 00335 hidarimae.setMode(AUTO_MODE); 00336 hidariusiro.setMode(AUTO_MODE); 00337 00338 // 目標値 00339 migimae.setSetPoint(RF); 00340 migiusiro.setSetPoint(RB); 00341 hidarimae.setSetPoint(LF); 00342 hidariusiro.setSetPoint(LB); 00343 00344 // センサ出力 00345 migimae.setProcessValue(abs(migimae_rpm)); 00346 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00347 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00348 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00349 00350 // 制御量(計算結果) 00351 migimae_data[0] = migimae.compute(); 00352 migiusiro_data[0] = migiusiro.compute(); 00353 hidarimae_data[0] = hidarimae.compute(); 00354 hidariusiro_data[0] = hidariusiro.compute(); 00355 00356 // 制御量をPWM値に変換 00357 true_migimae_data[0] = migimae_data[0]; 00358 true_migiusiro_data[0] = 0x7D - migiusiro_data[0]; 00359 true_hidarimae_data[0] = 0x7D - hidarimae_data[0]; 00360 true_hidariusiro_data[0] = hidariusiro_data[0]; 00361 00362 return 0; 00363 } 00364 int left_PID(int RF, int RB, int LF, int LB) { 00365 // センサ出力値の最小、最大 00366 migimae.setInputLimits(-400, 400); 00367 migiusiro.setInputLimits(-400, 400); 00368 hidarimae.setInputLimits(-400, 400); 00369 hidariusiro.setInputLimits(-400, 400); 00370 00371 // 制御量の最小、最大 00372 migimae.setOutputLimits(0x0C, 0x7C); 00373 migiusiro.setOutputLimits(0x84, 0xFF); 00374 hidarimae.setOutputLimits(0x84, 0xFF); 00375 hidariusiro.setOutputLimits(0x0C, 0x7C); 00376 00377 // よくわからんやつ 00378 migimae.setMode(AUTO_MODE); 00379 migiusiro.setMode(AUTO_MODE); 00380 hidarimae.setMode(AUTO_MODE); 00381 hidariusiro.setMode(AUTO_MODE); 00382 00383 // 目標値 00384 migimae.setSetPoint(RF); 00385 migiusiro.setSetPoint(RB); 00386 hidarimae.setSetPoint(LF); 00387 hidariusiro.setSetPoint(LB); 00388 00389 // センサ出力 00390 migimae.setProcessValue(abs(migimae_rpm)); 00391 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00392 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00393 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00394 00395 // 制御量(計算結果) 00396 migimae_data[0] = migimae.compute(); 00397 migiusiro_data[0] = migiusiro.compute(); 00398 hidarimae_data[0] = hidarimae.compute(); 00399 hidariusiro_data[0] = hidariusiro.compute(); 00400 00401 // 制御量をPWM値に変換 00402 true_migimae_data[0] = 0x7D - migimae_data[0]; 00403 true_migiusiro_data[0] = migiusiro_data[0]; 00404 true_hidarimae_data[0] = hidarimae_data[0]; 00405 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0]; 00406 00407 return 0; 00408 } 00409 //斜め右前 00410 int right_front_PID(int RB, int LF) { 00411 // センサ出力値の最小、最大 00412 migiusiro.setInputLimits(-400, 400); 00413 hidarimae.setInputLimits(-400, 400); 00414 00415 // 制御量の最小、最大 00416 migiusiro.setOutputLimits(0x0C, 0x7C); 00417 hidarimae.setOutputLimits(0x0C, 0x7C); 00418 00419 // よくわからんやつ 00420 migiusiro.setMode(AUTO_MODE); 00421 hidarimae.setMode(AUTO_MODE); 00422 00423 // 目標値 00424 migiusiro.setSetPoint(RB); 00425 hidarimae.setSetPoint(LF); 00426 00427 // センサ出力 00428 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00429 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00430 00431 // 制御量(計算結果) 00432 migiusiro_data[0] = migiusiro.compute(); 00433 hidarimae_data[0] = hidarimae.compute(); 00434 00435 // 制御量をPWM値に変換 00436 true_migiusiro_data[0] = 0x7D - migiusiro_data[0]; 00437 true_hidarimae_data[0] = 0x7D - hidarimae_data[0]; 00438 00439 return 0; 00440 } 00441 //斜め右後 00442 int right_back_PID(int RF, int LB) { 00443 // センサ出力値の最小、最大 00444 migimae.setInputLimits(-400, 400); 00445 hidariusiro.setInputLimits(-400, 400); 00446 00447 // 制御量の最小、最大 00448 migimae.setOutputLimits(0x84, 0xFF); 00449 hidariusiro.setOutputLimits(0x84, 0xFF); 00450 00451 // よくわからんやつ 00452 migimae.setMode(AUTO_MODE); 00453 hidariusiro.setMode(AUTO_MODE); 00454 00455 // 目標値 00456 migimae.setSetPoint(RF); 00457 hidariusiro.setSetPoint(LB); 00458 00459 // センサ出力 00460 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。 00461 migimae.setProcessValue(abs(migimae_rpm)); 00462 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00463 00464 // 制御量(計算結果) 00465 migimae_data[0] = migimae.compute(); 00466 hidariusiro_data[0] = hidariusiro.compute(); 00467 00468 true_migimae_data[0] = migimae_data[0]; 00469 true_hidariusiro_data[0] = hidariusiro_data[0]; 00470 00471 return 0; 00472 } 00473 //斜め左前 00474 int left_front_PID(int RF, int LB) { 00475 // センサ出力値の最小、最大 00476 migimae.setInputLimits(-400, 400); 00477 hidariusiro.setInputLimits(-400, 400); 00478 00479 // 制御量の最小、最大 00480 migimae.setOutputLimits(0x0C, 0x7C); 00481 hidariusiro.setOutputLimits(0x0C, 0x7C); 00482 00483 // よくわからんやつ 00484 migimae.setMode(AUTO_MODE); 00485 hidariusiro.setMode(AUTO_MODE); 00486 00487 // 目標値 00488 migimae.setSetPoint(RF); 00489 hidariusiro.setSetPoint(LB); 00490 00491 // センサ出力 00492 migimae.setProcessValue(abs(migimae_rpm)); 00493 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00494 00495 // 制御量(計算結果) 00496 migimae_data[0] = migimae.compute(); 00497 hidariusiro_data[0] = hidariusiro.compute(); 00498 00499 // 制御量をPWM値に変換 00500 true_migimae_data[0] = 0x7D - migimae_data[0]; 00501 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0]; 00502 00503 return 0; 00504 } 00505 //斜め左後 00506 int left_back_PID(int RB, int LF) { 00507 // センサ出力値の最小、最大 00508 migiusiro.setInputLimits(-400, 400); 00509 hidarimae.setInputLimits(-400, 400); 00510 00511 // 制御量の最小、最大 00512 migiusiro.setOutputLimits(0x84, 0xFF); 00513 hidarimae.setOutputLimits(0x84, 0xFF); 00514 00515 // よくわからんやつ 00516 migiusiro.setMode(AUTO_MODE); 00517 hidarimae.setMode(AUTO_MODE); 00518 00519 // 目標値 00520 migiusiro.setSetPoint(RB); 00521 hidarimae.setSetPoint(LF); 00522 00523 // センサ出力 00524 //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。 00525 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00526 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00527 00528 // 制御量(計算結果) 00529 migiusiro_data[0] = migiusiro.compute(); 00530 hidarimae_data[0] = hidarimae.compute(); 00531 00532 true_migiusiro_data[0] = migiusiro_data[0]; 00533 true_hidarimae_data[0] = hidarimae_data[0]; 00534 00535 return 0; 00536 } 00537 int turn_right_PID(int RF, int RB, int LF, int LB) { 00538 // センサ出力値の最小、最大 00539 migimae.setInputLimits(-400, 400); 00540 migiusiro.setInputLimits(-400, 400); 00541 hidarimae.setInputLimits(-400, 400); 00542 hidariusiro.setInputLimits(-400, 400); 00543 00544 // 制御量の最小、最大 00545 migimae.setOutputLimits(0x84, 0xFF); 00546 migiusiro.setOutputLimits(0x84, 0xFF); 00547 hidarimae.setOutputLimits(0x0C, 0x7C); 00548 hidariusiro.setOutputLimits(0x0C, 0x7C); 00549 00550 // よくわからんやつ 00551 migimae.setMode(AUTO_MODE); 00552 migiusiro.setMode(AUTO_MODE); 00553 hidarimae.setMode(AUTO_MODE); 00554 hidariusiro.setMode(AUTO_MODE); 00555 00556 // 目標値 00557 migimae.setSetPoint(RF); 00558 migiusiro.setSetPoint(RB); 00559 hidarimae.setSetPoint(LF); 00560 hidariusiro.setSetPoint(LB); 00561 00562 // センサ出力 00563 migimae.setProcessValue(abs(migimae_rpm)); 00564 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00565 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00566 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00567 00568 // 制御量(計算結果) 00569 migimae_data[0] = migimae.compute(); 00570 migiusiro_data[0] = migiusiro.compute(); 00571 hidarimae_data[0] = hidarimae.compute(); 00572 hidariusiro_data[0] = hidariusiro.compute(); 00573 00574 // 制御量をPWM値に変換 00575 true_migimae_data[0] = migimae_data[0]; 00576 true_migiusiro_data[0] = migiusiro_data[0]; 00577 true_hidarimae_data[0] = 0x7D - hidarimae_data[0]; 00578 true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0]; 00579 00580 return 0; 00581 } 00582 int turn_left_PID(int RF, int RB, int LF, int LB) { 00583 // センサ出力値の最小、最大 00584 migimae.setInputLimits(-400, 400); 00585 migiusiro.setInputLimits(-400, 400); 00586 hidarimae.setInputLimits(-400, 400); 00587 hidariusiro.setInputLimits(-400, 400); 00588 00589 // 制御量の最小、最大 00590 migimae.setOutputLimits(0x0C, 0x7C); 00591 migiusiro.setOutputLimits(0x0C, 0x7C); 00592 hidarimae.setOutputLimits(0x84, 0xFF); 00593 hidariusiro.setOutputLimits(0x84, 0xFF); 00594 00595 // よくわからんやつ 00596 migimae.setMode(AUTO_MODE); 00597 migiusiro.setMode(AUTO_MODE); 00598 hidarimae.setMode(AUTO_MODE); 00599 hidariusiro.setMode(AUTO_MODE); 00600 00601 // 目標値 00602 migimae.setSetPoint(RF); 00603 migiusiro.setSetPoint(RB); 00604 hidarimae.setSetPoint(LF); 00605 hidariusiro.setSetPoint(LB); 00606 00607 // センサ出力 00608 migimae.setProcessValue(abs(migimae_rpm)); 00609 migiusiro.setProcessValue(abs(migiusiro_rpm)); 00610 hidarimae.setProcessValue(abs(hidarimae_rpm)); 00611 hidariusiro.setProcessValue(abs(hidariusiro_rpm)); 00612 00613 // 制御量(計算結果) 00614 migimae_data[0] = migimae.compute(); 00615 migiusiro_data[0] = migiusiro.compute(); 00616 hidarimae_data[0] = hidarimae.compute(); 00617 hidariusiro_data[0] = hidariusiro.compute(); 00618 00619 // 制御量をPWM値に変換 00620 true_migimae_data[0] = 0x7D - migimae_data[0]; 00621 true_migiusiro_data[0] = 0x7D - migiusiro_data[0]; 00622 true_hidarimae_data[0] = hidarimae_data[0]; 00623 true_hidariusiro_data[0] = hidariusiro_data[0]; 00624 00625 return 0; 00626 } 00627 int roller_PID(int front, int back) { 00628 front_roller.setInputLimits(-2000, 2000); 00629 back_roller.setInputLimits(-2000, 2000); 00630 00631 front_roller.setOutputLimits(0x84, 0xFF); 00632 back_roller.setOutputLimits(0x84, 0xFF); 00633 00634 front_roller.setMode(AUTO_MODE); 00635 back_roller.setMode(AUTO_MODE); 00636 00637 front_roller.setSetPoint(front); 00638 back_roller.setSetPoint(back); 00639 00640 front_roller.setProcessValue(abs(front_roller_rpm)); 00641 back_roller.setProcessValue(abs(back_roller_rpm)); 00642 00643 front_roller_data[0] = front_roller.compute(); 00644 back_roller_data[0] = back_roller.compute(); 00645 00646 return 0; 00647 } 00648 void linetrace() { 00649 line_state = photo.getc(); 00650 } 00651 void ultrasonic() { 00652 sonic.start(); 00653 wait(0.01); 00654 //get_dist_cm関数は、計測から得られた距離(cm)を返します。 00655 distance = sonic.get_dist_cm(); 00656 //pc.printf("%d[cm]\r\n", distance); 00657 } 00658 int main(void) { 00659 pc.printf("HelloWorld\n"); 00660 emergency = 0; 00661 /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */ 00662 get_rps.attach_us(&flip, 40000); 00663 //get_distance.attach_us(&ultrasonic, 100000); 00664 photo.baud(115200); 00665 photo.attach(linetrace, Serial::RxIrq); 00666 side.mode(PullDown); 00667 00668 // setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。 00669 sonic.setTemp(25); 00670 00671 send_data[0] = 0x80; 00672 i2c.write(0x10, send_data, 1); 00673 i2c.write(0x12, send_data, 1); 00674 i2c.write(0x14, send_data, 1); 00675 i2c.write(0x16, send_data, 1); 00676 i2c.write(0x20, send_data, 1); 00677 i2c.write(0x22, send_data, 1); 00678 i2c.write(0x30, send_data, 1); 00679 i2c.write(0x40, send_data, 1); 00680 wait(0.1); 00681 00682 while(1) { 00683 ultrasonic(); 00684 //pc.printf("%d[cm]\r\n", distance); 00685 if(side == 1){ 00686 red_side = 1; 00687 blue_side = 0; 00688 } else { 00689 red_side = 0; 00690 blue_side = 1; 00691 } 00692 if(start_button == 1){ 00693 /* 00694 true_migimae_data[0] = 0x80; 00695 true_migiusiro_data[0] = 0x80; 00696 true_hidarimae_data[0] = 0x80; 00697 true_hidariusiro_data[0] = 0x80; 00698 i2c.write(0x10, true_migimae_data, 1, false); 00699 i2c.write(0x12, true_migiusiro_data, 1, false); 00700 i2c.write(0x14, true_hidarimae_data, 1, false); 00701 i2c.write(0x16, true_hidariusiro_data, 1, false); 00702 wait_us(20); 00703 */ 00704 cylinder_data[0] = 0x80; 00705 myled1 = 0; 00706 i2c.write(0x40, cylinder_data, 1); 00707 } else { 00708 /* 00709 front_PID(270, 270, 270, 270); 00710 i2c.write(0x10, true_migimae_data, 1, false); 00711 i2c.write(0x12, true_migiusiro_data, 1, false); 00712 i2c.write(0x14, true_hidarimae_data, 1, false); 00713 i2c.write(0x16, true_hidariusiro_data, 1, false); 00714 wait_us(20); 00715 */ 00716 myled1 = 1; 00717 cylinder_data[0] = 0x0F; 00718 i2c.write(0x40, cylinder_data, 1); 00719 } 00720 if(user_switch2 == 0 && user_switch3 == 1){ 00721 loading_data[0] = 0x0C; 00722 myled2 = 1; 00723 i2c.write(0x30, loading_data, 1); 00724 } 00725 else if(user_switch3 == 0 && user_switch2 == 1){ 00726 loading_data[0] = 0xFF; 00727 myled3 = 1; 00728 i2c.write(0x30, loading_data, 1); 00729 } else { 00730 loading_data[0] = 0x80; 00731 myled2 = 0; 00732 myled3 = 0; 00733 i2c.write(0x30, loading_data, 1); 00734 } 00735 00736 /* 00737 loading_data[0] = 0x0C; 00738 i2c.write(0x30, loading_data, 1); 00739 if(photo_interrupter.read() == 1) { 00740 myled4 = 0; 00741 loading_state = 1; 00742 loading_data[0] = 0x80; 00743 i2c.write(0x30, loading_data, 1); 00744 } 00745 */ 00746 00747 /* 00748 if(user_switch5 == 0){ 00749 roller_flag = 1; 00750 } 00751 if(roller_flag == 1) { 00752 myled5 = 1; 00753 timer.reset(); 00754 timer.start(); 00755 roller_PID(325, 650); 00756 i2c.write(0x20, front_roller_data, 1, false); 00757 i2c.write(0x22, back_roller_data, 1, false); 00758 wait_us(20); 00759 00760 if((timer.read() >= 5.0f) && (timer.read() < 5.5f)) { 00761 cylinder_data[0] = 0x0F; 00762 i2c.write(0x40, cylinder_data, 1); 00763 } 00764 else if((timer.read() >= 5.5f) && (timer.read() < 6.0f)) { 00765 cylinder_data[0] = 0x80; 00766 i2c.write(0x40, cylinder_data, 1); 00767 } 00768 else if((timer.read() >= 6.0f) && (timer.read() < 6.5f)) { 00769 cylinder_data[0] = 0x0F; 00770 i2c.write(0x40, cylinder_data, 1); 00771 } 00772 else if((timer.read() >= 6.5f) && (timer.read() < 7.0f)) { 00773 cylinder_data[0] = 0x80; 00774 i2c.write(0x40, cylinder_data, 1); 00775 } 00776 else if((timer.read() >= 7.0f) && (timer.read() < 7.5f)) { 00777 cylinder_data[0] = 0x0F; 00778 i2c.write(0x40, cylinder_data, 1); 00779 } else { 00780 cylinder_data[0] = 0x80; 00781 i2c.write(0x40, cylinder_data, 1); 00782 } 00783 timer.stop(); 00784 } else { 00785 myled5 = 0; 00786 front_roller_data[0] = 0x80; 00787 back_roller_data[0] = 0x80; 00788 i2c.write(0x20, front_roller_data, 1, false); 00789 i2c.write(0x22, back_roller_data, 1, false); 00790 } 00791 */ 00792 00793 /* 00794 if(distance < 30 && distance != 0) { 00795 //front_PID(200, 200, 200, 200); 00796 //i2c.write(0x10, true_migimae_data, 1, false); 00797 //i2c.write(0x12, true_migiusiro_data, 1, false); 00798 //i2c.write(0x14, true_hidarimae_data, 1, false); 00799 //i2c.write(0x16, true_hidariusiro_data, 1, false); 00800 } else { 00801 //send_data[0] = 0x80; 00802 //i2c.write(0x10, send_data, 1, false); 00803 //i2c.write(0x12, send_data, 1, false); 00804 //i2c.write(0x14, send_data, 1, false); 00805 //i2c.write(0x16, send_data, 1, false); 00806 } 00807 */ 00808 00809 //pc.printf("%d\n", line_state); 00810 if(line_state == 0){ 00811 pc.printf("真ん中\n"); 00812 front_PID(70, 70, 70, 70); 00813 i2c.write(0x10, true_migimae_data, 1, false); 00814 i2c.write(0x12, true_migiusiro_data, 1, false); 00815 i2c.write(0x14, true_hidarimae_data, 1, false); 00816 i2c.write(0x16, true_hidariusiro_data, 1, false); 00817 } 00818 else if(line_state == 1) { 00819 pc.printf("右\n"); 00820 turn_right_PID(30, 30, 30, 30); 00821 //right_front_PID(50, 50); 00822 i2c.write(0x10, true_migimae_data, 1, false); 00823 i2c.write(0x12, true_migiusiro_data, 1, false); 00824 i2c.write(0x14, true_hidarimae_data, 1, false); 00825 i2c.write(0x16, true_hidariusiro_data, 1, false); 00826 } 00827 else if(line_state == 2) { 00828 pc.printf("左\n"); 00829 turn_left_PID(30, 30, 30, 30); 00830 //left_front_PID(50, 50); 00831 i2c.write(0x10, true_migimae_data, 1, false); 00832 i2c.write(0x12, true_migiusiro_data, 1, false); 00833 i2c.write(0x14, true_hidarimae_data, 1, false); 00834 i2c.write(0x16, true_hidariusiro_data, 1, false); 00835 } 00836 else if(line_state == 3) { 00837 pc.printf("範囲外\n"); 00838 send_data[0] = 0x80; 00839 i2c.write(0x10, send_data, 1, false); 00840 i2c.write(0x12, send_data, 1, false); 00841 i2c.write(0x14, send_data, 1, false); 00842 i2c.write(0x16, send_data, 1, false); 00843 } 00844 00845 /* 00846 front_PID(270, 270, 270, 270); 00847 i2c.write(0x10, true_migimae_data, 1, false); 00848 i2c.write(0x12, true_migiusiro_data, 1, false); 00849 i2c.write(0x14, true_hidarimae_data, 1, false); 00850 i2c.write(0x16, true_hidariusiro_data, 1, false); 00851 wait_us(20); 00852 00853 back_PID(270, 270, 270, 270); 00854 i2c.write(0x10, true_migimae_data, 1, false); 00855 i2c.write(0x12, true_migiusiro_data, 1, false); 00856 i2c.write(0x14, true_hidarimae_data, 1, false); 00857 i2c.write(0x16, true_hidariusiro_data,1, false); 00858 wait_us(20); 00859 00860 right_PID(100, 100, 100, 100); 00861 i2c.write(0x10, true_migimae_data, 1, false); 00862 i2c.write(0x12, true_migiusiro_data, 1, false); 00863 i2c.write(0x14, true_hidarimae_data, 1, false); 00864 i2c.write(0x16, true_hidariusiro_data,1, false); 00865 wait_us(20); 00866 00867 left_PID(100, 100, 100, 100); 00868 i2c.write(0x10, true_migimae_data, 1, false); 00869 i2c.write(0x12, true_migiusiro_data, 1, false); 00870 i2c.write(0x14, true_hidarimae_data, 1, false); 00871 i2c.write(0x16, true_hidariusiro_data,1, false); 00872 wait_us(20); 00873 00874 //斜め右前 00875 right_front_PID(270, 270); 00876 true_migiusiro_data[0] = 0x80; 00877 true_hidariusiro_data[0] = 0x80; 00878 i2c.write(0x10, true_migimae_data, 1, false); 00879 i2c.write(0x12, true_migiusiro_data, 1, false); 00880 i2c.write(0x14, true_hidarimae_data, 1, false); 00881 i2c.write(0x16, true_hidariusiro_data,1, false); 00882 wait_us(20); 00883 00884 //斜め右後 00885 right_back_PID(270, 270); 00886 true_migimae_data[0] = 0x80; 00887 true_hidarimae_data[0] = 0x80; 00888 i2c.write(0x10, true_migimae_data, 1, false); 00889 i2c.write(0x12, true_migiusiro_data, 1, false); 00890 i2c.write(0x14, true_hidarimae_data, 1, false); 00891 i2c.write(0x16, true_hidariusiro_data,1, false); 00892 wait_us(20); 00893 00894 //斜め左前 00895 left_front_PID(270, 270); 00896 true_migimae_data[0] = 0x80; 00897 true_hidarimae_data[0] = 0x80; 00898 i2c.write(0x10, true_migimae_data, 1, false); 00899 i2c.write(0x12, true_migiusiro_data, 1, false); 00900 i2c.write(0x14, true_hidarimae_data, 1, false); 00901 i2c.write(0x16, true_hidariusiro_data,1, false); 00902 wait_us(20); 00903 00904 //斜め左後 00905 left_back_PID(270, 270); 00906 true_migiusiro_data[0] = 0x80; 00907 true_hidariusiro_data[0] = 0x80; 00908 i2c.write(0x10, true_migimae_data, 1, false); 00909 i2c.write(0x12, true_migiusiro_data, 1, false); 00910 i2c.write(0x14, true_hidarimae_data, 1, false); 00911 i2c.write(0x16, true_hidariusiro_data,1, false); 00912 wait_us(20); 00913 00914 turn_right_PID(200, 200, 200, 200); 00915 i2c.write(0x10, true_migimae_data, 1, false); 00916 i2c.write(0x12, true_migiusiro_data, 1, false); 00917 i2c.write(0x14, true_hidarimae_data, 1, false); 00918 i2c.write(0x16, true_hidariusiro_data,1, false); 00919 wait_us(20); 00920 00921 turn_left_PID(100, 100, 100, 100); 00922 i2c.write(0x10, true_migimae_data, 1, false); 00923 i2c.write(0x12, true_migiusiro_data, 1, false); 00924 i2c.write(0x14, true_hidarimae_data, 1, false); 00925 i2c.write(0x16, true_hidariusiro_data,1, false); 00926 wait_us(20); 00927 00928 roller_PID(1000, 1000); 00929 i2c.write(0x20, front_roller_data, 1, false); 00930 i2c.write(0x22, back_roller_data, 1, false); 00931 wait_us(20); 00932 */ 00933 00934 /* 00935 //どんどん加速(逆転) 00936 for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){ 00937 //ice(0x88, send_data); 00938 //ice(0xA2, send_data); 00939 00940 i2c.write(0xA0, send_data, 1); 00941 i2c.write(0xA2, send_data, 1); 00942 i2c.write(0xA4, send_data, 1); 00943 i2c.write(0xA6, send_data, 1); 00944 00945 i2c.write(0x20, send_data, 1); 00946 wait(0.1); 00947 } 00948 //だんだん減速(逆転) 00949 for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){ 00950 //ice(0x88, send_data); 00951 //ice(0xA2, send_data); 00952 00953 i2c.write(0xA0, send_data, 1); 00954 i2c.write(0xA2, send_data, 1); 00955 i2c.write(0xA4, send_data, 1); 00956 i2c.write(0xA6, send_data, 1); 00957 00958 i2c.write(0x20, send_data, 1); 00959 wait(0.1); 00960 } 00961 send_data[0] = 0x80; 00962 i2c.write(0x20, send_data, 1); 00963 wait(5); 00964 */ 00965 } 00966 }
Generated on Wed Jul 20 2022 00:51:17 by
1.7.2