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