mbed_robotcar
8/19 avoidance update 8/20 update
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 RawSerial pc(USBTX, USBRX); 00014 00015 /* マクロ定義、列挙型定義 */ 00016 #define MIN_V 2.0 // 電圧の最小値 00017 #define MAX_V 2.67 // 電圧の最大値 00018 #define LOW 0 // モーターOFF 00019 #define HIGH 1 // モーターON 00020 #define NORMAL 0 // 普通 00021 #define FAST 1 // 速い 00022 #define VERYFAST 2 // とても速い 00023 00024 /* 操作モード定義 */ 00025 enum MODE{ 00026 READY = -1, // -1：待ち 00027 ADVANCE = 1, // 1：前進 00028 RIGHT, // 2：右折 00029 LEFT, // 3：左折 00030 BACK, // 4：後退 00031 STOP, // 5：停止 00032 LINE_TRACE, // 6：ライントレース 00033 AVOIDANCE, // 7：障害物回避 00034 SPEED, // 8：スピード制御 00035 }; 00036 00037 /* ピン配置 */ 00038 ReceiverIR ir(p5); // リモコン操作 00039 DigitalOut trig(p6); // 超音波センサtrigger 00040 DigitalIn echo(p7); // 超音波センサecho 00041 DigitalIn ss1(p8); // ライントレースセンサ(左) 00042 DigitalIn ss2(p9); // ライントレースセンサ 00043 DigitalIn ss3(p10); // ライントレースセンサ 00044 DigitalIn ss4(p11); // ライントレースセンサ 00045 DigitalIn ss5(p12); // ライントレースセンサ(右) 00046 RawSerial esp(p13, p14); // Wi-Fiモジュール(tx, rx) 00047 AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V) 00048 PwmOut motorR2(p21); // 右モーター後退 00049 PwmOut motorR1(p22); // 右モーター前進 00050 PwmOut motorL2(p23); // 左モーター後退 00051 PwmOut motorL1(p24); // 左モーター前進 00052 PwmOut servo(p25); // サーボ 00053 I2C i2c_lcd(p28,p27); // LCD(tx, rx) 00054 00055 /* 変数宣言 */ 00056 int mode; // 操作モード 00057 int run; // 走行状態 00058 int beforeMode; // 前回のモード 00059 int flag_sp = 0; // スピード変化フラグ 00060 Timer viewTimer; // スピ―ド変更時に3秒計測タイマー 00061 float motorSpeed[9] = {0.4, 0.7, 0.8, 0.7, 0.8, 0.9, 0.8, 0.9, 1.0}; 00062 // モーター速度設定(後半はライントレース用) 00063 00064 Mutex mutex; // ミューテックス 00065 00066 /* decodeIR用変数 */ 00067 RemoteIR::Format format; 00068 uint8_t buf[32]; 00069 uint32_t bitcount; 00070 uint32_t code; 00071 00072 /* bChange, lcdbacklight用変数 */ 00073 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780); 00074 int b = 0; // バッテリー残量 00075 int flag_b = 0; // バックライト点滅フラグ 00076 int flag_t = 0; // バックライトタイマーフラグ 00077 00078 /* trace用変数 */ 00079 int sensArray[32] = {0,6,2,4,1,1,2,2, // ライントレースセンサパターン 00080 3,1,1,1,3,1,1,2, 00081 7,1,1,1,1,1,1,1, 00082 5,1,1,1,3,1,3,1}; 00083 00084 /* avoidance用変数 */ 00085 Timer timer; // 距離計測用タイマ 00086 int DT; // 距離 00087 int SC; // 正面 00088 int SL; // 左 00089 int SR; // 右 00090 int SLD; // 左前 00091 int SRD; // 右前 00092 int flag_a = 0; // 障害物有無のフラグ 00093 const int limit = 20; // 障害物の距離のリミット(単位：cm) 00094 int far; // 最も遠い距離 00095 int houkou; // 進行方向(1:前 2:左 3:右) 00096 int t1 = 0; 00097 00098 /*WiFi用変数*/ 00099 Timer time1; 00100 Timer time2; 00101 int bufflen, DataRX, ount, getcount, replycount, servreq, timeout; 00102 int bufl, ipdLen, linkID, weberror, webcounter,click_flag; 00103 float R1=100000, R2=10000; // resistor values to give a 10:1 reduction of measured AnalogIn voltage 00104 char webcount[8]; 00105 char type[16]; 00106 char channel[2]; 00107 char cmdbuff[32]; 00108 char replybuff[1024]; 00109 char webdata[1024]; // This may need to be bigger depending on WEB browser used 00110 char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added 00111 int port =80; // set server port 00112 int SERVtimeout =5; // set server timeout in seconds in case link breaks. 00113 char ssid[32] = "mbed02"; // enter WiFi router ssid inside the quotes 00114 char pwd [32] = "0123456789a"; // enter WiFi router password inside the quotes 00115 00116 00117 /* プロトタイプ宣言 */ 00118 void decodeIR(/*void const *argument*/); 00119 void motor(/*void const *argument*/); 00120 void changeSpeed(); 00121 void avoidance(/*void const *argument*/); 00122 void trace(/*void const *argument*/); 00123 void watchsurrounding3(); 00124 void watchsurrounding5(); 00125 int watch(); 00126 void bChange(); 00127 void display(); 00128 void lcdBacklight(void const *argument); 00129 void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck(); 00130 void wifi(/*void const *argument*/); 00131 Thread *deco_thread; // decodeIRをスレッド化 ：+3 00132 Thread *wifi_thread; 00133 //wifi_thread(wifi,NULL,osPriorityHigh); // wifiをスレッド化 00134 Thread *motor_thread; // motorをスレッド化 ：+2 00135 //Thread avoi_thread(avoidance, NULL, osPriorityHigh); // avoidanceをスレッド化：+2 00136 //Thread trace_thread(trace, NULL, osPriorityHigh); // traceをスレッド化 ：+2 00137 RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定 00138 //Ticker bTimer; 00139 Thread *avoi_thread; 00140 Thread *trace_thread; 00141 00142 DigitalOut led1(LED1); 00143 DigitalOut led2(LED2); 00144 DigitalOut led3(LED3); 00145 DigitalOut led4(LED4); 00146 00147 void setup(){ 00148 deco_thread = new Thread(decodeIR); 00149 deco_thread -> set_priority(osPriorityRealtime); 00150 motor_thread = new Thread(motor); 00151 motor_thread -> set_priority(osPriorityHigh); 00152 wifi_thread -> set_priority(osPriorityRealtime); 00153 display(); 00154 } 00155 00156 /* リモコン受信スレッド */ 00157 void decodeIR(/*void const *argument*/){ 00158 while(1){ 00159 // 受信待ち 00160 if (ir.getState() == ReceiverIR::Received){ // コード受信 00161 bitcount = ir.getData(&format, buf, sizeof(buf) * 8); 00162 if(bitcount > 1){ // 受信成功 00163 code=0; 00164 for(int j = 0; j < 4; j++){ 00165 code+=(buf[j]<<(8*(3-j))); 00166 } 00167 if(mode != SPEED){ // スピードモード以外なら 00168 beforeMode=mode; // 前回のモードに現在のモードを設定 00169 } 00170 switch(code){ 00171 case 0x40bf27d8: // クイック 00172 //pc.printf("mode = SPEED\r\n"); 00173 mode = SPEED; // スピードモード 00174 changeSpeed(); // 速度変更 00175 display(); // ディスプレイ表示 00176 mode = beforeMode; // 現在のモードに前回のモードを設定 00177 break; 00178 case 0x40be34cb: // レグザリンク 00179 //pc.printf("mode = LINE_TRACE\r\n"); 00180 if(trace_thread->get_state() == Thread::Deleted){ 00181 delete trace_thread; 00182 trace_thread = new Thread(trace); 00183 trace_thread -> set_priority(osPriorityHigh); 00184 } 00185 mode=LINE_TRACE; // ライントレースモード 00186 display(); // ディスプレイ表示 00187 break; 00188 case 0x40bf6e91: // 番組表 00189 //pc.printf("mode = AVOIDANCE\r\n"); 00190 if(avoi_thread->get_state() == Thread::Deleted){ 00191 delete avoi_thread; 00192 avoi_thread = new Thread(avoidance); 00193 avoi_thread -> set_priority(osPriorityHigh); 00194 } 00195 flag_a = 0; 00196 mode=AVOIDANCE; // 障害物回避モード 00197 run = ADVANCE; // 前進 00198 display(); // ディスプレイ表示 00199 break; 00200 case 0x40bf3ec1: // ↑ 00201 //pc.printf("mode = ADVANCE\r\n"); 00202 mode = ADVANCE; // 前進モード 00203 run = ADVANCE; // 前進 00204 display(); // ディスプレイ表示 00205 break; 00206 case 0x40bf3fc0: // ↓ 00207 //pc.printf("mode = BACK\r\n"); 00208 mode = BACK; // 後退モード 00209 run = BACK; // 後退 00210 display(); // ディスプレイ表示 00211 break; 00212 case 0x40bf5fa0: // ← 00213 //pc.printf("mode = LEFT\r\n"); 00214 mode = LEFT; // 左折モード 00215 run = LEFT; // 左折 00216 display(); // ディスプレイ表示 00217 break; 00218 case 0x40bf5ba4: // → 00219 //pc.printf("mode = RIGHT\r\n"); 00220 mode = RIGHT; // 右折モード 00221 run = RIGHT; // 右折 00222 display(); // ディスプレイ表示 00223 break; 00224 case 0x40bf3dc2: // 決定 00225 //pc.printf("mode = STOP\r\n"); 00226 mode = STOP; // 停止モード 00227 run = STOP; // 停止 00228 display(); // ディスプレイ表示 00229 break; 00230 default: 00231 ; 00232 } 00233 if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){ 00234 trace_thread->terminate(); 00235 } 00236 if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){ 00237 avoi_thread->terminate(); 00238 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00239 } 00240 } 00241 } 00242 if(viewTimer.read_ms()>=3000){ // スピードモードのまま3秒経過 00243 viewTimer.stop(); // タイマーストップ 00244 viewTimer.reset(); // タイマーリセット 00245 display(); // ディスプレイ表示 00246 } 00247 ThisThread::sleep_for(90); // 90ms待つ 00248 } 00249 } 00250 00251 /* モーター制御スレッド */ 00252 void motor(/*void const *argument*/){ 00253 while(1){ 00254 /* 走行状態の場合分け */ 00255 switch(run){ 00256 /* 前進 */ 00257 case ADVANCE: 00258 motorR1 = LOW; // 右前進モーターOFF 00259 motorR2 = motorSpeed[flag_sp]; // 右後退モーターON 00260 motorL1 = motorSpeed[flag_sp]; // 左前進モーターON 00261 motorL2 = LOW; // 左後退モーターOFF 00262 break; 00263 /* 右折 */ 00264 case RIGHT: 00265 motorR1 = motorSpeed[flag_sp]; // 右前進モーターON 00266 motorR2 = LOW; // 右後退モーターOFF 00267 motorL1 = motorSpeed[flag_sp]; // 左前進モーターON 00268 motorL2 = LOW; // 左後退モーターOFF 00269 break; 00270 /* 左折 */ 00271 case LEFT: 00272 motorR1 = LOW; // 右前進モーターOFF 00273 motorR2 = motorSpeed[flag_sp]; // 右後退モーターON 00274 motorL1 = LOW; // 左前進モーターOFF 00275 motorL2 = motorSpeed[flag_sp]; // 左後退モーターON 00276 break; 00277 /* 後退 */ 00278 case BACK: 00279 motorR1 = motorSpeed[flag_sp]; // 右前進モーターON 00280 motorR2 = LOW; // 右後退モーターOFF 00281 motorL1 = LOW; // 左前進モーターOFF 00282 motorL2 = motorSpeed[flag_sp]; // 左後退モーターON 00283 break; 00284 /* 停止 */ 00285 case STOP: 00286 motorR1 = LOW; // 右前進モーターOFF 00287 motorR2 = LOW; // 右後退モーターOFF 00288 motorL1 = LOW; // 左前進モーターOFF 00289 motorL2 = LOW; // 左後退モーターOFF 00290 break; 00291 } 00292 if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら 00293 flag_sp %= 3; // スピード変更フラグ調整 00294 } 00295 ThisThread::sleep_for(30); // 30ms待つ 00296 } 00297 } 00298 00299 /* スピード変更関数 */ 00300 void changeSpeed(){ 00301 if(flag_sp%3 == 2){ // スピード変更フラグを3で割った余りが2なら 00302 flag_sp -= 2; // スピード変更フラグを-2 00303 00304 }else{ // それ以外 00305 flag_sp = flag_sp + 1; // スピード変更フラグを+1 00306 } 00307 } 00308 00309 /* ライントレーススレッド */ 00310 void trace(){ 00311 while(1){ 00312 /* 各センサー値読み取り */ 00313 int sensor1 = ss1; 00314 int sensor2 = ss2; 00315 int sensor3 = ss3; 00316 int sensor4 = ss4; 00317 int sensor5 = ss5; 00318 pc.printf("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5); 00319 int sensD = 0; 00320 00321 /* センサー値の決定 */ 00322 if(sensor1 > 0) sensD |= 0x10; 00323 if(sensor2 > 0) sensD |= 0x08; 00324 if(sensor3 > 0) sensD |= 0x04; 00325 if(sensor4 > 0) sensD |= 0x02; 00326 if(sensor5 > 0) sensD |= 0x01; 00327 00328 /* センサー値によって場合分け */ 00329 switch(sensArray[sensD]){ 00330 case 1: 00331 run = ADVANCE; // 低速で前進 00332 break; 00333 case 2: 00334 // flag_sp = flag_sp % 3 + 6; 00335 run = RIGHT; // 低速で右折 00336 break; 00337 case 3: 00338 // flag_sp = flag_sp % 3 + 6; 00339 run = LEFT; // 低速で左折 00340 break; 00341 case 4: 00342 flag_sp = flag_sp % 3 + 3; 00343 run = RIGHT; // 中速で右折 00344 break; 00345 case 5: 00346 flag_sp = flag_sp % 3 + 3; 00347 run = LEFT; // 中速で左折 00348 break; 00349 case 6: 00350 flag_sp = flag_sp % 3 + 6; 00351 run = RIGHT; // 高速で右折 00352 break; 00353 case 7: 00354 flag_sp = flag_sp % 3 + 6; 00355 run = LEFT; // 高速で左折 00356 break; 00357 default: 00358 break; // 前回動作を継続 00359 } 00360 ThisThread::sleep_for(30); // 30ms待つ 00361 } 00362 } 00363 00364 /* 障害物回避走行スレッド */ 00365 void avoidance(){ 00366 int i; 00367 while(1){ 00368 watchsurrounding3(); 00369 pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); 00370 if(flag_a == 0){ // 障害物がない場合 00371 run = ADVANCE; // 前進 00372 }else{ // 障害物がある場合 00373 i = 0; 00374 if(SC < 15){ // 正面15cm以内に障害物が現れた場合 00375 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00376 ThisThread::sleep_for(100); // 100ms待つ 00377 run = BACK; // 後退 00378 int cnt_kyori=0; 00379 int kyori = watch(); 00380 while(kyori < limit){ // 正面20cm以内に障害物がある間 00381 if(kyori==-1){ 00382 cnt_kyori++; 00383 if(cnt_kyori>15){ 00384 cnt_kyori=0; 00385 break; 00386 } 00387 } 00388 kyori = watch(); 00389 } 00390 /*while(i < 30){ // 正面20cm以内に障害物がある間 00391 if(watch() < limit){ 00392 break; 00393 } 00394 i++; 00395 } 00396 i = 0;*/ 00397 run = STOP; // 停止 00398 } 00399 watchsurrounding5(); 00400 if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 00401 run = LEFT; // 左折 00402 while(i < 1){ // 進行方向確認 00403 if(watch() > limit){ 00404 i++; 00405 }else{ 00406 i = 0; 00407 } 00408 } 00409 run = STOP; // 停止 00410 }else { // 全方向以外 00411 far = SC; // 正面を最も遠い距離に設定 00412 houkou = 1; // 進行方向を前に設定 00413 if(far < SLD || far < SL){ // 左または左前がより遠い場合 00414 if(SL < SLD){ // 左前が左より遠い場合 00415 far = SLD; // 左前を最も遠い距離に設定 00416 }else{ // 左が左前より遠い場合 00417 far = SL; // 左を最も遠い距離に設定 00418 } 00419 houkou = 2; // 進行方向を左に設定 00420 } 00421 if(far < SRD || far < SR){ // 右または右前がより遠い場合 00422 if(SR < SRD){ // 右前が右より遠い場合 00423 far = SRD; // 右前を最も遠い距離に設定 00424 }else{ // 右が右前よりも遠い場合 00425 far = SR; // 右を最も遠い距離に設定 00426 } 00427 houkou = 3; // 進行方向を右に設定 00428 } 00429 switch(houkou){ // 進行方向の場合分け 00430 case 1: // 前の場合 00431 run = ADVANCE; // 前進 00432 ThisThread::sleep_for(500); // 0.5秒待つ 00433 break; 00434 case 2: // 左の場合 00435 run = LEFT; // 左折 00436 //int kyori = watch(); 00437 //int kyori_f=0; 00438 while(i < 20){ // 進行方向確認 00439 /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) 00440 break; // ループ+ 00441 }else if(kyori==-1){ 00442 kyori_f++; 00443 }else{ 00444 kyori_f = 0; 00445 i++; 00446 }*/ 00447 if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) 00448 break; // ループ+ 00449 }else{ 00450 i++; 00451 } 00452 } 00453 run = STOP; // 停止 00454 break; 00455 case 3: // 右の場合 00456 run = RIGHT; // 右折 00457 //int kyori = watch(); 00458 //int kyori_f=0; 00459 while(i < 20){ // 進行方向確認 00460 /*if(kyori > (far - 2) || kyori_f == 2){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) 00461 break; // ループ+ 00462 }else if(kyori==-1){ 00463 kyori_f++; 00464 }else{ 00465 kyori_f = 0; 00466 i++; 00467 }*/ 00468 if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) 00469 break; // ループ+ 00470 }else{ 00471 i++; 00472 } 00473 } 00474 run = STOP; // 停止 00475 break; 00476 } 00477 } 00478 } 00479 flag_a = 0; // 障害物有無フラグを0にセット 00480 if(SLD < 29){ // 正面15cm以内に障害物が現れた場合 00481 run = RIGHT; // 右折 00482 ThisThread::sleep_for(200); // 100ms待つ 00483 run = STOP; // 停止 00484 }else if(SRD < 29){ 00485 run = LEFT; // 左折 00486 ThisThread::sleep_for(200); // 100ms待つ 00487 run = STOP; // 停止 00488 } 00489 } 00490 } 00491 00492 /* 距離計測関数 */ 00493 int watch(){ 00494 do{ 00495 trig = 0; 00496 ThisThread::sleep_for(5); // 5ms待つ 00497 trig = 1; 00498 ThisThread::sleep_for(15); // 15ms待つ 00499 trig = 0; 00500 timer.start(); 00501 t1=timer.read_ms(); 00502 while(echo.read() == 0 && t1<10){ 00503 t1=timer.read_ms(); 00504 led1 = 1; 00505 } 00506 timer.stop(); 00507 timer.reset(); 00508 /*if((t1-t2) >= 10){ 00509 run = STOP;*/ 00510 }while(t1 >= 10); 00511 timer.start(); // 距離計測タイマースタート 00512 while(echo.read() == 1){ 00513 } 00514 timer.stop(); // 距離計測タイマーストップ 00515 DT = (int)(timer.read_us()*0.01657); // 距離計算 00516 if(DT > 1000){ 00517 DT = -1; 00518 }else if(DT > 150){ // 検知範囲外なら100cmに設定 00519 DT = 150; 00520 } 00521 timer.reset(); // 距離計測タイマーリセット 00522 led1 = 0; 00523 return DT; 00524 } 00525 00526 /* 障害物検知関数 */ 00527 void watchsurrounding3(){ 00528 //servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00529 //ThisThread::sleep_for(200); // 100ms待つ 00530 SC = watch(); 00531 if(SC < limit){ // 正面20cm以内に障害物がある場合 00532 if(SC!=-1){ 00533 run = STOP; // 停止 00534 flag_a = 1; 00535 return; 00536 } 00537 } 00538 servo.pulsewidth_us(1925); // サーボを左に40度回転 00539 ThisThread::sleep_for(100); // 250ms待つ 00540 SLD = watch(); 00541 if(SLD < limit){ // 左前20cm以内に障害物がある場合 00542 run = STOP; // 停止 00543 flag_a = 1; 00544 return; 00545 } 00546 servo.pulsewidth_us(1450); 00547 ThisThread::sleep_for(150); 00548 SC = watch(); 00549 if(SC < limit){ 00550 if(SC!=-1){ 00551 run = STOP; // 停止 00552 flag_a = 1; 00553 return; 00554 } 00555 } 00556 servo.pulsewidth_us(925); // サーボを右に40度回転 00557 ThisThread::sleep_for(100); // 250ms待つ 00558 SRD = watch(); 00559 if(SRD < limit){ // 右前20cm以内に障害物がある場合 00560 run = STOP; // 停止 00561 flag_a = 1; 00562 return; 00563 } 00564 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00565 ThisThread::sleep_for(100); // 100ms待つ 00566 /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 00567 flag_a = 1; // 障害物有無フラグに1をセット 00568 }*/ 00569 } 00570 00571 /* 障害物検知関数 */ 00572 void watchsurrounding5(){ 00573 //servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00574 //ThisThread::sleep_for(200); // 100ms待つ 00575 SC = watch(); 00576 servo.pulsewidth_us(1925); // サーボを左に40度回転 00577 ThisThread::sleep_for(100); // 250ms待つ 00578 SLD = watch(); 00579 servo.pulsewidth_us(2400); // サーボを左に90度回転 00580 ThisThread::sleep_for(100); // 250ms待つ 00581 SL = watch(); 00582 servo.pulsewidth_us(1450); 00583 ThisThread::sleep_for(250); 00584 SC = watch(); 00585 servo.pulsewidth_us(925); // サーボを右に40度回転 00586 ThisThread::sleep_for(100); // 250ms待つ 00587 SRD = watch(); 00588 servo.pulsewidth_us(500); // サーボを右に90度回転 00589 ThisThread::sleep_for(100); // 250ms待つ 00590 SR = watch(); 00591 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 00592 ThisThread::sleep_for(250); // 100ms待つ 00593 } 00594 00595 /* ディスプレイ表示関数 */ 00596 void display(){ 00597 mutex.lock(); // ミューテックスロック 00598 lcd.setAddress(0,1); 00599 00600 /* 操作モードによる場合分け */ 00601 switch(mode){ 00602 /* 前進 */ 00603 case ADVANCE: 00604 lcd.printf("Mode:Advance "); 00605 break; 00606 /* 右折 */ 00607 case RIGHT: 00608 lcd.printf("Mode:TurnRight "); 00609 break; 00610 /* 左折 */ 00611 case LEFT: 00612 lcd.printf("Mode:TurnLeft "); 00613 break; 00614 /* 後退 */ 00615 case BACK: 00616 lcd.printf("Mode:Back "); 00617 break; 00618 /* 停止 */ 00619 case STOP: 00620 lcd.printf("Mode:Stop "); 00621 break; 00622 /* 待ち */ 00623 case READY: 00624 lcd.printf("Mode:Ready "); 00625 break; 00626 /* ライントレース */ 00627 case LINE_TRACE: 00628 lcd.printf("Mode:LineTrace "); 00629 break; 00630 /* 障害物回避 */ 00631 case AVOIDANCE: 00632 lcd.printf("Mode:Avoidance "); 00633 break; 00634 /* スピード制御 */ 00635 case SPEED: 00636 /* スピードの状態で場合分け */ 00637 switch(flag_sp){ 00638 /* 普通 */ 00639 case(NORMAL): 00640 lcd.printf("Speed:Normal "); 00641 break; 00642 /* 速い */ 00643 case(FAST): 00644 lcd.printf("Speed:Fast "); 00645 break; 00646 /* とても速い */ 00647 case(VERYFAST): 00648 lcd.printf("Speed:VeryFast "); 00649 break; 00650 } 00651 viewTimer.reset(); // タイマーリセット 00652 viewTimer.start(); // タイマースタート 00653 break; 00654 } 00655 mutex.unlock(); // ミューテックスアンロック 00656 } 00657 00658 /* バックライト点滅 */ 00659 void lcdBacklight(void const *argument){ 00660 if(flag_b == 1){ // バックライト点滅フラグが1なら 00661 lcd.setBacklight(TextLCD::LightOn); // バックライトON 00662 }else{ // バックライト点滅フラグが0なら 00663 lcd.setBacklight(TextLCD::LightOff); // バックライトOFF 00664 } 00665 flag_b = !flag_b; // バックライト点滅フラグ切り替え 00666 } 00667 00668 /* バッテリー残量更新関数 */ 00669 void bChange(){ 00670 //pc.printf(" bChange1\r\n"); 00671 b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10; 00672 if(b <= 0){ // バッテリー残量0%なら全ての機能停止 00673 b = 0; 00674 //lcd.setBacklight(TextLCD::LightOff); 00675 //run = STOP; 00676 //exit(1); // 電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。 00677 } 00678 mutex.lock(); // ミューテックスロック 00679 lcd.setAddress(0,0); 00680 lcd.printf("Battery:%3d%%",b); // バッテリー残量表示 00681 mutex.unlock(); // ミューテックスアンロック 00682 if(b <= 30){ // バッテリー残量30%以下なら 00683 if(flag_t == 0){ // バックライトタイマーフラグが0なら 00684 //bTimer.attach(lcdBacklight,0.5); 00685 bTimer.start(500); // 0.5秒周期でバックライトタイマー割り込み 00686 flag_t = 1; // バックライトタイマーフラグを1に切り替え 00687 } 00688 }else{ 00689 if(flag_t == 1){ // バックライトタイマーフラグが1なら 00690 //bTimer.detach(); 00691 bTimer.stop(); // バックライトタイマーストップ 00692 lcd.setBacklight(TextLCD::LightOn); // バックライトON 00693 flag_t = 0; // バックライトタイマーフラグを0に切り替え 00694 } 00695 } 00696 } 00697 00698 // Serial Interrupt read ESP data 00699 void callback() 00700 { 00701 //pc.printf("\n\r------------ callback is being called --------------\n\r"); 00702 led3=1; 00703 while (esp.readable()) { 00704 webbuff[ount] = esp.getc(); 00705 ount++; 00706 } 00707 if(strlen(webbuff)>bufflen) { 00708 pc.printf("\f\n\r------------ webbuff over bufflen --------------\n\r"); 00709 DataRX=1; 00710 led3=0; 00711 } 00712 } 00713 00714 void wifi(/*void const *argument*/) 00715 { 00716 pc.printf("\f\n\r------------ ESP8266 Hardware Reset psq --------------\n\r"); 00717 ThisThread::sleep_for(100); 00718 led1=1,led2=0,led3=0; 00719 timeout=6000; 00720 getcount=500; 00721 getreply(); 00722 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F 00723 startserver(); 00724 00725 while(1) { 00726 if(DataRX==1) { 00727 pc.printf("\f\n\r------------ main while > if --------------\n\r"); 00728 click_flag = 1; 00729 ReadWebData(); 00730 pc.printf("\f\n\r------------ click_flag=%d --------------\n\r",click_flag); 00731 //if ((servreq == 1 && weberror == 0) && click_flag == 1) { 00732 if (servreq == 1 && weberror == 0) { 00733 pc.printf("\f\n\r------------ befor send page --------------\n\r"); 00734 sendpage(); 00735 } 00736 pc.printf("\f\n\r------------ send_check begin --------------\n\r"); 00737 00738 //sendcheck(); 00739 pc.printf("\f\n\r------------ ssend_check end--------------\n\r"); 00740 00741 esp.attach(&callback); 00742 pc.printf(" IPD Data:\r\n\n Link ID = %d,\r\n IPD Header Length = %d \r\n IPD Type = %s\r\n", linkID, ipdLen, type); 00743 pc.printf("\n\n HTTP Packet: \n\n%s\n", webdata); 00744 pc.printf(" Web Characters sent : %d\n\n", bufl); 00745 pc.printf(" -------------------------------------\n\n"); 00746 servreq=0; 00747 } 00748 ThisThread::sleep_for(100); 00749 } 00750 } 00751 // Static WEB page 00752 void sendpage() 00753 { 00754 // WEB page data 00755 00756 strcpy(webbuff, "<!DOCTYPE html>"); 00757 strcat(webbuff, "<html><head><title>RobotCar</title><meta name='viewport' content='width=device-width'/>"); 00758 //strcat(webbuff, "<meta http-equiv=\"refresh\" content=\"5\"; >"); 00759 strcat(webbuff, "<style type=\"text/css\">.noselect{ width:100px;height:60px;}.light{ width:100px;height:60px;background-color:#00ff66;}.load{ width: 50px; height: 30px;font-size:10px}</style>"); 00760 strcat(webbuff, "</head><body><center><p><strong>Robot Car Remote Controller"); 00761 strcat(webbuff, "</strong></p><td style='vertical-align:top;'><strong>Battery level "); 00762 if(b > 30) { //残電量表示 00763 sprintf(webbuff, "%s%3d", webbuff, b); 00764 } else { //30%より下の場合残電量を赤文字 00765 strcat(webbuff, "<font color=\"red\">"); 00766 sprintf(webbuff, "%s%3d", webbuff, b); 00767 strcat(webbuff, "</font>"); 00768 } 00769 strcat(webbuff, "%</strong>"); 00770 strcat(webbuff, "<button id=\"reloadbtn\" type=\"button\" class=\"load\" onclick=\"rel()\">RELOAD</button>"); 00771 strcat(webbuff, "</td></p>"); 00772 strcat(webbuff, "<br>"); 00773 strcat(webbuff, "<table><tr><td></td><td>"); 00774 00775 switch(mode) { //ブラウザ更新時の現在の車の状態からボタンの点灯判定 00776 case ADVANCE: //前進 00777 strcat(webbuff, "<button id='gobtn' type='button' class=\"light\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00778 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00779 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00780 strcat(webbuff, "</button></td><td>"); 00781 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00782 strcat(webbuff, "</button></td><td>"); 00783 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00784 strcat(webbuff, "</button></td></tr><td></td><td>"); 00785 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00786 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00787 strcat(webbuff, "<strong>Mode</strong>"); 00788 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00789 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00790 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00791 break; 00792 case LEFT: //左折 00793 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00794 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00795 strcat(webbuff, "<button id='leftbtn' type='button' class=\"light\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00796 strcat(webbuff, "</button></td><td>"); 00797 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00798 strcat(webbuff, "</button></td><td>"); 00799 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00800 strcat(webbuff, "</button></td></tr><td></td><td>"); 00801 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00802 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00803 strcat(webbuff, "<strong>Mode</strong>"); 00804 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00805 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00806 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00807 break; 00808 case STOP: //停止 00809 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00810 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00811 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00812 strcat(webbuff, "</button></td><td>"); 00813 strcat(webbuff, "<button id='stopbtn' type='button' class=\"light\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00814 strcat(webbuff, "</button></td><td>"); 00815 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00816 strcat(webbuff, "</button></td></tr><td></td><td>"); 00817 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00818 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00819 strcat(webbuff, "<strong>Mode</strong>"); 00820 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00821 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00822 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00823 break; 00824 case RIGHT: //右折 00825 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00826 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00827 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00828 strcat(webbuff, "</button></td><td>"); 00829 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00830 strcat(webbuff, "</button></td><td>"); 00831 strcat(webbuff, "<button id='rightbtn' type='button' class=\"light\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00832 strcat(webbuff, "</button></td></tr><td></td><td>"); 00833 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00834 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00835 strcat(webbuff, "<strong>Mode</strong>"); 00836 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00837 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00838 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00839 break; 00840 case BACK: //後進 00841 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00842 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00843 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00844 strcat(webbuff, "</button></td><td>"); 00845 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00846 strcat(webbuff, "</button></td><td>"); 00847 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00848 strcat(webbuff, "</button></td></tr><td></td><td>"); 00849 strcat(webbuff, "<button id='backbtn' type='button' class=\"light\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00850 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr><td>"); 00851 strcat(webbuff, "<strong>Mode</strong>"); 00852 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00853 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00854 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00855 break; 00856 case AVOIDANCE: //障害物回避 00857 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00858 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00859 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00860 strcat(webbuff, "</button></td><td>"); 00861 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00862 strcat(webbuff, "</button></td><td>"); 00863 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00864 strcat(webbuff, "</button></td></tr><td></td><td>"); 00865 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00866 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00867 strcat(webbuff, "<strong>Mode</strong>"); 00868 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"light\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00869 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00870 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00871 break; 00872 case LINE_TRACE: //ライントレース 00873 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00874 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00875 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00876 strcat(webbuff, "</button></td><td>"); 00877 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00878 strcat(webbuff, "</button></td><td>"); 00879 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00880 strcat(webbuff, "</button></td></tr><td></td><td>"); 00881 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00882 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00883 strcat(webbuff, "<strong>Mode</strong>"); 00884 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00885 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00886 strcat(webbuff, "<button id='tracebtn' type='button' class=\"light\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00887 break; 00888 default: //その他 00889 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.id,this.value)'>GO"); 00890 strcat(webbuff, "</button></td><td></td></tr><tr><td>"); 00891 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.id,this.value)' >LEFT"); 00892 strcat(webbuff, "</button></td><td>"); 00893 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.id,this.value)' >STOP"); 00894 strcat(webbuff, "</button></td><td>"); 00895 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.id,this.value)' >RIGHT"); 00896 strcat(webbuff, "</button></td></tr><td></td><td>"); 00897 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.id,this.value)' >BACK"); 00898 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>"); 00899 strcat(webbuff, "<strong>Mode</strong>"); 00900 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.id,this.value)' >"); 00901 strcat(webbuff, "AVOIDANCE</button></td><td>"); 00902 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.id,this.value)' >LINE_TRACE"); 00903 break; 00904 } 00905 strcat(webbuff, "</button></td></tr></table>"); 00906 strcat(webbuff, "<strong>Speed</strong>"); 00907 strcat(webbuff, "<table><tr><td>"); 00908 //ready示速度だけ点灯 00909 switch (flag_sp) { //現在の速度のボタン表示 00910 case 0: //ノーマル 00911 strcat(webbuff, "<button id='sp1btn' type='button' class=\"light\" value=\"Normal\" onClick='send_mes_spe(this.id,this.value)' >Normal"); 00912 strcat(webbuff, "</button></td><td>"); 00913 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.id,this.value)' >Fast"); 00914 strcat(webbuff, "</button></td><td>"); 00915 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.id,this.value)' >VeryFast"); 00916 break; 00917 case 1: //ファスト 00918 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.id,this.value)' >Normal"); 00919 strcat(webbuff, "</button></td><td>"); 00920 strcat(webbuff, "<button id='sp2btn' type='button' class=\"light\" value=\"Fast\" onClick='send_mes_spe(this.id,this.value)' >Fast"); 00921 strcat(webbuff, "</button></td><td>"); 00922 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.id,this.value)' >VeryFast"); 00923 break; 00924 case 2: //ベリーファスト 00925 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.id,this.value)' >Normal"); 00926 strcat(webbuff, "</button></td><td>"); 00927 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.id,this.value)' >Fast"); 00928 strcat(webbuff, "</button></td><td>"); 00929 strcat(webbuff, "<button id='sp3btn' type='button' class=\"light\" value=\"VeryFast\" onClick='send_mes_spe(this.id,this.value)' >VeryFast"); 00930 break; 00931 default: //その他 00932 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.id,this.value)' >Normal"); 00933 strcat(webbuff, "</button></td><td>"); 00934 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.id,this.value)' >Fast"); 00935 strcat(webbuff, "</button></td><td>"); 00936 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.id,this.value)' >VeryFast"); 00937 break; 00938 } 00939 strcat(webbuff, "</button></td></tr></table>"); 00940 00941 strcat(webbuff, "</center>"); 00942 strcat(webbuff, "</body>"); 00943 strcat(webbuff, "</html>"); 00944 strcat(webbuff, "<script language=\"javascript\" type=\"text/javascript\">"); //機能 00945 00946 strcat(webbuff, "function rel(){"); 00947 strcat(webbuff, "location.reload();"); 00948 strcat(webbuff, "}"); 00949 00950 strcat(webbuff, "function htmlacs(url) {"); 00951 strcat(webbuff, "var xhr = new XMLHttpRequest();"); 00952 strcat(webbuff, "xhr.open(\"GET\", url);"); 00953 strcat(webbuff, "xhr.send(\"\");"); 00954 strcat(webbuff, "}"); 00955 00956 strcat(webbuff, "function send_mes(btnmes,btnval){"); //mode変更ボタン入力時の点灯消灯判定 00957 strcat(webbuff, "console.log(btnval);"); 00958 00959 strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;"); 00960 strcat(webbuff, "htmlacs(url);"); 00961 strcat(webbuff, "console.log(url);"); 00962 strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");"); 00963 strcat(webbuff, "for(var i=1;i<8;i++){"); 00964 strcat(webbuff, "if(buttons[i].value == btnval){"); 00965 strcat(webbuff, "buttons[i].className=\"light\";"); 00966 strcat(webbuff, "}else{"); 00967 strcat(webbuff, "buttons[i].className=\"noselect\";"); 00968 strcat(webbuff, "}"); 00969 strcat(webbuff, "}"); 00970 strcat(webbuff, "}"); 00971 00972 strcat(webbuff, "function send_mes_spe(btnmes,btnval){"); //speed変更ボタン入力時の点灯消灯判定 00973 strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;"); 00974 strcat(webbuff, "htmlacs(url);"); 00975 strcat(webbuff, "console.log(url);"); 00976 strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");"); 00977 strcat(webbuff, "for(var i=8;i<11;i++){"); 00978 strcat(webbuff, "if(buttons[i].value == btnval){"); 00979 strcat(webbuff, "buttons[i].className=\"light\";"); 00980 strcat(webbuff, "}else{"); 00981 strcat(webbuff, "buttons[i].className=\"noselect\";"); 00982 strcat(webbuff, "}"); 00983 strcat(webbuff, "}"); 00984 strcat(webbuff, "}"); 00985 strcat(webbuff, "</script>"); 00986 // end of WEB page data 00987 bufl = strlen(webbuff); // get total page buffer length 00988 //sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length. 00989 00990 sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl>2048?2048:bufl)); // send IPD link channel and buffer character length. 00991 timeout=500; 00992 getcount=40; 00993 SendCMD(); 00994 getreply(); 00995 pc.printf(replybuff); 00996 //pc.printf("\n++++++++++ AT+CIPSENDBUF=%d,%d+++++++++\r\n", linkID, (bufl>2048?2048:bufl)); 00997 00998 pc.printf("\n++++++++++ bufl is %d ++++++++++\r\n",bufl); 00999 01000 //pastthrough mode 01001 SendWEB(); // send web page 01002 pc.printf("\n++++++++++ webbuff clear ++++++++++\r\n"); 01003 01004 memset(webbuff, '\0', sizeof(webbuff)); 01005 sendcheck(); 01006 } 01007 01008 // Large WEB buffer data send 01009 void SendWEB() 01010 { 01011 int i=0; 01012 if(esp.writeable()) { 01013 while(webbuff[i]!='\0') { 01014 esp.putc(webbuff[i]); 01015 01016 //**** 01017 //output at command when 2000 01018 if(((i%2047)==0) && (i>0)) { 01019 //wait_ms(10); 01020 ThisThread::sleep_for(10); 01021 sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); // send IPD link channel and buffer character length. 01022 //pc.printf("\r\n++++++++++ AT+CIPSENDBUF=%d,%d ++++++++++\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); 01023 timeout=600; 01024 getcount=50; 01025 SendCMD(); 01026 getreply(); 01027 //pc.printf(replybuff); 01028 //pc.printf("\r\n+++++++++++++++++++\r\n"); 01029 } 01030 //**** 01031 i++; 01032 //pc.printf("%c",webbuff[i]); 01033 } 01034 } 01035 pc.printf("\n++++++++++ send web i= %dinfo ++++++++++\r\n",i); 01036 } 01037 01038 01039 void sendcheck() 01040 { 01041 weberror=1; 01042 timeout=500; 01043 getcount=24; 01044 time2.reset(); 01045 time2.start(); 01046 01047 /* 01048 while(weberror==1 && time2.read() <5) { 01049 getreply(); 01050 if (strstr(replybuff, "SEND OK") != NULL) { 01051 weberror=0; // wait for valid SEND OK 01052 } 01053 } 01054 */ 01055 if(weberror==1) { // restart connection 01056 strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); 01057 timeout=500; 01058 getcount=10; 01059 SendCMD(); 01060 getreply(); 01061 pc.printf(replybuff); 01062 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port); 01063 timeout=500; 01064 getcount=10; 01065 SendCMD(); 01066 getreply(); 01067 pc.printf(replybuff); 01068 } else { 01069 sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection 01070 SendCMD(); 01071 getreply(); 01072 pc.printf(replybuff); 01073 } 01074 time2.reset(); 01075 } 01076 01077 // Reads and processes GET and POST web data 01078 void ReadWebData() 01079 { 01080 pc.printf("+++++++++++++++++Read Web Data+++++++++++++++++++++\r\n"); 01081 ThisThread::sleep_for(200); 01082 esp.attach(NULL); 01083 ount=0; 01084 DataRX=0; 01085 weberror=0; 01086 memset(webdata, '\0', sizeof(webdata)); 01087 int x = strcspn (webbuff,"+"); 01088 if(x) { 01089 strcpy(webdata, webbuff + x); 01090 weberror=0; 01091 int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type); 01092 //int i=0; 01093 pc.printf("+++++++++++++++++succed rec begin+++++++++++++++++++++\r\n"); 01094 pc.printf("%s",webdata); 01095 pc.printf("+++++++++++++++++succed rec end+++++++++++++++++++++\r\n"); 01096 if( strstr(webdata, "Normal") != NULL ) { 01097 pc.printf("++++++++++++++++++Normal++++++++++++++++++++"); 01098 mode = SPEED; // スピードモード 01099 flag_sp = 0; 01100 display(); // ディスプレイ表示 01101 mode = beforeMode; // 現在のモードに前回のモードを設定 01102 }else if( strstr(webdata, "VeryFast") != NULL ) { 01103 pc.printf("+++++++++++++++++++VeryFast+++++++++++++++++++"); 01104 mode = SPEED; // スピードモード 01105 flag_sp = 2; 01106 display(); // ディスプレイ表示 01107 mode = beforeMode; // 現在のモードに前回のモードを設定 01108 }else if( strstr(webdata, "Fast") != NULL ) { 01109 pc.printf("++++++++++++++++++++Fast++++++++++++++++++"); 01110 mode = SPEED; // スピードモード 01111 flag_sp = 1; 01112 display(); // ディスプレイ表示 01113 mode = beforeMode; // 現在のモードに前回のモードを設定 01114 }else{ 01115 beforeMode = mode; 01116 } 01117 if( strstr(webdata, "GO") != NULL ) { 01118 pc.printf("+++++++++++++++++前進+++++++++++++++++++++\r\n"); 01119 //delete avoi_thread; //障害物回避スレッド停止 01120 //delete trace_thread; //ライントレーススレッド停止 01121 run = ADVANCE; // 前進 01122 mode = ADVANCE; // モード変更 01123 display(); // ディスプレイ表示 01124 } 01125 01126 if( strstr(webdata, "LEFT") != NULL ) { 01127 pc.printf("+++++++++++++++++左折+++++++++++++++++++++\r\n"); 01128 //delete avoi_thread; //障害物回避スレッド停止 01129 //delete trace_thread; //ライントレーススレッド停止 01130 run = LEFT; // 左折 01131 mode = LEFT; // モード変更 01132 display(); // ディスプレイ表示 01133 } 01134 01135 if( strstr(webdata, "STOP") != NULL ) { 01136 pc.printf("+++++++++++++++++停止+++++++++++++++++++++\r\n"); 01137 //delete avoi_thread; //障害物回避スレッド停止 01138 //delete trace_thread; //ライントレーススレッド停止 01139 run = STOP; // 停止 01140 mode = STOP; // モード変更 01141 display(); // ディスプレイ表示 01142 } 01143 01144 if( strstr(webdata, "RIGHT") != NULL ) { 01145 pc.printf("+++++++++++++++++右折+++++++++++++++++++++\r\n"); 01146 //delete avoi_thread; //障害物回避スレッド停止 01147 //delete trace_thread; //ライントレーススレッド停止 01148 run = RIGHT; // 右折 01149 mode = RIGHT; // モード変更 01150 display(); // ディスプレイ表示 01151 } 01152 01153 if( strstr(webdata, "BACK") != NULL ) { 01154 pc.printf("+++++++++++++++++後進+++++++++++++++++++++\r\n"); 01155 //delete avoi_thread; //障害物回避スレッド停止 01156 //delete trace_thread; //ライントレーススレッド停止 01157 run = BACK; // 後進 01158 mode = BACK; // モード変更 01159 display(); // ディスプレイ表示 01160 } 01161 pc.printf("+++++++++++++++++succed+++++++++++++++++++++"); 01162 01163 if( strstr(webdata, "AVOIDANCE") != NULL ) { 01164 pc.printf("+++++++++++++++++AVOIDANCE+++++++++++++++++++++"); 01165 if(avoi_thread->get_state() == Thread::Deleted) { 01166 delete avoi_thread; //障害物回避スレッド停止 01167 avoi_thread = new Thread(avoidance); 01168 avoi_thread -> set_priority(osPriorityHigh); 01169 } 01170 mode=AVOIDANCE; 01171 run = ADVANCE; 01172 display(); // ディスプレイ表示 01173 } 01174 if( strstr(webdata, "LINE_TRACE") != NULL ) { 01175 pc.printf("+++++++++++++++++LINET RACE+++++++++++++++++++++"); 01176 pc.printf("mode = LINE_TRACE\r\n"); 01177 if(trace_thread->get_state() == Thread::Deleted) { 01178 delete trace_thread; //ライントレーススレッド停止 01179 trace_thread = new Thread(trace); 01180 trace_thread -> set_priority(osPriorityHigh); 01181 } 01182 mode=LINE_TRACE; 01183 display(); // ディスプレイ表示 01184 } 01185 if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){ 01186 trace_thread->terminate(); 01187 } 01188 if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){ 01189 avoi_thread->terminate(); 01190 servo.pulsewidth_us(1450); // サーボを中央位置に戻す 01191 } 01192 sprintf(channel, "%d",linkID); 01193 if (strstr(webdata, "GET") != NULL) { 01194 servreq=1; 01195 } 01196 if (strstr(webdata, "POST") != NULL) { 01197 servreq=1; 01198 } 01199 webcounter++; 01200 sprintf(webcount, "%d",webcounter); 01201 } else { 01202 memset(webbuff, '\0', sizeof(webbuff)); 01203 esp.attach(&callback); 01204 weberror=1; 01205 } 01206 } 01207 // Starts and restarts webserver if errors detected. 01208 void startserver() 01209 { 01210 pc.printf("++++++++++ Resetting ESP ++++++++++\r\n"); 01211 strcpy(cmdbuff,"AT+RST\r\n"); 01212 timeout=8000; 01213 getcount=1000; 01214 SendCMD(); 01215 getreply(); 01216 pc.printf(replybuff); 01217 pc.printf("%d",ount); 01218 if (strstr(replybuff, "OK") != NULL) { 01219 pc.printf("\n++++++++++ Starting Server ++++++++++\r\n"); 01220 strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); // set multiple connections. 01221 timeout=500; 01222 getcount=20; 01223 SendCMD(); 01224 getreply(); 01225 pc.printf(replybuff); 01226 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port); 01227 timeout=500; 01228 getcount=20; 01229 SendCMD(); 01230 getreply(); 01231 pc.printf(replybuff); 01232 ThisThread::sleep_for(500); 01233 sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout); 01234 timeout=500; 01235 getcount=50; 01236 SendCMD(); 01237 getreply(); 01238 pc.printf(replybuff); 01239 ThisThread::sleep_for(5000); 01240 pc.printf("\n Getting Server IP \r\n"); 01241 strcpy(cmdbuff, "AT+CIFSR\r\n"); 01242 timeout=2500; 01243 getcount=200; 01244 while(weberror==0) { 01245 SendCMD(); 01246 getreply(); 01247 if (strstr(replybuff, "0.0.0.0") == NULL) { 01248 weberror=1; // wait for valid IP 01249 } 01250 } 01251 pc.printf("\n Enter WEB address (IP) found below in your browser \r\n\n"); 01252 pc.printf("\n The MAC address is also shown below,if it is needed \r\n\n"); 01253 replybuff[strlen(replybuff)-1] = '\0'; 01254 //char* IP = replybuff + 5; 01255 sprintf(webdata,"%s", replybuff); 01256 pc.printf(webdata); 01257 led2=1; 01258 bufflen=200; 01259 //bufflen=100; 01260 ount=0; 01261 pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n"); 01262 setup(); 01263 esp.attach(&callback); 01264 } else { 01265 pc.printf("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n"); 01266 led1=1; 01267 led2=1; 01268 led3=1; 01269 led4=1; 01270 while(1) { 01271 led1=!led1; 01272 led2=!led2; 01273 led3=!led3; 01274 led4=!led4; 01275 ThisThread::sleep_for(1000); 01276 } 01277 } 01278 time2.reset(); 01279 time2.start(); 01280 } 01281 // ESP Command data send 01282 void SendCMD() 01283 { 01284 esp.printf("%s", cmdbuff); 01285 } 01286 // Get Command and ESP status replies 01287 void getreply() 01288 { 01289 memset(replybuff, '\0', sizeof(replybuff)); 01290 time1.reset(); 01291 time1.start(); 01292 replycount=0; 01293 while(time1.read_ms()< timeout && replycount < getcount) { 01294 if(esp.readable()) { 01295 replybuff[replycount] = esp.getc(); 01296 replycount++; 01297 } 01298 } 01299 time1.stop(); 01300 } 01301 01302 /* mainスレッド */ 01303 int main() { 01304 /* 初期設定 */ 01305 wifi_thread = new Thread(wifi); 01306 wifi_thread -> set_priority(osPriorityHigh); 01307 avoi_thread = new Thread(avoidance); 01308 avoi_thread->terminate(); 01309 trace_thread = new Thread(trace); 01310 trace_thread->terminate(); 01311 mode = READY; // 現在待ちモード 01312 beforeMode = READY; // 前回待ちモード 01313 run = STOP; // 停止 01314 flag_sp = NORMAL; // スピード(普通) 01315 lcd.setBacklight(TextLCD::LightOn); // バックライトON 01316 lcd.setAddress(0,1); 01317 lcd.printf("Mode:SetUp"); 01318 //display(); // ディスプレイ表示 01319 01320 while(1){ 01321 bChange(); // バッテリー残量更新 01322 } 01323 }
Generated on Fri Jul 29 2022 06:09:55 by
1.7.2