File content as of revision 37:0cde453dce7d:

/* 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    // とても速い

/* 操作モード定義 */
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(p21);   // 右モーター後退
PwmOut      motorR1(p22);   // 右モーター前進
PwmOut      motorL2(p23);   // 左モーター後退
PwmOut      motorL1(p24);   // 左モーター前進
PwmOut      servo(p25);     // サーボ
I2C         i2c_lcd(p28,p27);   // LCD(tx, rx)

/* 変数宣言 */
int mode;                   // 操作モード
int run;                    // 走行状態
int beforeMode;             // 前回のモード
int flag_sp = 0;            // スピード変化フラグ
Timer viewTimer;            // スピ―ド変更時に3秒計測タイマー
float motorSpeed[9] = {0.5, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0};
                            // モーター速度設定(後半はライントレース用)
                            
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] = {8,6,2,4,1,1,2,2,   // ライントレースセンサパターン
                     3,1,1,1,3,1,1,2,
                     7,1,1,1,1,1,1,1,
                     5,1,1,1,3,1,3,1};

/* avoidance用変数 */
Timer timer;            // 距離計測用タイマ
int DT;                 // 距離
int SC;                 // 正面   
int SL;                 // 左
int SR;                 // 右
int SLD;                // 左前
int SRD;                // 右前
int flag_a = 0;         // 障害物有無のフラグ
const int limit = 20;   // 障害物の距離のリミット(単位：cm)
int far;                // 最も遠い距離
int houkou;             // 進行方向(1:前 2:左 3:右)
int i;                  // ループ変数
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] = "mbed02"; // 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 watchsurrounding();
int watch();
void bChange();
void display();
void lcdBacklight(void const *argument);
void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck(),touchuan();
void wifi(void const *argument);
Thread *deco_thread;                                      // decodeIRをスレッド化 ：+3
Thread wifi_thread(wifi,NULL,osPriorityRealtime);         // wifiをスレッド化
Thread *motor_thread;                                     // motorをスレッド化    ：+2
//Thread avoi_thread(avoidance, NULL, osPriorityHigh);    // avoidanceをスレッド化：+2
//Thread trace_thread(trace, NULL, osPriorityHigh);       // traceをスレッド化    ：+2
RtosTimer bTimer(lcdBacklight, osTimerPeriodic);          // lcdBacklightをタイマー割り込みで設定
Thread *avoi_thread;
Thread *trace_thread;

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);
    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:    // クイック
                        //pc.printf("mode = SPEED\r\n");
                        mode = SPEED;       // スピードモード 
                        changeSpeed();      // 速度変更
                        display();          // ディスプレイ表示
                        mode = beforeMode;  // 現在のモードに前回のモードを設定
                        break;
                    case 0x40be34cb:    // レグザリンク
                        //pc.printf("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:    // 番組表
                        //pc.printf("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:    // ↑
                        //pc.printf("mode = ADVANCE\r\n");
                        mode = ADVANCE;     // 前進モード
                        run = ADVANCE;      // 前進
                        display();          // ディスプレイ表示
                        break;
                    case 0x40bf3fc0:    // ↓
                        //pc.printf("mode = BACK\r\n");
                        mode = BACK;        // 後退モード
                        run = BACK;         // 後退
                        display();          // ディスプレイ表示
                        break;
                    case 0x40bf5fa0:    // ←
                        //pc.printf("mode = LEFT\r\n");
                        mode = LEFT;        // 左折モード
                        run = LEFT;         // 左折
                        display();          // ディスプレイ表示
                        break;
                    case 0x40bf5ba4:    // →
                        //pc.printf("mode = RIGHT\r\n");
                        mode = RIGHT;       // 右折モード
                        run = RIGHT;        // 右折
                        display();          // ディスプレイ表示
                        break;
                    case 0x40bf3dc2:    // 決定
                        //pc.printf("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 = motorSpeed[flag_sp];  // 右前進モーターON
                motorR2 = LOW;                  // 右後退モーターOFF
                motorL1 = motorSpeed[flag_sp];  // 左前進モーターON
                motorL2 = LOW;                  // 左後退モーターOFF
                break;
            /* 右折 */
            case RIGHT:
                motorR1 = LOW;                  // 右前進モーターOFF
                motorR2 = motorSpeed[flag_sp];  // 右後退モーターON
                motorL1 = motorSpeed[flag_sp];  // 左前進モーターON
                motorL2 = LOW;                  // 左後退モーターOFF
                break;
            /* 左折 */
            case LEFT:
                motorR1 = motorSpeed[flag_sp];  // 右前進モーターON
                motorR2 = LOW;                  // 右後退モーターOFF
                motorL1 = LOW;                  // 左前進モーターOFF
                motorL2 = motorSpeed[flag_sp];  // 左後退モーターON
                break;
            /* 後退 */
            case BACK:
                motorR1 = LOW;                  // 右前進モーターOFF
                motorR2 = motorSpeed[flag_sp];  // 右後退モーターON
                motorL1 = LOW;                  // 左前進モーターOFF
                motorL2 = motorSpeed[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 * (flag_sp / 3);   // スピード変更フラグ調整
        }
        ThisThread::sleep_for(30);          // 30ms待つ
    }
}

/* スピード変更関数 */
void changeSpeed(){
    if(flag_sp%3 == 2){         // スピード変更フラグを3で割った余りが2なら
        flag_sp -= 2;           // スピード変更フラグを-2
        
    }else{                      // それ以外
        flag_sp = flag_sp + 1;  // スピード変更フラグを+1 
    }  
}

/* ライントレーススレッド */
void trace(){
    while(1){ 
            /* 各センサー値読み取り */
            int sensor1 = ss1;
            int sensor2 = ss2;
            int sensor3 = ss3;
            int sensor4 = ss4;
            int sensor5 = ss5;
            pc.printf("%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;
            
            /* センサー値によって場合分け */
            switch(sensArray[sensD]){
                case 1:
                    run = ADVANCE;      // 低速で前進
                    break;
                case 2:
                    flag_sp = flag_sp % 3 + 6;  
                    run = RIGHT;        // 低速で右折
                    break;
                case 3:
                    flag_sp = flag_sp % 3 + 6;  
                    run = LEFT;         // 低速で左折
                    break;
                case 4:
                    flag_sp = flag_sp % 3 + 6;   
                    run = RIGHT;        // 中速で右折
                    break;
                case 5:
                    flag_sp = flag_sp % 3 + 6;
                    run = LEFT;         // 中速で左折
                    break;
                case 6:
                    flag_sp = flag_sp % 3 + 6;
                    run = RIGHT;        // 高速で右折
                    break;
                case 7:
                    flag_sp = flag_sp % 3 + 6;
                    run = LEFT;         // 高速で左折
                    break;
                case 8:
                    break;
            }
            ThisThread::sleep_for(30);      // 30ms待つ
        }
}

/* 障害物回避走行スレッド */
void avoidance(){
    while(1){  
        watchsurrounding();
        pc.printf("%d  %d  %d  %d  %d  \r\n",SL,SLD,SC,SRD,SR);
        if(flag_a == 0){                     // 障害物がない場合
            run = ADVANCE;               // 前進
        }else{                                // 障害物がある場合                   
            i = 0;
            if(SC < 15){                     // 正面15cm以内に障害物が現れた場合
                run = BACK;                  // 後退
                while(watch() < limit){      // 正面20cm以内に障害物がある間
                }
                    run = STOP;                  // 停止                 
            }
            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合  
                run = LEFT;                  // 左折                   
                while(i < 1){               // 進行方向確認 
                    flag_sp = flag_sp % 3 + 6; 
                    if(watch() > limit){    
                        i++;
                    }else{                   
                        i = 0;              
                    }
                }
                run = STOP;                 // 停止
            }else {                          // 全方向以外                          
                far = SC;                   // 正面を最も遠い距離に設定
                houkou = 1;                 // 進行方向を前に設定
                if(far < SLD || far < SL){  // 左または左前がより遠い場合
                    if(SL < SLD){           // 左前が左より遠い場合
                        far = SLD;          // 左前を最も遠い距離に設定
                    }else{                   // 左が左前より遠い場合
                        far = SL;           // 左を最も遠い距離に設定
                    }
                    houkou = 2;             // 進行方向を左に設定
                }
                if(far < SRD || far < SR){  // 右または右前がより遠い場合
                    if(SR < SRD){           // 右前が右より遠い場合
                        far = SRD;          // 右前を最も遠い距離に設定
                    }else{                   // 右が右前よりも遠い場合
                        far = SR;           // 右を最も遠い距離に設定
                    }
                    houkou = 3;             // 進行方向を右に設定
                }
                switch(houkou){                        // 進行方向の場合分け
                    case 1:                            // 前の場合
                        run = ADVANCE;                 // 前進
                        ThisThread::sleep_for(1000);   // 1秒待つ
                        break;
                    case 2:                            // 左の場合
                        
                        run = LEFT;                    // 左折
                        while(i < 1){                 // 進行方向確認
                            flag_sp = flag_sp % 3 + 6; 
                            if(watch() > (far - 2)){   // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
                                i++;                   // ループ+  
                            }else{
                                i = 0;
                            }
                        }
                        run = STOP;                    // 停止
                        break;
                    case 3:                            // 右の場合
                        
                        run = RIGHT;                   // 右折
                        while(i < 1){                 // 進行方向確認
                            flag_sp = flag_sp % 3 + 6; 
                            if(watch() > (far - 2)){   // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
                                i++;                   // ループ+  
                            }else{
                                i = 0;
                            }
                        }
                        run = STOP;                    // 停止
                        break;
                }
            }
        }
        flag_a = 0;                   // 障害物有無フラグを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 watchsurrounding(){
    //servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
    //ThisThread::sleep_for(200);     // 100ms待つ
    SC = watch();
    if(SC < limit){         // 正面20cm以内に障害物がある場合
        if(SC!=-1){              
            run = STOP;     // 停止  
            flag_a = 1;     
        }                
    }
    servo.pulsewidth_us(1925);      // サーボを左に40度回転
    ThisThread::sleep_for(100);     // 250ms待つ
    SLD = watch();
    if(SLD < limit){                // 左前20cm以内に障害物がある場合
        run = STOP;             // 停止
        flag_a = 1;
    }
    servo.pulsewidth_us(2400);      // サーボを左に90度回転
    ThisThread::sleep_for(100);     // 250ms待つ
    SL = watch();
    if(SL < limit){                 // 左20cm以内に障害物がある場合
        run = STOP;             // 停止
        flag_a = 1;
    }
    servo.pulsewidth_us(1450);
    ThisThread::sleep_for(250);
    SC = watch();
    if(SC < limit){
        if(SC!=-1){              
            run = STOP;     // 停止  
            flag_a = 1;     
        } 
    }
    servo.pulsewidth_us(925);       // サーボを右に40度回転
    ThisThread::sleep_for(100);     // 250ms待つ
    SRD = watch();
    if(SRD < limit){                // 右前20cm以内に障害物がある場合
        run = STOP;             // 停止
        flag_a = 1;
    }
    servo.pulsewidth_us(500);       // サーボを右に90度回転
    ThisThread::sleep_for(100);     // 250ms待つ
    SR = watch();
    if(SR < limit){                 // 右20cm以内に障害物がある場合
        run = STOP;             // 停止   
        flag_a = 1;  
    }
    servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
    ThisThread::sleep_for(250);     // 100ms待つ
    /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
        flag_a = 1;                 // 障害物有無フラグに1をセット
    }*/
}

/* ディスプレイ表示関数 */
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){
                    /* 普通 */
                    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(){
    //pc.printf("                                                                              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.start(500);       // 0.5秒周期でバックライトタイマー割り込み
            flag_t = 1;              // バックライトタイマーフラグを1に切り替え
        }
    }else{
        if(flag_t == 1){             // バックライトタイマーフラグが1なら
            bTimer.stop();           // バックライトタイマーストップ
            lcd.setBacklight(TextLCD::LightOn); // バックライトON
            flag_t = 0;              // バックライトタイマーフラグを0に切り替え
        }
    } 
}

// Serial Interrupt read ESP data
void callback()
{
    //pc.printf("\n\r------------ callback is being called --------------\n\r");
    led3=1;
    while (esp.readable()) {
        webbuff[ount] = esp.getc();
        ount++;
    }
    if(strlen(webbuff)>bufflen) {
        pc.printf("\f\n\r------------ webbuff over bufflen --------------\n\r");
        DataRX=1;
        led3=0;
    }
}
 
void wifi(void const *argument)
{
    pc.printf("\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) {
            pc.printf("\f\n\r------------ main while > if --------------\n\r");
            click_flag = 1;
            ReadWebData();
            pc.printf("\f\n\r------------ click_flag=%d --------------\n\r",click_flag);
            //if ((servreq == 1 && weberror == 0) && click_flag == 1) {
            if (servreq == 1 && weberror == 0) {
                pc.printf("\f\n\r------------ befor send page --------------\n\r");
                sendpage();
            }
            pc.printf("\f\n\r------------ send_check begin --------------\n\r");
 
            //sendcheck();
            pc.printf("\f\n\r------------ ssend_check end--------------\n\r");
 
            esp.attach(&callback);
            pc.printf(" IPD Data:\r\n\n Link ID = %d,\r\n IPD Header Length = %d \r\n IPD Type = %s\r\n", linkID, ipdLen, type);
            pc.printf("\n\n  HTTP Packet: \n\n%s\n", webdata);
            pc.printf("  Web Characters sent : %d\n\n", bufl);
            pc.printf("  -------------------------------------\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;}</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, "</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.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case LEFT:  //左折
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case STOP:  //停止
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"light\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case RIGHT: //右折
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"light\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case BACK:  //後進
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case AVOIDANCE:     //障害物回避
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        case LINE_TRACE:    //ライントレース
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"light\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,this.value)' >LINE_TRACE");
            break;
        default:    //その他
            strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\"  value=\"GO\"  onClick='send_mes(this.id,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.id,this.value)' >LEFT");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\"  onClick='send_mes(this.id,this.value)' >STOP");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\"  onClick='send_mes(this.id,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.id,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.id,this.value)' >");
            strcat(webbuff, "AVOIDANCE</button></td><td>");
            strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\"  onClick='send_mes(this.id,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.id,this.value)' >Normal");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\"  onClick='send_mes_spe(this.id,this.value)' >Fast");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\"  onClick='send_mes_spe(this.id,this.value)' >VeryFast");
            break;
        case 1:         //ファスト
            strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\"  onClick='send_mes_spe(this.id,this.value)' >Normal");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp2btn' type='button' class=\"light\" value=\"Fast\"  onClick='send_mes_spe(this.id,this.value)' >Fast");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\"  onClick='send_mes_spe(this.id,this.value)' >VeryFast");
            break;
        case 2:         //ベリーファスト
            strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\"  onClick='send_mes_spe(this.id,this.value)' >Normal");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\"  onClick='send_mes_spe(this.id,this.value)' >Fast");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp3btn' type='button' class=\"light\" value=\"VeryFast\"  onClick='send_mes_spe(this.id,this.value)' >VeryFast");
            break;
        default:        //その他
            strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\"  onClick='send_mes_spe(this.id,this.value)' >Normal");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\"  onClick='send_mes_spe(this.id,this.value)' >Fast");
            strcat(webbuff, "</button></td><td>");
            strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\"  onClick='send_mes_spe(this.id,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, "function htmlacs(url) {");
    strcat(webbuff, "var xhr = new XMLHttpRequest();");
    strcat(webbuff, "xhr.open(\"GET\", url);");
    strcat(webbuff, "xhr.send(\"\");");
    strcat(webbuff, "}");
    
    strcat(webbuff, "function send_mes(btnmes,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=0;i<7;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(btnmes,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=7;i<10;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();
    pc.printf(replybuff);
    //pc.printf("\n++++++++++ AT+CIPSENDBUF=%d,%d+++++++++\r\n", linkID, (bufl>2048?2048:bufl));
 
    pc.printf("\n++++++++++ bufl is %d ++++++++++\r\n",bufl);
 
    //pastthrough mode
    SendWEB();  // send web page
    pc.printf("\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(10);
                sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); // send IPD link channel and buffer character length.
                //pc.printf("\r\n++++++++++ AT+CIPSENDBUF=%d,%d ++++++++++\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048));
                timeout=600;
                getcount=50;
                SendCMD();
                getreply();
                //pc.printf(replybuff);
                //pc.printf("\r\n+++++++++++++++++++\r\n");
            }
            //****
            i++;
            //pc.printf("%c",webbuff[i]);
        }
    }
    pc.printf("\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();
        pc.printf(replybuff);
        sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
        timeout=500;
        getcount=10;
        SendCMD();
        getreply();
        pc.printf(replybuff);
    } else {
        sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection
        SendCMD();
        getreply();
        pc.printf(replybuff);
    }
    time2.reset();
}
 
// Reads and processes GET and POST web data
void ReadWebData()
{
    pc.printf("+++++++++++++++++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;
        pc.printf("+++++++++++++++++succed rec begin+++++++++++++++++++++\r\n");
        pc.printf("%s",webdata);
        pc.printf("+++++++++++++++++succed rec end+++++++++++++++++++++\r\n");
        if( strstr(webdata, "GO") != NULL ) {
            pc.printf("+++++++++++++++++前進+++++++++++++++++++++\r\n");
            //delete avoi_thread;     //障害物回避スレッド停止
            //delete trace_thread;    //ライントレーススレッド停止
            run = ADVANCE;          // 前進
            mode = ADVANCE;         // モード変更
            display();              // ディスプレイ表示
        }

        if( strstr(webdata, "LEFT") != NULL ) {
            pc.printf("+++++++++++++++++左折+++++++++++++++++++++\r\n");
            //delete avoi_thread;     //障害物回避スレッド停止
            //delete trace_thread;    //ライントレーススレッド停止
            run = LEFT;             // 左折
            mode = LEFT;            // モード変更
            display();              // ディスプレイ表示
        }
 
        if( strstr(webdata, "STOP") != NULL ) {
            pc.printf("+++++++++++++++++停止+++++++++++++++++++++\r\n");
            //delete avoi_thread;     //障害物回避スレッド停止
            //delete trace_thread;    //ライントレーススレッド停止
            run = STOP;             // 停止
            mode = STOP;            // モード変更
            display();              // ディスプレイ表示
        }
 
        if( strstr(webdata, "RIGHT") != NULL ) {
            pc.printf("+++++++++++++++++右折+++++++++++++++++++++\r\n");
            //delete avoi_thread;     //障害物回避スレッド停止
            //delete trace_thread;    //ライントレーススレッド停止
            run = RIGHT;            // 右折
            mode = RIGHT;           // モード変更
            display();              // ディスプレイ表示
        }
 
        if( strstr(webdata, "BACK") != NULL ) {
            pc.printf("+++++++++++++++++後進+++++++++++++++++++++\r\n");
            //delete avoi_thread;     //障害物回避スレッド停止
            //delete trace_thread;    //ライントレーススレッド停止
            run = BACK;             // 後進
            mode = BACK;            // モード変更
            display();              // ディスプレイ表示
        }
        pc.printf("+++++++++++++++++succed+++++++++++++++++++++");
 
        if( strstr(webdata, "AVOIDANCE") != NULL ) {
            pc.printf("+++++++++++++++++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 ) {
            pc.printf("+++++++++++++++++LINET RACE+++++++++++++++++++++");
            pc.printf("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( strstr(webdata, "Normal") != NULL ) {
            pc.printf("++++++++++++++++++Normal++++++++++++++++++++");
            mode = SPEED;       // スピードモード
            flag_sp = 0;
            display();          // ディスプレイ表示
            mode = beforeMode;  // 現在のモードに前回のモードを設定
        }
        if( strstr(webdata, "Fast") != NULL ) {
            pc.printf("++++++++++++++++++++Fast++++++++++++++++++");
            mode = SPEED;       // スピードモード
            flag_sp = 1;
            display();          // ディスプレイ表示
            mode = beforeMode;  // 現在のモードに前回のモードを設定
        }
        if( strstr(webdata, "VeryFast") != NULL ) {
            pc.printf("+++++++++++++++++++VeryFast+++++++++++++++++++");
            mode = SPEED;       // スピードモード 
            flag_sp = 2;
            display();          // ディスプレイ表示
            mode = beforeMode;  // 現在のモードに前回のモードを設定
        }
        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()
{
    pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
    strcpy(cmdbuff,"AT+RST\r\n");
    timeout=8000;
    getcount=1000;
    SendCMD();
    getreply();
    pc.printf(replybuff);
    pc.printf("%d",ount);
    if (strstr(replybuff, "OK") != NULL) {
        pc.printf("\n++++++++++ Starting Server ++++++++++\r\n");
        strcpy(cmdbuff, "AT+CIPMUX=1\r\n");  // set multiple connections.
        timeout=500;
        getcount=20;
        SendCMD();
        getreply();
        pc.printf(replybuff);
        sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
        timeout=500;
        getcount=20;
        SendCMD();
        getreply();
        pc.printf(replybuff);
        ThisThread::sleep_for(500);
        sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout);
        timeout=500;
        getcount=50;
        SendCMD();
        getreply();
        pc.printf(replybuff);
        ThisThread::sleep_for(5000);
        pc.printf("\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
            }
        }
        pc.printf("\n Enter WEB address (IP) found below in your browser \r\n\n");
        pc.printf("\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);
        pc.printf(webdata);
        led2=1;
        bufflen=200;
        //bufflen=100;
        ount=0;
        pc.printf("\n\n++++++++++ Ready ++++++++++\r\n\n");
        setup();
        esp.attach(&callback);
    } else {
        pc.printf("\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() {
    /* 初期設定 */
    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:SetUp");
    //display();              // ディスプレイ表示
    
    while(1){
        bChange();          // バッテリー残量更新
    }
}