mbed_robotcar
ロボットカー
Dependencies: RemoteIR TextLCD
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* mbed Microcontroller Library 00002 * Copyright (c) 2019 ARM Limited 00003 * SPDX-License-Identifier: Apache-2.0 00004 */ 00005 00006 #include "mbed.h" 00007 #include "ReceiverIR.h" 00008 #include "rtos.h" 00009 #include <stdint.h> 00010 #include "platform/mbed_thread.h" 00011 #include "TextLCD.h" 00012 00013 Serial pc(USBTX, USBRX); 00014 00015 /* マクロ定義、列挙型定義 */ 00016 #define MIN_V 2.0 00017 #define MAX_V 2.67 00018 #define LOW 0 // 00019 #define HIGH 1 // 00020 #define NORMAL 0 // フラグ値 00021 #define FAST 1 // フラグ値 00022 #define VERYFAST 2 // フラグ値 00023 00024 enum MODE{ 00025 READY = -1, 00026 ADVANCE = 1, 00027 RIGHT, 00028 LEFT, 00029 BACK, 00030 STOP, 00031 LINE_TRACE, 00032 AVOIDANCE, 00033 SPEED, 00034 }; 00035 00036 /* ピン配置 */ 00037 ReceiverIR ir(p5); // リモコン操作 00038 DigitalOut trig(p6); // 超音波センサtrigger 00039 DigitalIn echo(p7); // 超音波センサecho 00040 DigitalIn ss1(p8); // ライントレースセンサ(左) 00041 DigitalIn ss2(p9); // ライントレースセンサ 00042 DigitalIn ss3(p10); // ライントレースセンサ 00043 DigitalIn ss4(p11); // ライントレースセンサ 00044 DigitalIn ss5(p12); // ライントレースセンサ(右) 00045 Serial esp(p13, p14); // Wi-Fiモジュール(tx, rx) 00046 AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V) 00047 PwmOut motorR2(p21); // 右モーター後退 00048 PwmOut motorR1(p22); // 右モーター前進 00049 PwmOut motorL2(p23); // 左モーター後退 00050 PwmOut motorL1(p24); // 左モーター前進 00051 PwmOut servo(p25); // サーボ 00052 I2C i2c_lcd(p28,p27); // LCD(tx, rx) 00053 00054 /* 変数宣言 */ 00055 int mode; // 操作モード 00056 int run; // 走行状態 00057 int beforeMode; // 前のモード 00058 int flag_sp = 0; // スピード変化フラグ 00059 Timer viewTimer; // スピ―ド変更時に3秒計測 00060 float motorSpeed[9] = {0.7, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0}; 00061 // モーター速度設定(後半はライントレース用) 00062 Mutex mutex; 00063 // ポートp15を赤外線受信モジュールの接続先に指定 00064 RemoteIR::Format format; 00065 uint8_t buf[32]; 00066 uint32_t bitcount; 00067 uint32_t code; 00068 00069 //bChangeのいろいろ 00070 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780); 00071 int b = 0; 00072 double test; 00073 int flag_b = 0; 00074 int flag_t = 0; 00075 00076 // ライントレース 00077 int sensArray[32] = {1,6,2,4,1,1,2,1, // ライントレースセンサパターン 00078 3,1,1,1,3,1,1,1, 00079 7,1,1,1,1,1,1,1, 00080 5,1,1,1,1,1,1,1}; 00081 00082 /* 障害物検知用の変数設定 */ 00083 Timer timer; // 距離計測用タイマ 00084 int DT; // 距離 00085 int SC; // 正面 00086 int SL; // 左 00087 int SR; // 右 00088 int SLD; // 左前 00089 int SRD; // 右前 00090 int flag_a = 0; // 障害物有無のフラグ 00091 const int limit = 20; // 障害物の距離のリミット(単位：cm) 00092 int far; // 最も遠い距離 00093 int houkou; // 進行方向(1:前 2:左 3:右) 00094 int i; // ループ変数 00095 00096 00097 00098 /* プロトタイプ宣言 */ 00099 void decodeIR(void const *argument); 00100 void motor(void const *argument); 00101 void changeSpeed(); 00102 void avoidance(void const *argument); 00103 void trace(void const *argument); 00104 void watchsurrounding(); 00105 int watch(); 00106 void bChange(); 00107 void display(); 00108 void lcdBacklight(void const *argument); 00109 Thread thread1(decodeIR, NULL, osPriorityRealtime);//+3 00110 Thread thread2(motor, NULL, osPriorityHigh);//+2 00111 Thread thread3(avoidance, NULL, osPriorityHigh);//+2 00112 Thread thread4(trace, NULL, osPriorityHigh);//+2 00113 RtosTimer bTimer(lcdBacklight, osTimerPeriodic); 00114 00115 DigitalOut led1(LED1); 00116 00117 00118 void decodeIR(void const *argument){ 00119 while(1){ 00120 // 受信待ち 00121 pc.printf("decodeIR\r\n"); 00122 if (ir.getState() == ReceiverIR::Received) { 00123 // コード受信 00124 bitcount = ir.getData(&format, buf, sizeof(buf) * 8); 00125 if(bitcount > 1){ 00126 // 受信成功 00127 code=0; 00128 for(int j=0;j<4;j++){ 00129 code+=(buf[j]<<(8*(3-j))); 00130 } 00131 if(mode!=SPEED){ 00132 beforeMode=mode; 00133 } 00134 00135 //pc.printf("%0x\r\n",code); 00136 switch(code){ 00137 case 0x40bf27d8://クイック 00138 pc.printf("mode = SPEED\r\n"); 00139 mode = SPEED; 00140 changeSpeed(); 00141 display(); 00142 mode = beforeMode; 00143 break; 00144 case 0x40be34cb://レグザリンク 00145 pc.printf("mode = LINE_TRACE\r\n"); 00146 mode=LINE_TRACE; 00147 display(); 00148 break; 00149 case 0x40bf6e91://番組表 00150 pc.printf("mode = AVOIDANCE\r\n"); 00151 mode=AVOIDANCE; 00152 display(); 00153 break; 00154 case 0x40bf3ec1://↑ 00155 pc.printf("mode = ADVANCE\r\n"); 00156 mode = ADVANCE; 00157 run = ADVANCE; 00158 display(); 00159 break; 00160 case 0x40bf3fc0://↓ 00161 pc.printf("mode = BACK\r\n"); 00162 mode = BACK; 00163 run = BACK; 00164 display(); 00165 break; 00166 case 0x40bf5fa0://← 00167 pc.printf("mode = LEFT\r\n"); 00168 mode = LEFT; 00169 run = LEFT; 00170 display(); 00171 break; 00172 case 0x40bf5ba4://→ 00173 pc.printf("mode = RIGHT\r\n"); 00174 mode = RIGHT; 00175 run = RIGHT; 00176 display(); 00177 break; 00178 case 0x40bf3dc2://決定 00179 pc.printf("mode = STOP\r\n"); 00180 mode = STOP; 00181 run = STOP; 00182 display(); 00183 break; 00184 default: 00185 ; 00186 } 00187 } 00188 } 00189 if(viewTimer.read_ms()>=3000){ 00190 viewTimer.stop(); 00191 viewTimer.reset(); 00192 display(); 00193 } 00194 ThisThread::sleep_for(90); 00195 } 00196 } 00197 void motor(void const *argument){ 00198 while(true){ 00199 pc.printf("motor\r\n"); 00200 switch(run){ 00201 case ADVANCE: 00202 motorR1 = motorSpeed[flag_sp]; 00203 motorR2 = LOW; 00204 motorL1 = motorSpeed[flag_sp]; 00205 motorL2 = LOW; 00206 break; 00207 case RIGHT: 00208 motorR1 = LOW; 00209 motorR2 = motorSpeed[flag_sp]; 00210 motorL1 = motorSpeed[flag_sp]; 00211 motorL2 = LOW; 00212 break; 00213 case LEFT: 00214 motorR1 = motorSpeed[flag_sp]; 00215 motorR2 = LOW; 00216 motorL1 = LOW; 00217 motorL2 = motorSpeed[flag_sp]; 00218 break; 00219 case BACK: 00220 motorR1 = LOW; 00221 motorR2 = motorSpeed[flag_sp]; 00222 motorL1 = LOW; 00223 motorL2 = motorSpeed[flag_sp]; 00224 break; 00225 case STOP: 00226 motorR1 = LOW; 00227 motorR2 = LOW; 00228 motorL1 = LOW; 00229 motorL2 = LOW; 00230 break; 00231 } 00232 if(flag_sp > VERYFAST){ 00233 flag_sp -= 3 * (flag_sp / 3); 00234 } 00235 pc.printf(" motor\r\n"); 00236 ThisThread::sleep_for(30); 00237 } 00238 } 00239 void changeSpeed(){ 00240 if(flag_sp%3 == 2){ 00241 flag_sp -= 2; 00242 00243 }else{ 00244 flag_sp = flag_sp + 1; 00245 } 00246 } 00247 void trace(void const *argument){ 00248 while(true){ 00249 if(mode==LINE_TRACE){ 00250 //pc.printf("line trace\r\n"); 00251 // センサー値読み取り 00252 int sensor1 = ss1; 00253 int sensor2 = ss2; 00254 int sensor3 = ss3; 00255 int sensor4 = ss4; 00256 int sensor5 = ss5; 00257 pc.printf("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5); 00258 int sensD = 0; 00259 int sensorNum; 00260 00261 if(sensor1 > 0) sensD |= 0x10; 00262 if(sensor2 > 0) sensD |= 0x08; 00263 if(sensor3 > 0) sensD |= 0x04; 00264 if(sensor4 > 0) sensD |= 0x02; 00265 if(sensor5 > 0) sensD |= 0x01; 00266 00267 sensorNum = sensArray[sensD]; 00268 00269 switch(sensorNum){ 00270 case 1: 00271 run = ADVANCE; 00272 pc.printf("ADVANCE"); 00273 break; 00274 case 2: 00275 run = RIGHT; 00276 pc.printf("RIGHT"); 00277 break; 00278 case 3: 00279 run = LEFT; 00280 pc.printf("LEFT"); 00281 break; 00282 case 4: 00283 flag_sp %= 3 + 3; 00284 run = RIGHT; 00285 pc.printf("RIGHT"); 00286 break; 00287 case 5: 00288 flag_sp %= 3 + 3; 00289 run = LEFT; 00290 pc.printf("LEFT"); 00291 break; 00292 case 6: 00293 flag_sp %= 3 + 6; 00294 run = RIGHT; 00295 pc.printf("RIGHT"); 00296 break; 00297 case 7: 00298 flag_sp %= 3 + 6; 00299 run = LEFT; 00300 pc.printf("LEFT"); 00301 break; 00302 } 00303 //pc.printf(" line trace\r\n"); 00304 ThisThread::sleep_for(30); 00305 }else{ 00306 ThisThread::sleep_for(1); 00307 } 00308 } 00309 } 00310 00311 /* 障害物回避走行 */ 00312 void avoidance(void const *argument){ 00313 while(1){ 00314 if(mode==AVOIDANCE){ 00315 pc.printf("avoidance\r\n"); 00316 pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); 00317 00318 if(flag_a == 0){ // 障害物がない場合 00319 run = ADVANCE; // 前進 00320 } 00321 else{ // 障害物がある場合 00322 i = 0; 00323 if(SC < 15){ // 正面15cm以内に障害物が現れた場合 00324 run = BACK; // 後退 00325 while(watch() < limit){ // 正面20cm以内に障害物がある間 00326 if(mode!=AVOIDANCE){ 00327 break; 00328 } 00329 } 00330 run = STOP; // 停止 00331 } 00332 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 00333 run = LEFT; // 左折 00334 while(i < 10){ // 進行方向確認 00335 if(mode!=AVOIDANCE){ 00336 break; 00337 } 00338 if(watch() > limit){ 00339 i++; 00340 00341 } 00342 else{ 00343 i = 0; 00344 } 00345 } 00346 run = STOP; // 停止 00347 } 00348 else { // 全方向以外 00349 far = SC; // 正面を最も遠い距離に設定 00350 houkou = 1; // 進行方向を前に設定 00351 if(far < SLD || far < SL){ // 左または左前がより遠い場合 00352 if(SL < SLD){ // 左前が左より遠い場合 00353 far = SLD; // 左前を最も遠い距離に設定 00354 } 00355 else{ // 左が左前より遠い場合 00356 far = SL; // 左を最も遠い距離に設定 00357 } 00358 houkou = 2; // 進行方向を左に設定 00359 } 00360 if(far < SRD || far < SR){ // 右または右前がより遠い場合 00361 if(SR < SRD){ // 右前が右より遠い場合 00362 far = SRD; // 右前を最も遠い距離に設定 00363 } 00364 else{ // 右が右前よりも遠い場合 00365 far = SR; // 右を最も遠い距離に設定 00366 } 00367 houkou = 3; // 進行方向を右に設定 00368 } 00369 switch(houkou){ // 進行方向の場合分け 00370 case 1: // 前の場合 00371 run = ADVANCE; // 前進 00372 pc.printf("Advance\r\n"); 00373 thread_sleep_for(100); // 1秒待つ 00374 break; 00375 case 2: // 左の場合 00376 run = LEFT; // 左折 00377 while(i < 10){ // 進行方向確認 00378 pc.printf("left\r\n"); 00379 if(mode!=AVOIDANCE){ 00380 break; 00381 } 00382 if(watch() > (far - 2)){ 00383 i++; 00384 00385 } 00386 else{ 00387 i = 0; 00388 } 00389 } 00390 run = STOP; // 停止 00391 break; 00392 case 3: // 右の場合 00393 run = RIGHT; // 右折 00394 while(i < 10){ // 進行方向確認 00395 pc.printf("right\r\n"); 00396 if(mode!=AVOIDANCE){ 00397 break; 00398 } 00399 if(watch() > (far - 2)){ 00400 i++; 00401 00402 } 00403 else{ 00404 i = 0; 00405 } 00406 } 00407 run = STOP; // 停止 00408 break; 00409 } 00410 } 00411 } 00412 pc.printf("こんにちは！\r\n"); 00413 flag_a = 0; // フラグを0にセット 00414 watchsurrounding(); 00415 pc.printf(" avoidance\r\n"); 00416 }else{ 00417 ThisThread::sleep_for(1); 00418 } 00419 } 00420 } 00421 int watch(){ 00422 00423 pc.printf("watch\r\n"); 00424 trig = 0; 00425 ThisThread::sleep_for(5); // 5ms待つ 00426 trig = 1; 00427 ThisThread::sleep_for(15); // 15ms待つ 00428 trig = 0; 00429 while(echo.read() == 0){ 00430 led1=1; 00431 if(mode!=AVOIDANCE){ 00432 break; 00433 } 00434 } 00435 timer.start(); // 距離計測タイマースタート 00436 while(echo.read() == 1){ 00437 } 00438 timer.stop(); // 距離計測タイマーストップ 00439 DT = (int)(timer.read_us()*0.01657); // 距離計算 00440 if(DT > 100){ // 検知範囲外なら400cmに設定 00441 DT = 100; 00442 } 00443 00444 timer.reset(); // 距離計測タイマーリセット 00445 //ThisThread::sleep_for(30); // 30ms待つ 00446 pc.printf("私はDTである。%d\r\n",DT); 00447 led1=0; 00448 return DT; 00449 } 00450 00451 void watchsurrounding(){ 00452 pc.printf("watchsurrounding\r\n"); 00453 SC = watch(); 00454 if(SC < limit){ // 正面20cm以内に障害物がある場合 00455 run = STOP; // 停止 00456 } 00457 servo.pulsewidth_us(1925); // サーボを左に40度回転 00458 ThisThread::sleep_for(250); // 250ms待つ 00459 SLD = watch(); 00460 if(SLD < limit){ // 左前20cm以内に障害物がある場合 00461 run = STOP; // 停止 00462 } 00463 servo.pulsewidth_us(2400); // サーボを左に90度回転 00464 ThisThread::sleep_for(250); // 250ms待つ 00465 SL = watch(); 00466 if(SL < limit){ // 左20cm以内に障害物がある場合 00467 run = STOP; // 停止 00468 } 00469 servo.pulsewidth_us(1450); 00470 ThisThread::sleep_for(100); 00471 servo.pulsewidth_us(925); // サーボを右に40度回転 00472 ThisThread::sleep_for(250); // 250ms待つ 00473 SRD = watch(); 00474 if(SRD < limit){ // 右前20cm以内に障害物がある場合 00475 run = STOP; // 停止 00476 } 00477 servo.pulsewidth_us(500); // サーボを右に90度回転 00478 ThisThread::sleep_for(250); // 250ms待つ 00479 SR = watch(); 00480 if(SR < limit){ // 右20cm以内に障害物がある場合 00481 run = STOP; // 停止 00482 } 00483 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00484 ThisThread::sleep_for(100); // 100ms待つ 00485 if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 00486 flag_a = 1; // フラグに1をセット 00487 } 00488 pc.printf(" watchsurrounding\r\n"); 00489 } 00490 00491 void defaultView(){ 00492 lcd.setAddress(0,0); 00493 lcd.printf("Battery:"); 00494 lcd.setAddress(0,1); 00495 lcd.printf("Mode:"); 00496 } 00497 00498 void display(){ 00499 mutex.lock(); 00500 lcd.setAddress(0,1); 00501 switch(mode){ 00502 case ADVANCE: 00503 //strcpy(DispMode,"Mode:Advance"); 00504 lcd.printf("Mode:Advance "); 00505 break; 00506 case RIGHT: 00507 //strcpy(DispMode,"Mode:Right"); 00508 lcd.printf("Mode:TurnRight "); 00509 break; 00510 case LEFT: 00511 //strcpy(DispMode,"Mode:Left"); 00512 lcd.printf("Mode:TurnLeft "); 00513 break; 00514 case BACK: 00515 //strcpy(DispMode,"Mode:Back"); 00516 lcd.printf("Mode:Back "); 00517 break; 00518 case STOP: 00519 //strcpy(DispMode,"Mode:Stop"); 00520 lcd.printf("Mode:Stop "); 00521 break; 00522 case READY: 00523 //strcpy(DispMode,"Mode:Stop"); 00524 lcd.printf("Mode:Ready "); 00525 break; 00526 case LINE_TRACE: 00527 //strcpy(DispMode,"Mode:LineTrace"); 00528 lcd.printf("Mode:LineTrace "); 00529 break; 00530 case AVOIDANCE: 00531 //strcpy(DispMode,"Mode:Avoidance"); 00532 lcd.setAddress(0,1); 00533 lcd.printf("Mode:Avoidance "); 00534 break; 00535 case SPEED: 00536 switch(flag_sp){ 00537 case(NORMAL): 00538 lcd.printf("Speed:Normal "); 00539 break; 00540 case(FAST): 00541 lcd.printf("Speed:Fast "); 00542 break; 00543 case(VERYFAST): 00544 lcd.printf("Speed:VeryFast "); 00545 break; 00546 } 00547 viewTimer.reset(); 00548 viewTimer.start(); 00549 break; 00550 } 00551 mutex.unlock(); 00552 } 00553 00554 void lcdBacklight(void const *argument){ 00555 if(flag_b == 1){ 00556 lcd.setBacklight(TextLCD::LightOn); 00557 }else{ 00558 lcd.setBacklight(TextLCD::LightOff); 00559 } 00560 flag_b = !flag_b; 00561 } 00562 00563 void bChange(){ 00564 lcd.setBacklight(TextLCD::LightOn); 00565 while(1){ 00566 pc.printf(" bChange1\r\n"); 00567 b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10; 00568 //pc.printf("%f",battery.read()); 00569 if(b <= 0){//すべての機能停止(今はなし) 00570 b = 0; 00571 //lcd.setBacklight(TextLCD::LightOff); 00572 //run = STOP; 00573 //exit(1);//電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。 00574 }else if(b > 50){ 00575 b = 50; 00576 } 00577 mutex.lock(); 00578 lcd.setAddress(0,0); 00579 lcd.printf("Battery:%3d%%",b); 00580 pc.printf(" bChange2\r\n"); 00581 mutex.unlock(); 00582 if(b <= 30){ 00583 if(flag_t == 0){ 00584 bTimer.start(500); 00585 flag_t = 1; 00586 } 00587 }else{ 00588 if(flag_t == 1){ 00589 bTimer.stop(); 00590 lcd.setBacklight(TextLCD::LightOn); 00591 flag_t = 0; 00592 } 00593 } 00594 } 00595 ThisThread::sleep_for(500); 00596 } 00597 int main() { 00598 mode = READY; 00599 beforeMode = READY; 00600 run = STOP; 00601 flag_sp = NORMAL; 00602 display(); 00603 00604 while(1){ 00605 pc.printf(" main1\r\n"); 00606 bChange(); 00607 pc.printf(" main2\r\n"); 00608 ThisThread::sleep_for(1); 00609 } 00610 }
Generated on Tue Jul 26 2022 02:57:10 by
1.7.2