denki
1
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 0:4c4775017e8a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Sep 23 01:45:03 2020 +0000
@@ -0,0 +1,1310 @@
+
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "ReceiverIR.h"
+#include "rtos.h"
+#include <stdint.h>
+#include "platform/mbed_thread.h"
+#include "TextLCD.h"
+
+RawSerial pc(USBTX, USBRX);
+
+/* マクロ定義、列挙型定義 */
+#define MIN_V 2.0 // 電圧の最小値
+#define MAX_V 2.67 // 電圧の最大値
+#define LOW 0 // モーターOFF
+#define HIGH 1 // モーターON
+#define NORMAL 0 // 普通
+#define FAST 1 // 速い
+#define VERYFAST 2 // とても速い
+
+#define SLOW 0.8
+#define REVERSE 0.5
+
+#define MBED04 0.781//Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
+#define MBED05 0.953//Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
+#define MSR0 0.6
+#define MSR1 0.65
+#define MSR2 0.7
+#define MSR3 0.75
+#define MSR4 0.8
+#define MSR5 0.85
+#define MSR6 0.9
+#define MSR7 0.95
+#define MSR8 1
+#define MSL0 MSR0
+#define MSL1 MSR1
+#define MSL2 MSR2//*MBED05
+#define MSL3 MSR3//*MBED05
+#define MSL4 MSR4//*MBED05
+#define MSL5 MSR5//*MBED05
+#define MSL6 MSR6//*MBED05
+#define MSL7 MSR7//*MBED05
+#define MSL8 MSR8//*MBED05
+
+/* 操作モード定義 */
+enum MODE{
+ READY = -1, // -1：待ち
+ ADVANCE = 1, // 1：前進
+ RIGHT, // 2：右折
+ LEFT, // 3：左折
+ BACK, // 4：後退
+ STOP, // 5：停止
+ LINE_TRACE, // 6：ライントレース
+ AVOIDANCE, // 7：障害物回避
+ SPEED, // 8：スピード制御
+};
+
+/* ピン配置 */
+ReceiverIR ir(p5); // リモコン操作
+DigitalOut trig(p6); // 超音波センサtrigger
+DigitalIn echo(p7); // 超音波センサecho
+DigitalIn ss1(p8); // ライントレースセンサ(左)
+DigitalIn ss2(p9); // ライントレースセンサ
+DigitalIn ss3(p10); // ライントレースセンサ
+DigitalIn ss4(p11); // ライントレースセンサ
+DigitalIn ss5(p12); // ライントレースセンサ(右)
+RawSerial esp(p13, p14); // Wi-Fiモジュール(tx, rx)
+AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
+PwmOut motorR2(p22); // 右モーター後退
+PwmOut motorR1(p21); // 右モーター前進
+PwmOut motorL2(p23); // 左モーター後退
+PwmOut motorL1(p24); // 左モーター前進
+PwmOut servo(p25); // サーボ
+I2C i2c_lcd(p28,p27); // LCD(tx, rx)
+
+
+
+/* 変数宣言 */
+int mode; // 操作モード
+int run; // 走行状態
+int beforeRun = STOP; // 前回の走行状態
+int beforeMode; // 前回のモード
+int flag_sp = 0; // スピード変化フラグ
+Timer viewTimer; // スピ―ド変更時に3秒計測タイマー
+
+float motorSpeedR1[9] = {MSR0, MSR1, MSR2, MSR3 , MSR4 , MSR5 , MSR6 , MSR7 , MSR8 };
+//float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6*REVERSE, MSR7*REVERSE, MSR8*REVERSE};
+float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3, MSR4, MSR5, MSR6, MSR7, MSR8};
+float motorSpeedL1[9] = {MSL0, MSL1, MSL2, MSL3 , MSL4 , MSL5 , MSL6 , MSL7 , MSL8 };
+//float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6*REVERSE, MSL7*REVERSE, MSL8*REVERSE};
+float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3, MSL4, MSL5, MSL6, MSL7, MSL8};
+ // モーター速度設定(後半はライントレース用)
+ // 0,1,2:基準速度
+ // 3,4,5:低速
+ // 6,7,8:高速
+ // R1 : 右前, R2 : 右後, L1 : 左前, L2 : 左後
+
+Mutex mutex; // ミューテックス
+
+/* decodeIR用変数 */
+RemoteIR::Format format;
+uint8_t buf[32];
+uint32_t bitcount;
+uint32_t code;
+
+/* bChange, lcdbacklight用変数 */
+TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
+int b = 0; // バッテリー残量
+int flag_b = 0; // バックライト点滅フラグ
+int flag_t = 0; // バックライトタイマーフラグ
+
+/* trace用変数 */
+int sensArray[32] = {0,6,2,4,1,2,1,4, // ライントレースセンサパターン
+ 3,6,1,6,1,1,1,6, // 0:前回動作継続
+ 7,1,7,1,3,1,1,1, // 1:高速前進
+ 5,1,7,1,5,1,7,1}; // 2:低速右折
+ // 3:低速左折
+ // 4:中速右折
+ // 5:中速左折
+ // 6:高速右折
+ // 7:高速左折
+
+//int sensArray[32] = {0,6,0,4,1,0,1,4, // ライントレースセンサパターン
+// 0,6,1,6,1,1,1,6, // 0:前回動作継続
+// 7,1,7,1,0,1,1,1, // 1:高速前進
+// 5,1,7,1,5,1,7,1}; // 2:低速右折
+// // 3:低速左折
+// // 4:中速右折
+// // 5:中速左折
+// // 6:高速右折
+// // 7:高速左折
+
+/* avoidance用変数 */
+Timer timer; // 距離計測用タイマ
+int DT; // 距離
+int SC; // 正面
+int SL; // 左
+int SR; // 右
+int SLD; // 左前
+int SRD; // 右前
+int flag_a = 0; // 障害物有無のフラグ
+const int limit = 17; // 障害物の距離のリミット(単位：cm)
+int far; // 最も遠い距離
+int houkou; // 進行方向(1:前 2:左 3:右)
+int t1 = 0;
+
+/*WiFi用変数*/
+Timer time1;
+Timer time2;
+int bufflen, DataRX, ount, getcount, replycount, servreq, timeout;
+int bufl, ipdLen, linkID, weberror, webcounter,click_flag;
+float R1=100000, R2=10000; // resistor values to give a 10:1 reduction of measured AnalogIn voltage
+char webcount[8];
+char type[16];
+char channel[2];
+char cmdbuff[32];
+char replybuff[1024];
+char webdata[1024]; // This may need to be bigger depending on WEB browser used
+char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added
+int port =80; // set server port
+int SERVtimeout =5; // set server timeout in seconds in case link breaks.
+char ssid[32] = "mbed05"; // enter WiFi router ssid inside the quotes
+char pwd [32] = "0123456789a"; // enter WiFi router password inside the quotes
+
+/* プロトタイプ宣言 */
+void decodeIR(/*void const *argument*/);
+void motor(/*void const *argument*/);
+void changeSpeed();
+void avoidance(/*void const *argument*/);
+void trace(/*void const *argument*/);
+void watchsurrounding3();
+void watchsurrounding5();
+int watch();
+char battery_ch[8];
+void bChange();
+void display();
+void lcdBacklight(void const *argument);
+void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck();
+void wifi(/*void const *argument*/);
+Thread *deco_thread; // decodeIRをスレッド化 ：+3
+Thread *wifi_thread;
+//wifi_thread(wifi,NULL,osPriorityHigh); // wifiをスレッド化
+Thread *motor_thread; // motorをスレッド化 ：+2
+RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定
+Thread *avoi_thread; // avoidanceをスレッド化：+2
+Thread *trace_thread; // traceをスレッド化 ：+2
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+void setup(){
+ deco_thread = new Thread(decodeIR);
+ deco_thread -> set_priority(osPriorityRealtime);
+ motor_thread = new Thread(motor);
+ motor_thread -> set_priority(osPriorityHigh);
+ //wifi_thread -> set_priority(osPriorityRealtime);
+ display();
+}
+
+/* リモコン受信スレッド */
+void decodeIR(/*void const *argument*/){
+ while(1){
+ // 受信待ち
+ if (ir.getState() == ReceiverIR::Received){ // コード受信
+ bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
+ if(bitcount > 1){ // 受信成功
+ code=0;
+ for(int j = 0; j < 4; j++){
+ code+=(buf[j]<<(8*(3-j)));
+ }
+ if(mode != SPEED){ // スピードモード以外なら
+ beforeMode=mode; // 前回のモードに現在のモードを設定
+ }
+ switch(code){
+ case 0x40bf27d8: // クイック
+ ////*-*-*-5("mode = SPEED\r\n");
+ mode = SPEED; // スピードモード
+ changeSpeed(); // 速度変更
+ display(); // ディスプレイ表示
+ mode = beforeMode; // 現在のモードに前回のモードを設定
+ break;
+ case 0x40be34cb: // レグザリンク
+ ////*-*-*-5("mode = LINE_TRACE\r\n");
+ if(trace_thread->get_state() == Thread::Deleted){
+ delete trace_thread;
+ trace_thread = new Thread(trace);
+ trace_thread -> set_priority(osPriorityHigh);
+ }
+ mode=LINE_TRACE; // ライントレースモード
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf6e91: // 番組表
+ ////*-*-*-5("mode = AVOIDANCE\r\n");
+ if(avoi_thread->get_state() == Thread::Deleted){
+ delete avoi_thread;
+ avoi_thread = new Thread(avoidance);
+ avoi_thread -> set_priority(osPriorityHigh);
+ }
+ flag_a = 0;
+ mode=AVOIDANCE; // 障害物回避モード
+ run = ADVANCE; // 前進
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf3ec1: // ↑
+ ////*-*-*-5("mode = ADVANCE\r\n");
+ mode = ADVANCE; // 前進モード
+ run = ADVANCE; // 前進
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf3fc0: // ↓
+ ////*-*-*-5("mode = BACK\r\n");
+ mode = BACK; // 後退モード
+ run = BACK; // 後退
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf5fa0: // ←
+ ////*-*-*-5("mode = LEFT\r\n");
+ mode = LEFT; // 左折モード
+ run = LEFT; // 左折
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf5ba4: // →
+ ////*-*-*-5("mode = RIGHT\r\n");
+ mode = RIGHT; // 右折モード
+ run = RIGHT; // 右折
+ display(); // ディスプレイ表示
+ break;
+ case 0x40bf3dc2: // 決定
+ ////*-*-*-5("mode = STOP\r\n");
+ mode = STOP; // 停止モード
+ run = STOP; // 停止
+ display(); // ディスプレイ表示
+ break;
+ default:
+ ;
+ }
+ if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
+ trace_thread->terminate();
+ }
+ if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
+ avoi_thread->terminate();
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ }
+ }
+ }
+ if(viewTimer.read_ms()>=3000){ // スピードモードのまま3秒経過
+ viewTimer.stop(); // タイマーストップ
+ viewTimer.reset(); // タイマーリセット
+ display(); // ディスプレイ表示
+ }
+ ThisThread::sleep_for(90); // 90ms待つ
+ }
+}
+
+/* モーター制御スレッド */
+void motor(/*void const *argument*/){
+ while(1){
+ /* 走行状態の場合分け */
+ switch(run){
+ /* 前進 */
+ case ADVANCE:
+ motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
+ motorL2 = LOW; // 左後退モーターOFF
+ break;
+ /* 右折 */
+ case RIGHT:
+ motorR1 = LOW; // 右前進モーターOFF
+ motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
+ motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
+ motorL2 = LOW; // 左後退モーターOFF
+
+ break;
+ /* 左折 */
+ case LEFT:
+ motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = LOW; // 左前進モーターOFF
+ motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
+
+ break;
+ /* 後退 */
+ case BACK:
+ motorR1 = LOW; // 右前進モーターOFF
+ motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
+ motorL1 = LOW; // 左前進モーターOFF
+ motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
+ break;
+ /* 停止 */
+ case STOP:
+ motorR1 = LOW; // 右前進モーターOFF
+ motorR2 = LOW; // 右後退モーターOFF
+ motorL1 = LOW; // 左前進モーターOFF
+ motorL2 = LOW; // 左後退モーターOFF
+ break;
+ }
+ if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら
+ flag_sp %= 3; // スピード変更フラグ調整
+ }
+ ThisThread::sleep_for(3); // 30ms待つ
+ }
+}
+
+/* スピード変更関数 */
+void changeSpeed(){
+ // それ以外
+ flag_sp = flag_sp + 3; // スピード変更フラグを+1
+ if(flag_sp>6)flag_sp=0;
+}
+
+/* ライントレーススレッド */
+void trace(){
+ while(1){
+ /* 各センサー値読み取り */
+ int sensor1 = ss1;
+ int sensor2 = ss2;
+ int sensor3 = ss3;
+ int sensor4 = ss4;
+ int sensor5 = ss5;
+ ////*-*-*-5("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5);
+ int sensD = 0;
+ /* センサー値の決定 */
+ if(sensor1 > 0) sensD |= 0x10;
+ if(sensor2 > 0) sensD |= 0x08;
+ if(sensor3 > 0) sensD |= 0x04;
+ if(sensor4 > 0) sensD |= 0x02;
+ if(sensor5 > 0) sensD |= 0x01;
+ pc.printf("%d \r\n",sensArray[sensD]);
+ /* センサー値によって場合分け */
+ switch(sensArray[sensD]){
+ case 1:
+ flag_sp = 6;
+ beforeRun = run;
+ run = ADVANCE; // 高速で前進
+ break;
+ case 2:
+ flag_sp = 2;
+ beforeRun = run;
+ run = RIGHT;
+ break;
+ case 3:
+ flag_sp = 2;
+ beforeRun = run;
+ run = LEFT;
+ break;
+ case 4:
+ flag_sp = 0;
+ beforeRun = run;
+ run = RIGHT;
+ break;
+ case 5:
+ flag_sp = 0;
+ beforeRun = run;
+ run = LEFT;
+ break;
+ case 6:
+ flag_sp = 4;
+ beforeRun = run;
+ run = RIGHT;
+ break;
+ case 7:
+ flag_sp = 4;
+ beforeRun = run;
+ run = LEFT;
+ break;
+ default:
+ break;
+ }
+// ThisThread::sleep_for(30); // 30ms待つ
+ ThisThread::sleep_for(3);
+ }
+}
+
+/* 障害物回避走行スレッド */
+void avoidance(){
+ int i;
+ watchsurrounding3();
+ while(1){
+ if(flag_a==0){run=ADVANCE;watchsurrounding3();}
+ else{
+ i=0;
+ watchsurrounding5();
+ if(SLD>=SC&&SRD>=SC){
+ if(SL>=SR){
+ run = BACK;
+ ThisThread::sleep_for(200-flag_sp*10);
+
+ run = LEFT;
+ ThisThread::sleep_for(480-flag_sp*25);
+ run=STOP;
+ }
+ if(SL<SR){
+ run = BACK;
+ ThisThread::sleep_for(200-flag_sp*10);
+ run = RIGHT;
+ ThisThread::sleep_for(480-flag_sp*25);
+ run=STOP;}
+ }
+ if(SLD<SC){
+ run = BACK;
+ ThisThread::sleep_for(200-flag_sp*10);
+ run = RIGHT;
+ ThisThread::sleep_for(250-flag_sp*10);
+ run=STOP;
+ }
+ if(SRD<SC){
+ run = BACK;
+ ThisThread::sleep_for(200-flag_sp*10);
+ run = LEFT;
+ ThisThread::sleep_for(250-flag_sp*10);
+ run=STOP;
+ }
+
+ flag_a=0;
+
+}
+}
+}
+/* 距離計測関数 */
+int watch(){
+ do{
+ trig = 0;
+ ThisThread::sleep_for(5); // 5ms待つ
+ trig = 1;
+ ThisThread::sleep_for(15); // 15ms待つ
+ trig = 0;
+ timer.start();
+ t1=timer.read_ms();
+ while(echo.read() == 0 && t1<10){
+ t1=timer.read_ms();
+ led1 = 1;
+ }
+ timer.stop();
+ timer.reset();
+ /*if((t1-t2) >= 10){
+ run = STOP;*/
+ }while(t1 >= 10);
+ timer.start(); // 距離計測タイマースタート
+ while(echo.read() == 1){
+ }
+ timer.stop(); // 距離計測タイマーストップ
+ DT = (int)(timer.read_us()*0.01657); // 距離計算
+ if(DT > 1000){
+ DT = -1;
+ }else if(DT > 150){ // 検知範囲外なら100cmに設定
+ DT = 150;
+ }
+ timer.reset(); // 距離計測タイマーリセット
+ led1 = 0;
+ return DT;
+}
+
+/* 障害物検知関数 */
+void watchsurrounding3(){
+ //servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ //ThisThread::sleep_for(200); // 100ms待つ
+ SC = watch();
+ if(SC < limit){ // 正面20cm以内に障害物がある場合
+ if(SC!=-1){
+ run = STOP; // 停止
+ flag_a = 1;
+ return;
+ }
+ }
+ servo.pulsewidth_us(1925); // サーボを左に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SLD = watch();
+ if(SLD < limit){ // 左前20cm以内に障害物がある場合
+ run = STOP; // 停止
+ flag_a = 1;
+ return;
+ }
+ servo.pulsewidth_us(1450);
+ ThisThread::sleep_for(150);
+ SC = watch();
+ if(SC < limit){
+ if(SC!=-1){
+ run = STOP; // 停止
+ flag_a = 1;
+ return;
+ }
+ }
+ servo.pulsewidth_us(925); // サーボを右に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SRD = watch();
+ if(SRD < limit){ // 右前20cm以内に障害物がある場合
+ run = STOP; // 停止
+ flag_a = 1;
+ return;
+ }
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ ThisThread::sleep_for(100); // 100ms待つ
+ /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
+ flag_a = 1; // 障害物有無フラグに1をセット
+ }*/
+}
+
+/* 障害物検知関数 */
+void watchsurrounding5(){
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ ThisThread::sleep_for(200); // 100ms待つ
+ SC = watch();
+ servo.pulsewidth_us(1925); // サーボを左に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SLD = watch();
+ servo.pulsewidth_us(2400); // サーボを左に90度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SL = watch();
+ servo.pulsewidth_us(1450);
+ ThisThread::sleep_for(250);
+ SC = watch();
+ servo.pulsewidth_us(925); // サーボを右に40度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SRD = watch();
+ servo.pulsewidth_us(500); // サーボを右に90度回転
+ ThisThread::sleep_for(100); // 250ms待つ
+ SR = watch();
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ ThisThread::sleep_for(250); // 100ms待つ
+}
+
+/* ディスプレイ表示関数 */
+void display(){
+ mutex.lock(); // ミューテックスロック
+ lcd.setAddress(0,1);
+
+ /* 操作モードによる場合分け */
+ switch(mode){
+ /* 前進 */
+ case ADVANCE:
+ lcd.printf("Mode:Advance ");
+ break;
+ /* 右折 */
+ case RIGHT:
+ lcd.printf("Mode:TurnRight ");
+ break;
+ /* 左折 */
+ case LEFT:
+ lcd.printf("Mode:TurnLeft ");
+ break;
+ /* 後退 */
+ case BACK:
+ lcd.printf("Mode:Back ");
+ break;
+ /* 停止 */
+ case STOP:
+ lcd.printf("Mode:Stop ");
+ break;
+ /* 待ち */
+ case READY:
+ lcd.printf("Mode:Ready ");
+ break;
+ /* ライントレース */
+ case LINE_TRACE:
+ lcd.printf("Mode:LineTrace ");
+ break;
+ /* 障害物回避 */
+ case AVOIDANCE:
+ lcd.printf("Mode:Avoidance ");
+ break;
+ /* スピード制御 */
+ case SPEED:
+ /* スピードの状態で場合分け */
+ switch(flag_sp % 3){
+ /* 普通 */
+ case(NORMAL):
+ lcd.printf("Speed:Normal ");
+ break;
+ /* 速い */
+ case(FAST):
+ lcd.printf("Speed:Fast ");
+ break;
+ /* とても速い */
+ case(VERYFAST):
+ lcd.printf("Speed:VeryFast ");
+ break;
+ }
+ viewTimer.reset(); // タイマーリセット
+ viewTimer.start(); // タイマースタート
+ break;
+ }
+ mutex.unlock(); // ミューテックスアンロック
+}
+
+/* バックライト点滅 */
+void lcdBacklight(void const *argument){
+ if(flag_b == 1){ // バックライト点滅フラグが1なら
+ lcd.setBacklight(TextLCD::LightOn); // バックライトON
+ }else{ // バックライト点滅フラグが0なら
+ lcd.setBacklight(TextLCD::LightOff); // バックライトOFF
+ }
+ flag_b = !flag_b; // バックライト点滅フラグ切り替え
+}
+
+/* バッテリー残量更新関数 */
+void bChange(){
+ ////*-*-*-5(" bChange1\r\n");
+ b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
+ if(b <= 0){ // バッテリー残量0%なら全ての機能停止
+ b = 0;
+ //lcd.setBacklight(TextLCD::LightOff);
+ //run = STOP;
+ //exit(1); // 電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。
+ }
+ mutex.lock(); // ミューテックスロック
+ lcd.setAddress(0,0);
+ lcd.printf("Battery:%3d%%",b); // バッテリー残量表示
+ mutex.unlock(); // ミューテックスアンロック
+ if(b <= 30){ // バッテリー残量30%以下なら
+ if(flag_t == 0){ // バックライトタイマーフラグが0なら
+ //bTimer.attach(lcdBacklight,0.5);
+ bTimer.start(500); // 0.5秒周期でバックライトタイマー割り込み
+ flag_t = 1; // バックライトタイマーフラグを1に切り替え
+ }
+ }else{
+ if(flag_t == 1){ // バックライトタイマーフラグが1なら
+ //bTimer.detach();
+ bTimer.stop(); // バックライトタイマーストップ
+ lcd.setBacklight(TextLCD::LightOn); // バックライトON
+ flag_t = 0; // バックライトタイマーフラグを0に切り替え
+ }
+ }
+}
+// Serial Interrupt read ESP data
+void callback()
+{
+ ////*-*-*-5("\n\r------------ callback is being called --------------\n\r");
+ led3=1;
+ while (esp.readable()) {
+ webbuff[ount] = esp.getc();
+ ount++;
+ }
+ if(strlen(webbuff)>bufflen) {
+// //*-*-*-5("\f\n\r------------ webbuff over bufflen --------------\n\r");
+ DataRX=1;
+ led3=0;
+ }
+}
+
+void wifi(/*void const *argument*/)
+{
+ //*-*-*-5("\f\n\r------------ ESP8266 Hardware Reset psq --------------\n\r");
+ ThisThread::sleep_for(100);
+ led1=1,led2=0,led3=0;
+ timeout=6000;
+ getcount=500;
+ getreply();
+ esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
+ startserver();
+
+ while(1) {
+ if(DataRX==1) {
+ //*-*-*-5("\f\n\r------------ main while > if --------------\n\r");
+ click_flag = 1;
+ ReadWebData();
+ //*-*-*-5("\f\n\r------------ click_flag=%d --------------\n\r",click_flag);
+ //if ((servreq == 1 && weberror == 0) && click_flag == 1) {
+ if (servreq == 1 && weberror == 0) {
+ //*-*-*-5("\f\n\r------------ befor send page --------------\n\r");
+ sendpage();
+ }
+ //*-*-*-5("\f\n\r------------ send_check begin --------------\n\r");
+
+ //sendcheck();
+ //*-*-*-5("\f\n\r------------ ssend_check end--------------\n\r");
+
+ esp.attach(&callback);
+ //*-*-*-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);
+ //*-*-*-5("\n\n HTTP Packet: \n\n%s\n", webdata);
+ //*-*-*-5(" Web Characters sent : %d\n\n", bufl);
+ //*-*-*-5(" -------------------------------------\n\n");
+ servreq=0;
+ }
+ ThisThread::sleep_for(100);
+ }
+}
+// Static WEB page
+void sendpage()
+{
+// WEB page data
+
+ strcpy(webbuff, "<!DOCTYPE html>");
+ strcat(webbuff, "<html><head><title>RobotCar</title><meta name='viewport' content='width=device-width'/>");
+ //strcat(webbuff, "<meta http-equiv=\"refresh\" content=\"5\"; >");
+ 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>");
+ strcat(webbuff, "</head><body><center><p><strong>Robot Car Remote Controller");
+ strcat(webbuff, "</strong></p><td style='vertical-align:top;'><strong>Battery level ");
+ if(b > 30) { //残電量表示
+ sprintf(webbuff, "%s%3d", webbuff, b);
+ } else { //30%より下の場合残電量を赤文字
+ strcat(webbuff, "<font color=\"red\">");
+ sprintf(webbuff, "%s%3d", webbuff, b);
+ strcat(webbuff, "</font>");
+ }
+ strcat(webbuff, "%</strong>");
+ strcat(webbuff, "<button id=\"reloadbtn\" type=\"button\" class=\"load\" onclick=\"location.reload()\">RELOAD</button>");
+ strcat(webbuff, "</td></p>");
+ strcat(webbuff, "<br>");
+ strcat(webbuff, "<table><tr><td></td><td>");
+
+ switch(mode) { //ブラウザ更新時の現在の車の状態からボタンの点灯判定
+ case ADVANCE: //前進
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"light\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case LEFT: //左折
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"light\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case STOP: //停止
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"light\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case RIGHT: //右折
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"light\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case BACK: //後進
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"light\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr><td>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case AVOIDANCE: //障害物回避
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"light\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ case LINE_TRACE: //ライントレース
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"light\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ default: //その他
+ strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
+ strcat(webbuff, "</button></td><td></td></tr><tr><td>");
+ strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
+ strcat(webbuff, "</button></td></tr><td></td><td>");
+ strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
+ strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
+ strcat(webbuff, "<strong>Mode</strong>");
+ strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
+ strcat(webbuff, "AVOIDANCE</button></td><td>");
+ strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
+ break;
+ }
+ strcat(webbuff, "</button></td></tr></table>");
+ strcat(webbuff, "<strong>Speed</strong>");
+ strcat(webbuff, "<table><tr><td>");
+ //ready示速度だけ点灯
+ switch (flag_sp) { //現在の速度のボタン表示
+ case 0: //ノーマル
+ strcat(webbuff, "<button id='sp1btn' type='button' class=\"light\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
+ break;
+ case 1: //ファスト
+ strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp2btn' type='button' class=\"light\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
+ break;
+ case 2: //ベリーファスト
+ strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp3btn' type='button' class=\"light\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
+ break;
+ default: //その他
+ strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
+ strcat(webbuff, "</button></td><td>");
+ strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
+ break;
+ }
+ strcat(webbuff, "</button></td></tr></table>");
+
+ strcat(webbuff, "</center>");
+ strcat(webbuff, "</body>");
+ strcat(webbuff, "</html>");
+
+ strcat(webbuff, "<script language=\"javascript\" type=\"text/javascript\">"); //機能
+
+ strcat(webbuff, "var button_9 = document.getElementsByTagName(\"button\");");
+
+//ボタン入力時それぞれの関数から呼び出されサーバーとの通信を行う
+ strcat(webbuff, "function htmlacs(url) {");
+ strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=true;}");
+ strcat(webbuff, "var xhr = new XMLHttpRequest();");
+ strcat(webbuff, "xhr.open(\"GET\", url);");
+ strcat(webbuff, "xhr.onreadystatechange = function(){");
+ strcat(webbuff, "if(this.readyState == 4 || this.status == 200){");
+ strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=false;}");
+ strcat(webbuff, "}");
+ strcat(webbuff, "};");
+ strcat(webbuff, "xhr.send(\"\");");
+ strcat(webbuff, "}");
+
+//mode変更ボタン入力時動作 //sendmes
+ strcat(webbuff, "function send_mes(btnval){"); //mode変更ボタン入力時の点灯消灯判定
+ strcat(webbuff, "console.log(btnval);");
+ strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
+ strcat(webbuff, "htmlacs(url);");
+ strcat(webbuff, "console.log(url);");
+ strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
+ strcat(webbuff, "for(var i=1;i<8;i++){");
+ strcat(webbuff, "if(buttons[i].value == btnval){");
+ strcat(webbuff, "buttons[i].className=\"light\";");
+ strcat(webbuff, "}else{");
+ strcat(webbuff, "buttons[i].className=\"noselect\";");
+ strcat(webbuff, "}");
+ strcat(webbuff, "}");
+ strcat(webbuff, "}");
+
+ strcat(webbuff, "function send_mes_spe(btnval){"); //speed変更ボタン入力時の点灯消灯判定
+ strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
+ strcat(webbuff, "htmlacs(url);");
+ strcat(webbuff, "console.log(url);");
+ strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
+ strcat(webbuff, "for(var i=8;i<11;i++){");
+ strcat(webbuff, "if(buttons[i].value == btnval){");
+ strcat(webbuff, "buttons[i].className=\"light\";");
+ strcat(webbuff, "}else{");
+ strcat(webbuff, "buttons[i].className=\"noselect\";");
+ strcat(webbuff, "}");
+ strcat(webbuff, "}");
+ strcat(webbuff, "}");
+ strcat(webbuff, "</script>");
+
+
+// end of WEB page data
+ bufl = strlen(webbuff); // get total page buffer length
+ //sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length.
+
+ sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl>2048?2048:bufl)); // send IPD link channel and buffer character length.
+ timeout=500;
+ getcount=40;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ ////*-*-*-5("\n++++++++++ AT+CIPSENDBUF=%d,%d+++++++++\r\n", linkID, (bufl>2048?2048:bufl));
+
+ //*-*-*-5("\n++++++++++ bufl is %d ++++++++++\r\n",bufl);
+
+ //pastthrough mode
+ SendWEB(); // send web page
+ //*-*-*-5("\n++++++++++ webbuff clear ++++++++++\r\n");
+
+ memset(webbuff, '\0', sizeof(webbuff));
+ sendcheck();
+}
+
+// Large WEB buffer data send
+void SendWEB()
+{
+ int i=0;
+ if(esp.writeable()) {
+ while(webbuff[i]!='\0') {
+ esp.putc(webbuff[i]);
+
+ //****
+ //output at command when 2000
+ if(((i%2047)==0) && (i>0)) {
+ //wait_ms(10);
+ ThisThread::sleep_for(100);
+ sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); // send IPD link channel and buffer character length.
+ ////*-*-*-5("\r\n++++++++++ AT+CIPSENDBUF=%d,%d ++++++++++\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048));
+ timeout=600;
+ getcount=50;
+ SendCMD();
+ getreply();
+ ////*-*-*-5(replybuff);
+ ////*-*-*-5("\r\n+++++++++++++++++++\r\n");
+ }
+ //****
+ i++;
+ ////*-*-*-5("%c",webbuff[i]);
+ }
+ }
+ //*-*-*-5("\n++++++++++ send web i= %dinfo ++++++++++\r\n",i);
+}
+
+
+void sendcheck()
+{
+ weberror=1;
+ timeout=500;
+ getcount=24;
+ time2.reset();
+ time2.start();
+
+ /*
+ while(weberror==1 && time2.read() <5) {
+ getreply();
+ if (strstr(replybuff, "SEND OK") != NULL) {
+ weberror=0; // wait for valid SEND OK
+ }
+ }
+ */
+ if(weberror==1) { // restart connection
+ strcpy(cmdbuff, "AT+CIPMUX=1\r\n");
+ timeout=500;
+ getcount=10;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
+ timeout=500;
+ getcount=10;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ } else {
+ sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ }
+ time2.reset();
+}
+
+// Reads and processes GET and POST web data
+void ReadWebData()
+{
+ //*-*-*-5("+++++++++++++++++Read Web Data+++++++++++++++++++++\r\n");
+ ThisThread::sleep_for(200);
+ esp.attach(NULL);
+ ount=0;
+ DataRX=0;
+ weberror=0;
+ memset(webdata, '\0', sizeof(webdata));
+ int x = strcspn (webbuff,"+");
+ if(x) {
+ strcpy(webdata, webbuff + x);
+ weberror=0;
+ int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type);
+ //int i=0;
+ //*-*-*-5("+++++++++++++++++succed rec begin+++++++++++++++++++++\r\n");
+ //*-*-*-5("%s",webdata);
+ //*-*-*-5("+++++++++++++++++succed rec end+++++++++++++++++++++\r\n");
+ if( strstr(webdata, "Normal") != NULL ) {
+ //*-*-*-5("++++++++++++++++++Normal++++++++++++++++++++");
+ mode = SPEED; // スピードモード
+ flag_sp = 0;
+ display(); // ディスプレイ表示
+ mode = beforeMode; // 現在のモードに前回のモードを設定
+ }else if( strstr(webdata, "VeryFast") != NULL ) {
+ //*-*-*-5("+++++++++++++++++++VeryFast+++++++++++++++++++");
+ mode = SPEED; // スピードモード
+ flag_sp = 2;
+ display(); // ディスプレイ表示
+ mode = beforeMode; // 現在のモードに前回のモードを設定
+ }else if( strstr(webdata, "Fast") != NULL ) {
+ //*-*-*-5("++++++++++++++++++++Fast++++++++++++++++++");
+ mode = SPEED; // スピードモード
+ flag_sp = 1;
+ display(); // ディスプレイ表示
+ mode = beforeMode; // 現在のモードに前回のモードを設定
+ }else{
+ beforeMode = mode;
+ }
+ if( strstr(webdata, "GO") != NULL ) {
+ //*-*-*-5("+++++++++++++++++前進+++++++++++++++++++++\r\n");
+ //delete avoi_thread; //障害物回避スレッド停止
+ //delete trace_thread; //ライントレーススレッド停止
+ run = ADVANCE; // 前進
+ mode = ADVANCE; // モード変更
+ display(); // ディスプレイ表示
+ }
+
+ if( strstr(webdata, "LEFT") != NULL ) {
+ //*-*-*-5("+++++++++++++++++左折+++++++++++++++++++++\r\n");
+ //delete avoi_thread; //障害物回避スレッド停止
+ //delete trace_thread; //ライントレーススレッド停止
+ run = LEFT; // 左折
+ mode = LEFT; // モード変更
+ display(); // ディスプレイ表示
+ }
+
+ if( strstr(webdata, "STOP") != NULL ) {
+ //*-*-*-5("+++++++++++++++++停止+++++++++++++++++++++\r\n");
+ //delete avoi_thread; //障害物回避スレッド停止
+ //delete trace_thread; //ライントレーススレッド停止
+ run = STOP; // 停止
+ mode = STOP; // モード変更
+ display(); // ディスプレイ表示
+ }
+
+ if( strstr(webdata, "RIGHT") != NULL ) {
+ //*-*-*-5("+++++++++++++++++右折+++++++++++++++++++++\r\n");
+ //delete avoi_thread; //障害物回避スレッド停止
+ //delete trace_thread; //ライントレーススレッド停止
+ run = RIGHT; // 右折
+ mode = RIGHT; // モード変更
+ display(); // ディスプレイ表示
+ }
+
+ if( strstr(webdata, "BACK") != NULL ) {
+ //*-*-*-5("+++++++++++++++++後進+++++++++++++++++++++\r\n");
+ //delete avoi_thread; //障害物回避スレッド停止
+ //delete trace_thread; //ライントレーススレッド停止
+ run = BACK; // 後進
+ mode = BACK; // モード変更
+ display(); // ディスプレイ表示
+ }
+ //*-*-*-5("+++++++++++++++++succed+++++++++++++++++++++");
+
+ if( strstr(webdata, "AVOIDANCE") != NULL ) {
+ //*-*-*-5("+++++++++++++++++AVOIDANCE+++++++++++++++++++++");
+ if(avoi_thread->get_state() == Thread::Deleted) {
+ delete avoi_thread; //障害物回避スレッド停止
+ avoi_thread = new Thread(avoidance);
+ avoi_thread -> set_priority(osPriorityHigh);
+ }
+ mode=AVOIDANCE;
+ run = ADVANCE;
+ display(); // ディスプレイ表示
+ }
+ if( strstr(webdata, "LINE_TRACE") != NULL ) {
+ //*-*-*-5("+++++++++++++++++LINET RACE+++++++++++++++++++++");
+ //*-*-*-5("mode = LINE_TRACE\r\n");
+ if(trace_thread->get_state() == Thread::Deleted) {
+ delete trace_thread; //ライントレーススレッド停止
+ trace_thread = new Thread(trace);
+ trace_thread -> set_priority(osPriorityHigh);
+ }
+ mode=LINE_TRACE;
+ display(); // ディスプレイ表示
+ }
+ if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
+ trace_thread->terminate();
+ }
+ if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
+ avoi_thread->terminate();
+ servo.pulsewidth_us(1450); // サーボを中央位置に戻す
+ }
+ sprintf(channel, "%d",linkID);
+ if (strstr(webdata, "GET") != NULL) {
+ servreq=1;
+ }
+ if (strstr(webdata, "POST") != NULL) {
+ servreq=1;
+ }
+ webcounter++;
+ sprintf(webcount, "%d",webcounter);
+ } else {
+ memset(webbuff, '\0', sizeof(webbuff));
+ esp.attach(&callback);
+ weberror=1;
+ }
+}
+// Starts and restarts webserver if errors detected.
+void startserver()
+{
+ //*-*-*-5("++++++++++ Resetting ESP ++++++++++\r\n");
+ strcpy(cmdbuff,"AT+RST\r\n");
+ timeout=8000;
+ getcount=1000;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ //*-*-*-5("%d",ount);
+ if (strstr(replybuff, "OK") != NULL) {
+ //*-*-*-5("\n++++++++++ Starting Server ++++++++++\r\n");
+ strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); // set multiple connections.
+ timeout=500;
+ getcount=20;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
+ timeout=500;
+ getcount=20;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ ThisThread::sleep_for(500);
+ sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout);
+ timeout=500;
+ getcount=50;
+ SendCMD();
+ getreply();
+ //*-*-*-5(replybuff);
+ ThisThread::sleep_for(5000);
+ //*-*-*-5("\n Getting Server IP \r\n");
+ strcpy(cmdbuff, "AT+CIFSR\r\n");
+ timeout=2500;
+ getcount=200;
+ while(weberror==0) {
+ SendCMD();
+ getreply();
+ if (strstr(replybuff, "0.0.0.0") == NULL) {
+ weberror=1; // wait for valid IP
+ }
+ }
+ //*-*-*-5("\n Enter WEB address (IP) found below in your browser \r\n\n");
+ //*-*-*-5("\n The MAC address is also shown below,if it is needed \r\n\n");
+ replybuff[strlen(replybuff)-1] = '\0';
+ //char* IP = replybuff + 5;
+ sprintf(webdata,"%s", replybuff);
+ //*-*-*-5(webdata);
+ led2=1;
+ bufflen=200;
+ //bufflen=100;
+ ount=0;
+ //*-*-*-5("\n\n++++++++++ Ready ++++++++++\r\n\n");
+ setup();
+ esp.attach(&callback);
+ } else {
+ //*-*-*-5("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n");
+ led1=1;
+ led2=1;
+ led3=1;
+ led4=1;
+ while(1) {
+ led1=!led1;
+ led2=!led2;
+ led3=!led3;
+ led4=!led4;
+ ThisThread::sleep_for(1000);
+ }
+ }
+ time2.reset();
+ time2.start();
+}
+
+// ESP Command data send
+void SendCMD()
+{
+ esp.printf("%s", cmdbuff);
+}
+// Get Command and ESP status replies
+void getreply()
+{
+ memset(replybuff, '\0', sizeof(replybuff));
+ time1.reset();
+ time1.start();
+ replycount=0;
+ while(time1.read_ms()< timeout && replycount < getcount) {
+ if(esp.readable()) {
+ replybuff[replycount] = esp.getc();
+ replycount++;
+ }
+ }
+ time1.stop();
+}
+
+/* mainスレッド */
+int main() {
+ /* 初期設定 */
+ //wifi_thread = new Thread(wifi);
+ //wifi_thread -> set_priority(osPriorityHigh);
+ motorR2.period_us(40);
+ motorR1.period_us(40);
+ motorL2.period_us(40);
+ motorL1.period_us(40);
+ setup();
+ avoi_thread = new Thread(avoidance);
+ avoi_thread->terminate();
+ trace_thread = new Thread(trace);
+ trace_thread->terminate();
+ mode = READY; // 現在待ちモード
+ beforeMode = READY; // 前回待ちモード
+ run = STOP; // 停止
+ flag_sp = NORMAL; // スピード(普通)
+ lcd.setBacklight(TextLCD::LightOn); // バックライトON
+ lcd.setAddress(0,1);
+ lcd.printf("Mode:ready");
+// display(); // ディスプレイ表示
+
+ while(1){
+ bChange(); // バッテリー残量更新
+ }
+}
\ No newline at end of file