denki / Mbed OS l1

1

Dependencies:   RemoteIR TextLCD

main.cpp@0:4c4775017e8a, 2020-09-23 (annotated)

Committer:
jiang111
Date:
Wed Sep 23 01:45:03 2020 +0000
Revision:
0:4c4775017e8a
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jiang111 0:4c4775017e8a 1
jiang111 0:4c4775017e8a 2 /* mbed Microcontroller Library
jiang111 0:4c4775017e8a 3 * Copyright (c) 2019 ARM Limited
jiang111 0:4c4775017e8a 4 * SPDX-License-Identifier: Apache-2.0
jiang111 0:4c4775017e8a 5 */
jiang111 0:4c4775017e8a 6
jiang111 0:4c4775017e8a 7 #include "mbed.h"
jiang111 0:4c4775017e8a 8 #include "ReceiverIR.h"
jiang111 0:4c4775017e8a 9 #include "rtos.h"
jiang111 0:4c4775017e8a 10 #include <stdint.h>
jiang111 0:4c4775017e8a 11 #include "platform/mbed_thread.h"
jiang111 0:4c4775017e8a 12 #include "TextLCD.h"
jiang111 0:4c4775017e8a 13
jiang111 0:4c4775017e8a 14 RawSerial pc(USBTX, USBRX);
jiang111 0:4c4775017e8a 15
jiang111 0:4c4775017e8a 16 /* マクロ定義、列挙型定義 */
jiang111 0:4c4775017e8a 17 #define MIN_V 2.0 // 電圧の最小値
jiang111 0:4c4775017e8a 18 #define MAX_V 2.67 // 電圧の最大値
jiang111 0:4c4775017e8a 19 #define LOW 0 // モーターOFF
jiang111 0:4c4775017e8a 20 #define HIGH 1 // モーターON
jiang111 0:4c4775017e8a 21 #define NORMAL 0 // 普通
jiang111 0:4c4775017e8a 22 #define FAST 1 // 速い
jiang111 0:4c4775017e8a 23 #define VERYFAST 2 // とても速い
jiang111 0:4c4775017e8a 24
jiang111 0:4c4775017e8a 25 #define SLOW 0.8
jiang111 0:4c4775017e8a 26 #define REVERSE 0.5
jiang111 0:4c4775017e8a 27
jiang111 0:4c4775017e8a 28 #define MBED04 0.781//Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
jiang111 0:4c4775017e8a 29 #define MBED05 0.953//Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
jiang111 0:4c4775017e8a 30 #define MSR0 0.6
jiang111 0:4c4775017e8a 31 #define MSR1 0.65
jiang111 0:4c4775017e8a 32 #define MSR2 0.7
jiang111 0:4c4775017e8a 33 #define MSR3 0.75
jiang111 0:4c4775017e8a 34 #define MSR4 0.8
jiang111 0:4c4775017e8a 35 #define MSR5 0.85
jiang111 0:4c4775017e8a 36 #define MSR6 0.9
jiang111 0:4c4775017e8a 37 #define MSR7 0.95
jiang111 0:4c4775017e8a 38 #define MSR8 1
jiang111 0:4c4775017e8a 39 #define MSL0 MSR0
jiang111 0:4c4775017e8a 40 #define MSL1 MSR1
jiang111 0:4c4775017e8a 41 #define MSL2 MSR2//*MBED05
jiang111 0:4c4775017e8a 42 #define MSL3 MSR3//*MBED05
jiang111 0:4c4775017e8a 43 #define MSL4 MSR4//*MBED05
jiang111 0:4c4775017e8a 44 #define MSL5 MSR5//*MBED05
jiang111 0:4c4775017e8a 45 #define MSL6 MSR6//*MBED05
jiang111 0:4c4775017e8a 46 #define MSL7 MSR7//*MBED05
jiang111 0:4c4775017e8a 47 #define MSL8 MSR8//*MBED05
jiang111 0:4c4775017e8a 48
jiang111 0:4c4775017e8a 49 /* 操作モード定義 */
jiang111 0:4c4775017e8a 50 enum MODE{
jiang111 0:4c4775017e8a 51 READY = -1, // -1：待ち
jiang111 0:4c4775017e8a 52 ADVANCE = 1, // 1：前進
jiang111 0:4c4775017e8a 53 RIGHT, // 2：右折
jiang111 0:4c4775017e8a 54 LEFT, // 3：左折
jiang111 0:4c4775017e8a 55 BACK, // 4：後退
jiang111 0:4c4775017e8a 56 STOP, // 5：停止
jiang111 0:4c4775017e8a 57 LINE_TRACE, // 6：ライントレース
jiang111 0:4c4775017e8a 58 AVOIDANCE, // 7：障害物回避
jiang111 0:4c4775017e8a 59 SPEED, // 8：スピード制御
jiang111 0:4c4775017e8a 60 };
jiang111 0:4c4775017e8a 61
jiang111 0:4c4775017e8a 62 /* ピン配置 */
jiang111 0:4c4775017e8a 63 ReceiverIR ir(p5); // リモコン操作
jiang111 0:4c4775017e8a 64 DigitalOut trig(p6); // 超音波センサtrigger
jiang111 0:4c4775017e8a 65 DigitalIn echo(p7); // 超音波センサecho
jiang111 0:4c4775017e8a 66 DigitalIn ss1(p8); // ライントレースセンサ(左)
jiang111 0:4c4775017e8a 67 DigitalIn ss2(p9); // ライントレースセンサ
jiang111 0:4c4775017e8a 68 DigitalIn ss3(p10); // ライントレースセンサ
jiang111 0:4c4775017e8a 69 DigitalIn ss4(p11); // ライントレースセンサ
jiang111 0:4c4775017e8a 70 DigitalIn ss5(p12); // ライントレースセンサ(右)
jiang111 0:4c4775017e8a 71 RawSerial esp(p13, p14); // Wi-Fiモジュール(tx, rx)
jiang111 0:4c4775017e8a 72 AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
jiang111 0:4c4775017e8a 73 PwmOut motorR2(p22); // 右モーター後退
jiang111 0:4c4775017e8a 74 PwmOut motorR1(p21); // 右モーター前進
jiang111 0:4c4775017e8a 75 PwmOut motorL2(p23); // 左モーター後退
jiang111 0:4c4775017e8a 76 PwmOut motorL1(p24); // 左モーター前進
jiang111 0:4c4775017e8a 77 PwmOut servo(p25); // サーボ
jiang111 0:4c4775017e8a 78 I2C i2c_lcd(p28,p27); // LCD(tx, rx)
jiang111 0:4c4775017e8a 79
jiang111 0:4c4775017e8a 80
jiang111 0:4c4775017e8a 81
jiang111 0:4c4775017e8a 82 /* 変数宣言 */
jiang111 0:4c4775017e8a 83 int mode; // 操作モード
jiang111 0:4c4775017e8a 84 int run; // 走行状態
jiang111 0:4c4775017e8a 85 int beforeRun = STOP; // 前回の走行状態
jiang111 0:4c4775017e8a 86 int beforeMode; // 前回のモード
jiang111 0:4c4775017e8a 87 int flag_sp = 0; // スピード変化フラグ
jiang111 0:4c4775017e8a 88 Timer viewTimer; // スピ―ド変更時に3秒計測タイマー
jiang111 0:4c4775017e8a 89
jiang111 0:4c4775017e8a 90 float motorSpeedR1[9] = {MSR0, MSR1, MSR2, MSR3 , MSR4 , MSR5 , MSR6 , MSR7 , MSR8 };
jiang111 0:4c4775017e8a 91 //float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6*REVERSE, MSR7*REVERSE, MSR8*REVERSE};
jiang111 0:4c4775017e8a 92 float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3, MSR4, MSR5, MSR6, MSR7, MSR8};
jiang111 0:4c4775017e8a 93 float motorSpeedL1[9] = {MSL0, MSL1, MSL2, MSL3 , MSL4 , MSL5 , MSL6 , MSL7 , MSL8 };
jiang111 0:4c4775017e8a 94 //float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6*REVERSE, MSL7*REVERSE, MSL8*REVERSE};
jiang111 0:4c4775017e8a 95 float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3, MSL4, MSL5, MSL6, MSL7, MSL8};
jiang111 0:4c4775017e8a 96 // モーター速度設定(後半はライントレース用)
jiang111 0:4c4775017e8a 97 // 0,1,2:基準速度
jiang111 0:4c4775017e8a 98 // 3,4,5:低速
jiang111 0:4c4775017e8a 99 // 6,7,8:高速
jiang111 0:4c4775017e8a 100 // R1 : 右前, R2 : 右後, L1 : 左前, L2 : 左後
jiang111 0:4c4775017e8a 101
jiang111 0:4c4775017e8a 102 Mutex mutex; // ミューテックス
jiang111 0:4c4775017e8a 103
jiang111 0:4c4775017e8a 104 /* decodeIR用変数 */
jiang111 0:4c4775017e8a 105 RemoteIR::Format format;
jiang111 0:4c4775017e8a 106 uint8_t buf[32];
jiang111 0:4c4775017e8a 107 uint32_t bitcount;
jiang111 0:4c4775017e8a 108 uint32_t code;
jiang111 0:4c4775017e8a 109
jiang111 0:4c4775017e8a 110 /* bChange, lcdbacklight用変数 */
jiang111 0:4c4775017e8a 111 TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
jiang111 0:4c4775017e8a 112 int b = 0; // バッテリー残量
jiang111 0:4c4775017e8a 113 int flag_b = 0; // バックライト点滅フラグ
jiang111 0:4c4775017e8a 114 int flag_t = 0; // バックライトタイマーフラグ
jiang111 0:4c4775017e8a 115
jiang111 0:4c4775017e8a 116 /* trace用変数 */
jiang111 0:4c4775017e8a 117 int sensArray[32] = {0,6,2,4,1,2,1,4, // ライントレースセンサパターン
jiang111 0:4c4775017e8a 118 3,6,1,6,1,1,1,6, // 0:前回動作継続
jiang111 0:4c4775017e8a 119 7,1,7,1,3,1,1,1, // 1:高速前進
jiang111 0:4c4775017e8a 120 5,1,7,1,5,1,7,1}; // 2:低速右折
jiang111 0:4c4775017e8a 121 // 3:低速左折
jiang111 0:4c4775017e8a 122 // 4:中速右折
jiang111 0:4c4775017e8a 123 // 5:中速左折
jiang111 0:4c4775017e8a 124 // 6:高速右折
jiang111 0:4c4775017e8a 125 // 7:高速左折
jiang111 0:4c4775017e8a 126
jiang111 0:4c4775017e8a 127 //int sensArray[32] = {0,6,0,4,1,0,1,4, // ライントレースセンサパターン
jiang111 0:4c4775017e8a 128 // 0,6,1,6,1,1,1,6, // 0:前回動作継続
jiang111 0:4c4775017e8a 129 // 7,1,7,1,0,1,1,1, // 1:高速前進
jiang111 0:4c4775017e8a 130 // 5,1,7,1,5,1,7,1}; // 2:低速右折
jiang111 0:4c4775017e8a 131 // // 3:低速左折
jiang111 0:4c4775017e8a 132 // // 4:中速右折
jiang111 0:4c4775017e8a 133 // // 5:中速左折
jiang111 0:4c4775017e8a 134 // // 6:高速右折
jiang111 0:4c4775017e8a 135 // // 7:高速左折
jiang111 0:4c4775017e8a 136
jiang111 0:4c4775017e8a 137 /* avoidance用変数 */
jiang111 0:4c4775017e8a 138 Timer timer; // 距離計測用タイマ
jiang111 0:4c4775017e8a 139 int DT; // 距離
jiang111 0:4c4775017e8a 140 int SC; // 正面
jiang111 0:4c4775017e8a 141 int SL; // 左
jiang111 0:4c4775017e8a 142 int SR; // 右
jiang111 0:4c4775017e8a 143 int SLD; // 左前
jiang111 0:4c4775017e8a 144 int SRD; // 右前
jiang111 0:4c4775017e8a 145 int flag_a = 0; // 障害物有無のフラグ
jiang111 0:4c4775017e8a 146 const int limit = 17; // 障害物の距離のリミット(単位：cm)
jiang111 0:4c4775017e8a 147 int far; // 最も遠い距離
jiang111 0:4c4775017e8a 148 int houkou; // 進行方向(1:前 2:左 3:右)
jiang111 0:4c4775017e8a 149 int t1 = 0;
jiang111 0:4c4775017e8a 150
jiang111 0:4c4775017e8a 151 /*WiFi用変数*/
jiang111 0:4c4775017e8a 152 Timer time1;
jiang111 0:4c4775017e8a 153 Timer time2;
jiang111 0:4c4775017e8a 154 int bufflen, DataRX, ount, getcount, replycount, servreq, timeout;
jiang111 0:4c4775017e8a 155 int bufl, ipdLen, linkID, weberror, webcounter,click_flag;
jiang111 0:4c4775017e8a 156 float R1=100000, R2=10000; // resistor values to give a 10:1 reduction of measured AnalogIn voltage
jiang111 0:4c4775017e8a 157 char webcount[8];
jiang111 0:4c4775017e8a 158 char type[16];
jiang111 0:4c4775017e8a 159 char channel[2];
jiang111 0:4c4775017e8a 160 char cmdbuff[32];
jiang111 0:4c4775017e8a 161 char replybuff[1024];
jiang111 0:4c4775017e8a 162 char webdata[1024]; // This may need to be bigger depending on WEB browser used
jiang111 0:4c4775017e8a 163 char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added
jiang111 0:4c4775017e8a 164 int port =80; // set server port
jiang111 0:4c4775017e8a 165 int SERVtimeout =5; // set server timeout in seconds in case link breaks.
jiang111 0:4c4775017e8a 166 char ssid[32] = "mbed05"; // enter WiFi router ssid inside the quotes
jiang111 0:4c4775017e8a 167 char pwd [32] = "0123456789a"; // enter WiFi router password inside the quotes
jiang111 0:4c4775017e8a 168
jiang111 0:4c4775017e8a 169 /* プロトタイプ宣言 */
jiang111 0:4c4775017e8a 170 void decodeIR(/*void const *argument*/);
jiang111 0:4c4775017e8a 171 void motor(/*void const *argument*/);
jiang111 0:4c4775017e8a 172 void changeSpeed();
jiang111 0:4c4775017e8a 173 void avoidance(/*void const *argument*/);
jiang111 0:4c4775017e8a 174 void trace(/*void const *argument*/);
jiang111 0:4c4775017e8a 175 void watchsurrounding3();
jiang111 0:4c4775017e8a 176 void watchsurrounding5();
jiang111 0:4c4775017e8a 177 int watch();
jiang111 0:4c4775017e8a 178 char battery_ch[8];
jiang111 0:4c4775017e8a 179 void bChange();
jiang111 0:4c4775017e8a 180 void display();
jiang111 0:4c4775017e8a 181 void lcdBacklight(void const *argument);
jiang111 0:4c4775017e8a 182 void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck();
jiang111 0:4c4775017e8a 183 void wifi(/*void const *argument*/);
jiang111 0:4c4775017e8a 184 Thread *deco_thread; // decodeIRをスレッド化 ：+3
jiang111 0:4c4775017e8a 185 Thread *wifi_thread;
jiang111 0:4c4775017e8a 186 //wifi_thread(wifi,NULL,osPriorityHigh); // wifiをスレッド化
jiang111 0:4c4775017e8a 187 Thread *motor_thread; // motorをスレッド化 ：+2
jiang111 0:4c4775017e8a 188 RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定
jiang111 0:4c4775017e8a 189 Thread *avoi_thread; // avoidanceをスレッド化：+2
jiang111 0:4c4775017e8a 190 Thread *trace_thread; // traceをスレッド化 ：+2
jiang111 0:4c4775017e8a 191
jiang111 0:4c4775017e8a 192 DigitalOut led1(LED1);
jiang111 0:4c4775017e8a 193 DigitalOut led2(LED2);
jiang111 0:4c4775017e8a 194 DigitalOut led3(LED3);
jiang111 0:4c4775017e8a 195 DigitalOut led4(LED4);
jiang111 0:4c4775017e8a 196
jiang111 0:4c4775017e8a 197 void setup(){
jiang111 0:4c4775017e8a 198 deco_thread = new Thread(decodeIR);
jiang111 0:4c4775017e8a 199 deco_thread -> set_priority(osPriorityRealtime);
jiang111 0:4c4775017e8a 200 motor_thread = new Thread(motor);
jiang111 0:4c4775017e8a 201 motor_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 202 //wifi_thread -> set_priority(osPriorityRealtime);
jiang111 0:4c4775017e8a 203 display();
jiang111 0:4c4775017e8a 204 }
jiang111 0:4c4775017e8a 205
jiang111 0:4c4775017e8a 206 /* リモコン受信スレッド */
jiang111 0:4c4775017e8a 207 void decodeIR(/*void const *argument*/){
jiang111 0:4c4775017e8a 208 while(1){
jiang111 0:4c4775017e8a 209 // 受信待ち
jiang111 0:4c4775017e8a 210 if (ir.getState() == ReceiverIR::Received){ // コード受信
jiang111 0:4c4775017e8a 211 bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
jiang111 0:4c4775017e8a 212 if(bitcount > 1){ // 受信成功
jiang111 0:4c4775017e8a 213 code=0;
jiang111 0:4c4775017e8a 214 for(int j = 0; j < 4; j++){
jiang111 0:4c4775017e8a 215 code+=(buf[j]<<(8*(3-j)));
jiang111 0:4c4775017e8a 216 }
jiang111 0:4c4775017e8a 217 if(mode != SPEED){ // スピードモード以外なら
jiang111 0:4c4775017e8a 218 beforeMode=mode; // 前回のモードに現在のモードを設定
jiang111 0:4c4775017e8a 219 }
jiang111 0:4c4775017e8a 220 switch(code){
jiang111 0:4c4775017e8a 221 case 0x40bf27d8: // クイック
jiang111 0:4c4775017e8a 222 ////*-*-*-5("mode = SPEED\r\n");
jiang111 0:4c4775017e8a 223 mode = SPEED; // スピードモード
jiang111 0:4c4775017e8a 224 changeSpeed(); // 速度変更
jiang111 0:4c4775017e8a 225 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 226 mode = beforeMode; // 現在のモードに前回のモードを設定
jiang111 0:4c4775017e8a 227 break;
jiang111 0:4c4775017e8a 228 case 0x40be34cb: // レグザリンク
jiang111 0:4c4775017e8a 229 ////*-*-*-5("mode = LINE_TRACE\r\n");
jiang111 0:4c4775017e8a 230 if(trace_thread->get_state() == Thread::Deleted){
jiang111 0:4c4775017e8a 231 delete trace_thread;
jiang111 0:4c4775017e8a 232 trace_thread = new Thread(trace);
jiang111 0:4c4775017e8a 233 trace_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 234 }
jiang111 0:4c4775017e8a 235 mode=LINE_TRACE; // ライントレースモード
jiang111 0:4c4775017e8a 236 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 237 break;
jiang111 0:4c4775017e8a 238 case 0x40bf6e91: // 番組表
jiang111 0:4c4775017e8a 239 ////*-*-*-5("mode = AVOIDANCE\r\n");
jiang111 0:4c4775017e8a 240 if(avoi_thread->get_state() == Thread::Deleted){
jiang111 0:4c4775017e8a 241 delete avoi_thread;
jiang111 0:4c4775017e8a 242 avoi_thread = new Thread(avoidance);
jiang111 0:4c4775017e8a 243 avoi_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 244 }
jiang111 0:4c4775017e8a 245 flag_a = 0;
jiang111 0:4c4775017e8a 246 mode=AVOIDANCE; // 障害物回避モード
jiang111 0:4c4775017e8a 247 run = ADVANCE; // 前進
jiang111 0:4c4775017e8a 248 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 249 break;
jiang111 0:4c4775017e8a 250 case 0x40bf3ec1: // ↑
jiang111 0:4c4775017e8a 251 ////*-*-*-5("mode = ADVANCE\r\n");
jiang111 0:4c4775017e8a 252 mode = ADVANCE; // 前進モード
jiang111 0:4c4775017e8a 253 run = ADVANCE; // 前進
jiang111 0:4c4775017e8a 254 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 255 break;
jiang111 0:4c4775017e8a 256 case 0x40bf3fc0: // ↓
jiang111 0:4c4775017e8a 257 ////*-*-*-5("mode = BACK\r\n");
jiang111 0:4c4775017e8a 258 mode = BACK; // 後退モード
jiang111 0:4c4775017e8a 259 run = BACK; // 後退
jiang111 0:4c4775017e8a 260 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 261 break;
jiang111 0:4c4775017e8a 262 case 0x40bf5fa0: // ←
jiang111 0:4c4775017e8a 263 ////*-*-*-5("mode = LEFT\r\n");
jiang111 0:4c4775017e8a 264 mode = LEFT; // 左折モード
jiang111 0:4c4775017e8a 265 run = LEFT; // 左折
jiang111 0:4c4775017e8a 266 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 267 break;
jiang111 0:4c4775017e8a 268 case 0x40bf5ba4: // →
jiang111 0:4c4775017e8a 269 ////*-*-*-5("mode = RIGHT\r\n");
jiang111 0:4c4775017e8a 270 mode = RIGHT; // 右折モード
jiang111 0:4c4775017e8a 271 run = RIGHT; // 右折
jiang111 0:4c4775017e8a 272 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 273 break;
jiang111 0:4c4775017e8a 274 case 0x40bf3dc2: // 決定
jiang111 0:4c4775017e8a 275 ////*-*-*-5("mode = STOP\r\n");
jiang111 0:4c4775017e8a 276 mode = STOP; // 停止モード
jiang111 0:4c4775017e8a 277 run = STOP; // 停止
jiang111 0:4c4775017e8a 278 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 279 break;
jiang111 0:4c4775017e8a 280 default:
jiang111 0:4c4775017e8a 281 ;
jiang111 0:4c4775017e8a 282 }
jiang111 0:4c4775017e8a 283 if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
jiang111 0:4c4775017e8a 284 trace_thread->terminate();
jiang111 0:4c4775017e8a 285 }
jiang111 0:4c4775017e8a 286 if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
jiang111 0:4c4775017e8a 287 avoi_thread->terminate();
jiang111 0:4c4775017e8a 288 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 289 }
jiang111 0:4c4775017e8a 290 }
jiang111 0:4c4775017e8a 291 }
jiang111 0:4c4775017e8a 292 if(viewTimer.read_ms()>=3000){ // スピードモードのまま3秒経過
jiang111 0:4c4775017e8a 293 viewTimer.stop(); // タイマーストップ
jiang111 0:4c4775017e8a 294 viewTimer.reset(); // タイマーリセット
jiang111 0:4c4775017e8a 295 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 296 }
jiang111 0:4c4775017e8a 297 ThisThread::sleep_for(90); // 90ms待つ
jiang111 0:4c4775017e8a 298 }
jiang111 0:4c4775017e8a 299 }
jiang111 0:4c4775017e8a 300
jiang111 0:4c4775017e8a 301 /* モーター制御スレッド */
jiang111 0:4c4775017e8a 302 void motor(/*void const *argument*/){
jiang111 0:4c4775017e8a 303 while(1){
jiang111 0:4c4775017e8a 304 /* 走行状態の場合分け */
jiang111 0:4c4775017e8a 305 switch(run){
jiang111 0:4c4775017e8a 306 /* 前進 */
jiang111 0:4c4775017e8a 307 case ADVANCE:
jiang111 0:4c4775017e8a 308 motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
jiang111 0:4c4775017e8a 309 motorR2 = LOW; // 右後退モーターOFF
jiang111 0:4c4775017e8a 310 motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
jiang111 0:4c4775017e8a 311 motorL2 = LOW; // 左後退モーターOFF
jiang111 0:4c4775017e8a 312 break;
jiang111 0:4c4775017e8a 313 /* 右折 */
jiang111 0:4c4775017e8a 314 case RIGHT:
jiang111 0:4c4775017e8a 315 motorR1 = LOW; // 右前進モーターOFF
jiang111 0:4c4775017e8a 316 motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
jiang111 0:4c4775017e8a 317 motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
jiang111 0:4c4775017e8a 318 motorL2 = LOW; // 左後退モーターOFF
jiang111 0:4c4775017e8a 319
jiang111 0:4c4775017e8a 320 break;
jiang111 0:4c4775017e8a 321 /* 左折 */
jiang111 0:4c4775017e8a 322 case LEFT:
jiang111 0:4c4775017e8a 323 motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
jiang111 0:4c4775017e8a 324 motorR2 = LOW; // 右後退モーターOFF
jiang111 0:4c4775017e8a 325 motorL1 = LOW; // 左前進モーターOFF
jiang111 0:4c4775017e8a 326 motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
jiang111 0:4c4775017e8a 327
jiang111 0:4c4775017e8a 328 break;
jiang111 0:4c4775017e8a 329 /* 後退 */
jiang111 0:4c4775017e8a 330 case BACK:
jiang111 0:4c4775017e8a 331 motorR1 = LOW; // 右前進モーターOFF
jiang111 0:4c4775017e8a 332 motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
jiang111 0:4c4775017e8a 333 motorL1 = LOW; // 左前進モーターOFF
jiang111 0:4c4775017e8a 334 motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
jiang111 0:4c4775017e8a 335 break;
jiang111 0:4c4775017e8a 336 /* 停止 */
jiang111 0:4c4775017e8a 337 case STOP:
jiang111 0:4c4775017e8a 338 motorR1 = LOW; // 右前進モーターOFF
jiang111 0:4c4775017e8a 339 motorR2 = LOW; // 右後退モーターOFF
jiang111 0:4c4775017e8a 340 motorL1 = LOW; // 左前進モーターOFF
jiang111 0:4c4775017e8a 341 motorL2 = LOW; // 左後退モーターOFF
jiang111 0:4c4775017e8a 342 break;
jiang111 0:4c4775017e8a 343 }
jiang111 0:4c4775017e8a 344 if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら
jiang111 0:4c4775017e8a 345 flag_sp %= 3; // スピード変更フラグ調整
jiang111 0:4c4775017e8a 346 }
jiang111 0:4c4775017e8a 347 ThisThread::sleep_for(3); // 30ms待つ
jiang111 0:4c4775017e8a 348 }
jiang111 0:4c4775017e8a 349 }
jiang111 0:4c4775017e8a 350
jiang111 0:4c4775017e8a 351 /* スピード変更関数 */
jiang111 0:4c4775017e8a 352 void changeSpeed(){
jiang111 0:4c4775017e8a 353 // それ以外
jiang111 0:4c4775017e8a 354 flag_sp = flag_sp + 3; // スピード変更フラグを+1
jiang111 0:4c4775017e8a 355 if(flag_sp>6)flag_sp=0;
jiang111 0:4c4775017e8a 356 }
jiang111 0:4c4775017e8a 357
jiang111 0:4c4775017e8a 358 /* ライントレーススレッド */
jiang111 0:4c4775017e8a 359 void trace(){
jiang111 0:4c4775017e8a 360 while(1){
jiang111 0:4c4775017e8a 361 /* 各センサー値読み取り */
jiang111 0:4c4775017e8a 362 int sensor1 = ss1;
jiang111 0:4c4775017e8a 363 int sensor2 = ss2;
jiang111 0:4c4775017e8a 364 int sensor3 = ss3;
jiang111 0:4c4775017e8a 365 int sensor4 = ss4;
jiang111 0:4c4775017e8a 366 int sensor5 = ss5;
jiang111 0:4c4775017e8a 367 ////*-*-*-5("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5);
jiang111 0:4c4775017e8a 368 int sensD = 0;
jiang111 0:4c4775017e8a 369 /* センサー値の決定 */
jiang111 0:4c4775017e8a 370 if(sensor1 > 0) sensD |= 0x10;
jiang111 0:4c4775017e8a 371 if(sensor2 > 0) sensD |= 0x08;
jiang111 0:4c4775017e8a 372 if(sensor3 > 0) sensD |= 0x04;
jiang111 0:4c4775017e8a 373 if(sensor4 > 0) sensD |= 0x02;
jiang111 0:4c4775017e8a 374 if(sensor5 > 0) sensD |= 0x01;
jiang111 0:4c4775017e8a 375 pc.printf("%d \r\n",sensArray[sensD]);
jiang111 0:4c4775017e8a 376 /* センサー値によって場合分け */
jiang111 0:4c4775017e8a 377 switch(sensArray[sensD]){
jiang111 0:4c4775017e8a 378 case 1:
jiang111 0:4c4775017e8a 379 flag_sp = 6;
jiang111 0:4c4775017e8a 380 beforeRun = run;
jiang111 0:4c4775017e8a 381 run = ADVANCE; // 高速で前進
jiang111 0:4c4775017e8a 382 break;
jiang111 0:4c4775017e8a 383 case 2:
jiang111 0:4c4775017e8a 384 flag_sp = 2;
jiang111 0:4c4775017e8a 385 beforeRun = run;
jiang111 0:4c4775017e8a 386 run = RIGHT;
jiang111 0:4c4775017e8a 387 break;
jiang111 0:4c4775017e8a 388 case 3:
jiang111 0:4c4775017e8a 389 flag_sp = 2;
jiang111 0:4c4775017e8a 390 beforeRun = run;
jiang111 0:4c4775017e8a 391 run = LEFT;
jiang111 0:4c4775017e8a 392 break;
jiang111 0:4c4775017e8a 393 case 4:
jiang111 0:4c4775017e8a 394 flag_sp = 0;
jiang111 0:4c4775017e8a 395 beforeRun = run;
jiang111 0:4c4775017e8a 396 run = RIGHT;
jiang111 0:4c4775017e8a 397 break;
jiang111 0:4c4775017e8a 398 case 5:
jiang111 0:4c4775017e8a 399 flag_sp = 0;
jiang111 0:4c4775017e8a 400 beforeRun = run;
jiang111 0:4c4775017e8a 401 run = LEFT;
jiang111 0:4c4775017e8a 402 break;
jiang111 0:4c4775017e8a 403 case 6:
jiang111 0:4c4775017e8a 404 flag_sp = 4;
jiang111 0:4c4775017e8a 405 beforeRun = run;
jiang111 0:4c4775017e8a 406 run = RIGHT;
jiang111 0:4c4775017e8a 407 break;
jiang111 0:4c4775017e8a 408 case 7:
jiang111 0:4c4775017e8a 409 flag_sp = 4;
jiang111 0:4c4775017e8a 410 beforeRun = run;
jiang111 0:4c4775017e8a 411 run = LEFT;
jiang111 0:4c4775017e8a 412 break;
jiang111 0:4c4775017e8a 413 default:
jiang111 0:4c4775017e8a 414 break;
jiang111 0:4c4775017e8a 415 }
jiang111 0:4c4775017e8a 416 // ThisThread::sleep_for(30); // 30ms待つ
jiang111 0:4c4775017e8a 417 ThisThread::sleep_for(3);
jiang111 0:4c4775017e8a 418 }
jiang111 0:4c4775017e8a 419 }
jiang111 0:4c4775017e8a 420
jiang111 0:4c4775017e8a 421 /* 障害物回避走行スレッド */
jiang111 0:4c4775017e8a 422 void avoidance(){
jiang111 0:4c4775017e8a 423 int i;
jiang111 0:4c4775017e8a 424 watchsurrounding3();
jiang111 0:4c4775017e8a 425 while(1){
jiang111 0:4c4775017e8a 426 if(flag_a==0){run=ADVANCE;watchsurrounding3();}
jiang111 0:4c4775017e8a 427 else{
jiang111 0:4c4775017e8a 428 i=0;
jiang111 0:4c4775017e8a 429 watchsurrounding5();
jiang111 0:4c4775017e8a 430 if(SLD>=SC&&SRD>=SC){
jiang111 0:4c4775017e8a 431 if(SL>=SR){
jiang111 0:4c4775017e8a 432 run = BACK;
jiang111 0:4c4775017e8a 433 ThisThread::sleep_for(200-flag_sp*10);
jiang111 0:4c4775017e8a 434
jiang111 0:4c4775017e8a 435 run = LEFT;
jiang111 0:4c4775017e8a 436 ThisThread::sleep_for(480-flag_sp*25);
jiang111 0:4c4775017e8a 437 run=STOP;
jiang111 0:4c4775017e8a 438 }
jiang111 0:4c4775017e8a 439 if(SL<SR){
jiang111 0:4c4775017e8a 440 run = BACK;
jiang111 0:4c4775017e8a 441 ThisThread::sleep_for(200-flag_sp*10);
jiang111 0:4c4775017e8a 442 run = RIGHT;
jiang111 0:4c4775017e8a 443 ThisThread::sleep_for(480-flag_sp*25);
jiang111 0:4c4775017e8a 444 run=STOP;}
jiang111 0:4c4775017e8a 445 }
jiang111 0:4c4775017e8a 446 if(SLD<SC){
jiang111 0:4c4775017e8a 447 run = BACK;
jiang111 0:4c4775017e8a 448 ThisThread::sleep_for(200-flag_sp*10);
jiang111 0:4c4775017e8a 449 run = RIGHT;
jiang111 0:4c4775017e8a 450 ThisThread::sleep_for(250-flag_sp*10);
jiang111 0:4c4775017e8a 451 run=STOP;
jiang111 0:4c4775017e8a 452 }
jiang111 0:4c4775017e8a 453 if(SRD<SC){
jiang111 0:4c4775017e8a 454 run = BACK;
jiang111 0:4c4775017e8a 455 ThisThread::sleep_for(200-flag_sp*10);
jiang111 0:4c4775017e8a 456 run = LEFT;
jiang111 0:4c4775017e8a 457 ThisThread::sleep_for(250-flag_sp*10);
jiang111 0:4c4775017e8a 458 run=STOP;
jiang111 0:4c4775017e8a 459 }
jiang111 0:4c4775017e8a 460
jiang111 0:4c4775017e8a 461 flag_a=0;
jiang111 0:4c4775017e8a 462
jiang111 0:4c4775017e8a 463 }
jiang111 0:4c4775017e8a 464 }
jiang111 0:4c4775017e8a 465 }
jiang111 0:4c4775017e8a 466 /* 距離計測関数 */
jiang111 0:4c4775017e8a 467 int watch(){
jiang111 0:4c4775017e8a 468 do{
jiang111 0:4c4775017e8a 469 trig = 0;
jiang111 0:4c4775017e8a 470 ThisThread::sleep_for(5); // 5ms待つ
jiang111 0:4c4775017e8a 471 trig = 1;
jiang111 0:4c4775017e8a 472 ThisThread::sleep_for(15); // 15ms待つ
jiang111 0:4c4775017e8a 473 trig = 0;
jiang111 0:4c4775017e8a 474 timer.start();
jiang111 0:4c4775017e8a 475 t1=timer.read_ms();
jiang111 0:4c4775017e8a 476 while(echo.read() == 0 && t1<10){
jiang111 0:4c4775017e8a 477 t1=timer.read_ms();
jiang111 0:4c4775017e8a 478 led1 = 1;
jiang111 0:4c4775017e8a 479 }
jiang111 0:4c4775017e8a 480 timer.stop();
jiang111 0:4c4775017e8a 481 timer.reset();
jiang111 0:4c4775017e8a 482 /*if((t1-t2) >= 10){
jiang111 0:4c4775017e8a 483 run = STOP;*/
jiang111 0:4c4775017e8a 484 }while(t1 >= 10);
jiang111 0:4c4775017e8a 485 timer.start(); // 距離計測タイマースタート
jiang111 0:4c4775017e8a 486 while(echo.read() == 1){
jiang111 0:4c4775017e8a 487 }
jiang111 0:4c4775017e8a 488 timer.stop(); // 距離計測タイマーストップ
jiang111 0:4c4775017e8a 489 DT = (int)(timer.read_us()*0.01657); // 距離計算
jiang111 0:4c4775017e8a 490 if(DT > 1000){
jiang111 0:4c4775017e8a 491 DT = -1;
jiang111 0:4c4775017e8a 492 }else if(DT > 150){ // 検知範囲外なら100cmに設定
jiang111 0:4c4775017e8a 493 DT = 150;
jiang111 0:4c4775017e8a 494 }
jiang111 0:4c4775017e8a 495 timer.reset(); // 距離計測タイマーリセット
jiang111 0:4c4775017e8a 496 led1 = 0;
jiang111 0:4c4775017e8a 497 return DT;
jiang111 0:4c4775017e8a 498 }
jiang111 0:4c4775017e8a 499
jiang111 0:4c4775017e8a 500 /* 障害物検知関数 */
jiang111 0:4c4775017e8a 501 void watchsurrounding3(){
jiang111 0:4c4775017e8a 502 //servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 503 //ThisThread::sleep_for(200); // 100ms待つ
jiang111 0:4c4775017e8a 504 SC = watch();
jiang111 0:4c4775017e8a 505 if(SC < limit){ // 正面20cm以内に障害物がある場合
jiang111 0:4c4775017e8a 506 if(SC!=-1){
jiang111 0:4c4775017e8a 507 run = STOP; // 停止
jiang111 0:4c4775017e8a 508 flag_a = 1;
jiang111 0:4c4775017e8a 509 return;
jiang111 0:4c4775017e8a 510 }
jiang111 0:4c4775017e8a 511 }
jiang111 0:4c4775017e8a 512 servo.pulsewidth_us(1925); // サーボを左に40度回転
jiang111 0:4c4775017e8a 513 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 514 SLD = watch();
jiang111 0:4c4775017e8a 515 if(SLD < limit){ // 左前20cm以内に障害物がある場合
jiang111 0:4c4775017e8a 516 run = STOP; // 停止
jiang111 0:4c4775017e8a 517 flag_a = 1;
jiang111 0:4c4775017e8a 518 return;
jiang111 0:4c4775017e8a 519 }
jiang111 0:4c4775017e8a 520 servo.pulsewidth_us(1450);
jiang111 0:4c4775017e8a 521 ThisThread::sleep_for(150);
jiang111 0:4c4775017e8a 522 SC = watch();
jiang111 0:4c4775017e8a 523 if(SC < limit){
jiang111 0:4c4775017e8a 524 if(SC!=-1){
jiang111 0:4c4775017e8a 525 run = STOP; // 停止
jiang111 0:4c4775017e8a 526 flag_a = 1;
jiang111 0:4c4775017e8a 527 return;
jiang111 0:4c4775017e8a 528 }
jiang111 0:4c4775017e8a 529 }
jiang111 0:4c4775017e8a 530 servo.pulsewidth_us(925); // サーボを右に40度回転
jiang111 0:4c4775017e8a 531 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 532 SRD = watch();
jiang111 0:4c4775017e8a 533 if(SRD < limit){ // 右前20cm以内に障害物がある場合
jiang111 0:4c4775017e8a 534 run = STOP; // 停止
jiang111 0:4c4775017e8a 535 flag_a = 1;
jiang111 0:4c4775017e8a 536 return;
jiang111 0:4c4775017e8a 537 }
jiang111 0:4c4775017e8a 538 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 539 ThisThread::sleep_for(100); // 100ms待つ
jiang111 0:4c4775017e8a 540 /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
jiang111 0:4c4775017e8a 541 flag_a = 1; // 障害物有無フラグに1をセット
jiang111 0:4c4775017e8a 542 }*/
jiang111 0:4c4775017e8a 543 }
jiang111 0:4c4775017e8a 544
jiang111 0:4c4775017e8a 545 /* 障害物検知関数 */
jiang111 0:4c4775017e8a 546 void watchsurrounding5(){
jiang111 0:4c4775017e8a 547 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 548 ThisThread::sleep_for(200); // 100ms待つ
jiang111 0:4c4775017e8a 549 SC = watch();
jiang111 0:4c4775017e8a 550 servo.pulsewidth_us(1925); // サーボを左に40度回転
jiang111 0:4c4775017e8a 551 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 552 SLD = watch();
jiang111 0:4c4775017e8a 553 servo.pulsewidth_us(2400); // サーボを左に90度回転
jiang111 0:4c4775017e8a 554 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 555 SL = watch();
jiang111 0:4c4775017e8a 556 servo.pulsewidth_us(1450);
jiang111 0:4c4775017e8a 557 ThisThread::sleep_for(250);
jiang111 0:4c4775017e8a 558 SC = watch();
jiang111 0:4c4775017e8a 559 servo.pulsewidth_us(925); // サーボを右に40度回転
jiang111 0:4c4775017e8a 560 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 561 SRD = watch();
jiang111 0:4c4775017e8a 562 servo.pulsewidth_us(500); // サーボを右に90度回転
jiang111 0:4c4775017e8a 563 ThisThread::sleep_for(100); // 250ms待つ
jiang111 0:4c4775017e8a 564 SR = watch();
jiang111 0:4c4775017e8a 565 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 566 ThisThread::sleep_for(250); // 100ms待つ
jiang111 0:4c4775017e8a 567 }
jiang111 0:4c4775017e8a 568
jiang111 0:4c4775017e8a 569 /* ディスプレイ表示関数 */
jiang111 0:4c4775017e8a 570 void display(){
jiang111 0:4c4775017e8a 571 mutex.lock(); // ミューテックスロック
jiang111 0:4c4775017e8a 572 lcd.setAddress(0,1);
jiang111 0:4c4775017e8a 573
jiang111 0:4c4775017e8a 574 /* 操作モードによる場合分け */
jiang111 0:4c4775017e8a 575 switch(mode){
jiang111 0:4c4775017e8a 576 /* 前進 */
jiang111 0:4c4775017e8a 577 case ADVANCE:
jiang111 0:4c4775017e8a 578 lcd.printf("Mode:Advance ");
jiang111 0:4c4775017e8a 579 break;
jiang111 0:4c4775017e8a 580 /* 右折 */
jiang111 0:4c4775017e8a 581 case RIGHT:
jiang111 0:4c4775017e8a 582 lcd.printf("Mode:TurnRight ");
jiang111 0:4c4775017e8a 583 break;
jiang111 0:4c4775017e8a 584 /* 左折 */
jiang111 0:4c4775017e8a 585 case LEFT:
jiang111 0:4c4775017e8a 586 lcd.printf("Mode:TurnLeft ");
jiang111 0:4c4775017e8a 587 break;
jiang111 0:4c4775017e8a 588 /* 後退 */
jiang111 0:4c4775017e8a 589 case BACK:
jiang111 0:4c4775017e8a 590 lcd.printf("Mode:Back ");
jiang111 0:4c4775017e8a 591 break;
jiang111 0:4c4775017e8a 592 /* 停止 */
jiang111 0:4c4775017e8a 593 case STOP:
jiang111 0:4c4775017e8a 594 lcd.printf("Mode:Stop ");
jiang111 0:4c4775017e8a 595 break;
jiang111 0:4c4775017e8a 596 /* 待ち */
jiang111 0:4c4775017e8a 597 case READY:
jiang111 0:4c4775017e8a 598 lcd.printf("Mode:Ready ");
jiang111 0:4c4775017e8a 599 break;
jiang111 0:4c4775017e8a 600 /* ライントレース */
jiang111 0:4c4775017e8a 601 case LINE_TRACE:
jiang111 0:4c4775017e8a 602 lcd.printf("Mode:LineTrace ");
jiang111 0:4c4775017e8a 603 break;
jiang111 0:4c4775017e8a 604 /* 障害物回避 */
jiang111 0:4c4775017e8a 605 case AVOIDANCE:
jiang111 0:4c4775017e8a 606 lcd.printf("Mode:Avoidance ");
jiang111 0:4c4775017e8a 607 break;
jiang111 0:4c4775017e8a 608 /* スピード制御 */
jiang111 0:4c4775017e8a 609 case SPEED:
jiang111 0:4c4775017e8a 610 /* スピードの状態で場合分け */
jiang111 0:4c4775017e8a 611 switch(flag_sp % 3){
jiang111 0:4c4775017e8a 612 /* 普通 */
jiang111 0:4c4775017e8a 613 case(NORMAL):
jiang111 0:4c4775017e8a 614 lcd.printf("Speed:Normal ");
jiang111 0:4c4775017e8a 615 break;
jiang111 0:4c4775017e8a 616 /* 速い */
jiang111 0:4c4775017e8a 617 case(FAST):
jiang111 0:4c4775017e8a 618 lcd.printf("Speed:Fast ");
jiang111 0:4c4775017e8a 619 break;
jiang111 0:4c4775017e8a 620 /* とても速い */
jiang111 0:4c4775017e8a 621 case(VERYFAST):
jiang111 0:4c4775017e8a 622 lcd.printf("Speed:VeryFast ");
jiang111 0:4c4775017e8a 623 break;
jiang111 0:4c4775017e8a 624 }
jiang111 0:4c4775017e8a 625 viewTimer.reset(); // タイマーリセット
jiang111 0:4c4775017e8a 626 viewTimer.start(); // タイマースタート
jiang111 0:4c4775017e8a 627 break;
jiang111 0:4c4775017e8a 628 }
jiang111 0:4c4775017e8a 629 mutex.unlock(); // ミューテックスアンロック
jiang111 0:4c4775017e8a 630 }
jiang111 0:4c4775017e8a 631
jiang111 0:4c4775017e8a 632 /* バックライト点滅 */
jiang111 0:4c4775017e8a 633 void lcdBacklight(void const *argument){
jiang111 0:4c4775017e8a 634 if(flag_b == 1){ // バックライト点滅フラグが1なら
jiang111 0:4c4775017e8a 635 lcd.setBacklight(TextLCD::LightOn); // バックライトON
jiang111 0:4c4775017e8a 636 }else{ // バックライト点滅フラグが0なら
jiang111 0:4c4775017e8a 637 lcd.setBacklight(TextLCD::LightOff); // バックライトOFF
jiang111 0:4c4775017e8a 638 }
jiang111 0:4c4775017e8a 639 flag_b = !flag_b; // バックライト点滅フラグ切り替え
jiang111 0:4c4775017e8a 640 }
jiang111 0:4c4775017e8a 641
jiang111 0:4c4775017e8a 642 /* バッテリー残量更新関数 */
jiang111 0:4c4775017e8a 643 void bChange(){
jiang111 0:4c4775017e8a 644 ////*-*-*-5(" bChange1\r\n");
jiang111 0:4c4775017e8a 645 b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
jiang111 0:4c4775017e8a 646 if(b <= 0){ // バッテリー残量0%なら全ての機能停止
jiang111 0:4c4775017e8a 647 b = 0;
jiang111 0:4c4775017e8a 648 //lcd.setBacklight(TextLCD::LightOff);
jiang111 0:4c4775017e8a 649 //run = STOP;
jiang111 0:4c4775017e8a 650 //exit(1); // 電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。
jiang111 0:4c4775017e8a 651 }
jiang111 0:4c4775017e8a 652 mutex.lock(); // ミューテックスロック
jiang111 0:4c4775017e8a 653 lcd.setAddress(0,0);
jiang111 0:4c4775017e8a 654 lcd.printf("Battery:%3d%%",b); // バッテリー残量表示
jiang111 0:4c4775017e8a 655 mutex.unlock(); // ミューテックスアンロック
jiang111 0:4c4775017e8a 656 if(b <= 30){ // バッテリー残量30%以下なら
jiang111 0:4c4775017e8a 657 if(flag_t == 0){ // バックライトタイマーフラグが0なら
jiang111 0:4c4775017e8a 658 //bTimer.attach(lcdBacklight,0.5);
jiang111 0:4c4775017e8a 659 bTimer.start(500); // 0.5秒周期でバックライトタイマー割り込み
jiang111 0:4c4775017e8a 660 flag_t = 1; // バックライトタイマーフラグを1に切り替え
jiang111 0:4c4775017e8a 661 }
jiang111 0:4c4775017e8a 662 }else{
jiang111 0:4c4775017e8a 663 if(flag_t == 1){ // バックライトタイマーフラグが1なら
jiang111 0:4c4775017e8a 664 //bTimer.detach();
jiang111 0:4c4775017e8a 665 bTimer.stop(); // バックライトタイマーストップ
jiang111 0:4c4775017e8a 666 lcd.setBacklight(TextLCD::LightOn); // バックライトON
jiang111 0:4c4775017e8a 667 flag_t = 0; // バックライトタイマーフラグを0に切り替え
jiang111 0:4c4775017e8a 668 }
jiang111 0:4c4775017e8a 669 }
jiang111 0:4c4775017e8a 670 }
jiang111 0:4c4775017e8a 671 // Serial Interrupt read ESP data
jiang111 0:4c4775017e8a 672 void callback()
jiang111 0:4c4775017e8a 673 {
jiang111 0:4c4775017e8a 674 ////*-*-*-5("\n\r------------ callback is being called --------------\n\r");
jiang111 0:4c4775017e8a 675 led3=1;
jiang111 0:4c4775017e8a 676 while (esp.readable()) {
jiang111 0:4c4775017e8a 677 webbuff[ount] = esp.getc();
jiang111 0:4c4775017e8a 678 ount++;
jiang111 0:4c4775017e8a 679 }
jiang111 0:4c4775017e8a 680 if(strlen(webbuff)>bufflen) {
jiang111 0:4c4775017e8a 681 // //*-*-*-5("\f\n\r------------ webbuff over bufflen --------------\n\r");
jiang111 0:4c4775017e8a 682 DataRX=1;
jiang111 0:4c4775017e8a 683 led3=0;
jiang111 0:4c4775017e8a 684 }
jiang111 0:4c4775017e8a 685 }
jiang111 0:4c4775017e8a 686
jiang111 0:4c4775017e8a 687 void wifi(/*void const *argument*/)
jiang111 0:4c4775017e8a 688 {
jiang111 0:4c4775017e8a 689 //*-*-*-5("\f\n\r------------ ESP8266 Hardware Reset psq --------------\n\r");
jiang111 0:4c4775017e8a 690 ThisThread::sleep_for(100);
jiang111 0:4c4775017e8a 691 led1=1,led2=0,led3=0;
jiang111 0:4c4775017e8a 692 timeout=6000;
jiang111 0:4c4775017e8a 693 getcount=500;
jiang111 0:4c4775017e8a 694 getreply();
jiang111 0:4c4775017e8a 695 esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
jiang111 0:4c4775017e8a 696 startserver();
jiang111 0:4c4775017e8a 697
jiang111 0:4c4775017e8a 698 while(1) {
jiang111 0:4c4775017e8a 699 if(DataRX==1) {
jiang111 0:4c4775017e8a 700 //*-*-*-5("\f\n\r------------ main while > if --------------\n\r");
jiang111 0:4c4775017e8a 701 click_flag = 1;
jiang111 0:4c4775017e8a 702 ReadWebData();
jiang111 0:4c4775017e8a 703 //*-*-*-5("\f\n\r------------ click_flag=%d --------------\n\r",click_flag);
jiang111 0:4c4775017e8a 704 //if ((servreq == 1 && weberror == 0) && click_flag == 1) {
jiang111 0:4c4775017e8a 705 if (servreq == 1 && weberror == 0) {
jiang111 0:4c4775017e8a 706 //*-*-*-5("\f\n\r------------ befor send page --------------\n\r");
jiang111 0:4c4775017e8a 707 sendpage();
jiang111 0:4c4775017e8a 708 }
jiang111 0:4c4775017e8a 709 //*-*-*-5("\f\n\r------------ send_check begin --------------\n\r");
jiang111 0:4c4775017e8a 710
jiang111 0:4c4775017e8a 711 //sendcheck();
jiang111 0:4c4775017e8a 712 //*-*-*-5("\f\n\r------------ ssend_check end--------------\n\r");
jiang111 0:4c4775017e8a 713
jiang111 0:4c4775017e8a 714 esp.attach(&callback);
jiang111 0:4c4775017e8a 715 //*-*-*-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);
jiang111 0:4c4775017e8a 716 //*-*-*-5("\n\n HTTP Packet: \n\n%s\n", webdata);
jiang111 0:4c4775017e8a 717 //*-*-*-5(" Web Characters sent : %d\n\n", bufl);
jiang111 0:4c4775017e8a 718 //*-*-*-5(" -------------------------------------\n\n");
jiang111 0:4c4775017e8a 719 servreq=0;
jiang111 0:4c4775017e8a 720 }
jiang111 0:4c4775017e8a 721 ThisThread::sleep_for(100);
jiang111 0:4c4775017e8a 722 }
jiang111 0:4c4775017e8a 723 }
jiang111 0:4c4775017e8a 724 // Static WEB page
jiang111 0:4c4775017e8a 725 void sendpage()
jiang111 0:4c4775017e8a 726 {
jiang111 0:4c4775017e8a 727 // WEB page data
jiang111 0:4c4775017e8a 728
jiang111 0:4c4775017e8a 729 strcpy(webbuff, "<!DOCTYPE html>");
jiang111 0:4c4775017e8a 730 strcat(webbuff, "<html><head><title>RobotCar</title><meta name='viewport' content='width=device-width'/>");
jiang111 0:4c4775017e8a 731 //strcat(webbuff, "<meta http-equiv=\"refresh\" content=\"5\"; >");
jiang111 0:4c4775017e8a 732 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>");
jiang111 0:4c4775017e8a 733 strcat(webbuff, "</head><body><center><p><strong>Robot Car Remote Controller");
jiang111 0:4c4775017e8a 734 strcat(webbuff, "</strong></p><td style='vertical-align:top;'><strong>Battery level ");
jiang111 0:4c4775017e8a 735 if(b > 30) { //残電量表示
jiang111 0:4c4775017e8a 736 sprintf(webbuff, "%s%3d", webbuff, b);
jiang111 0:4c4775017e8a 737 } else { //30%より下の場合残電量を赤文字
jiang111 0:4c4775017e8a 738 strcat(webbuff, "<font color=\"red\">");
jiang111 0:4c4775017e8a 739 sprintf(webbuff, "%s%3d", webbuff, b);
jiang111 0:4c4775017e8a 740 strcat(webbuff, "</font>");
jiang111 0:4c4775017e8a 741 }
jiang111 0:4c4775017e8a 742 strcat(webbuff, "%</strong>");
jiang111 0:4c4775017e8a 743 strcat(webbuff, "<button id=\"reloadbtn\" type=\"button\" class=\"load\" onclick=\"location.reload()\">RELOAD</button>");
jiang111 0:4c4775017e8a 744 strcat(webbuff, "</td></p>");
jiang111 0:4c4775017e8a 745 strcat(webbuff, "<br>");
jiang111 0:4c4775017e8a 746 strcat(webbuff, "<table><tr><td></td><td>");
jiang111 0:4c4775017e8a 747
jiang111 0:4c4775017e8a 748 switch(mode) { //ブラウザ更新時の現在の車の状態からボタンの点灯判定
jiang111 0:4c4775017e8a 749 case ADVANCE: //前進
jiang111 0:4c4775017e8a 750 strcat(webbuff, "<button id='gobtn' type='button' class=\"light\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 751 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 752 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 753 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 754 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 755 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 756 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 757 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 758 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 759 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 760 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 761 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 762 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 763 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 764 break;
jiang111 0:4c4775017e8a 765 case LEFT: //左折
jiang111 0:4c4775017e8a 766 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 767 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 768 strcat(webbuff, "<button id='leftbtn' type='button' class=\"light\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 769 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 770 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 771 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 772 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 773 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 774 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 775 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 776 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 777 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 778 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 779 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 780 break;
jiang111 0:4c4775017e8a 781 case STOP: //停止
jiang111 0:4c4775017e8a 782 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 783 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 784 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 785 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 786 strcat(webbuff, "<button id='stopbtn' type='button' class=\"light\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 787 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 788 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 789 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 790 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 791 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 792 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 793 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 794 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 795 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 796 break;
jiang111 0:4c4775017e8a 797 case RIGHT: //右折
jiang111 0:4c4775017e8a 798 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 799 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 800 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 801 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 802 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 803 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 804 strcat(webbuff, "<button id='rightbtn' type='button' class=\"light\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 805 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 806 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 807 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 808 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 809 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 810 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 811 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 812 break;
jiang111 0:4c4775017e8a 813 case BACK: //後進
jiang111 0:4c4775017e8a 814 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 815 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 816 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 817 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 818 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 819 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 820 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 821 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 822 strcat(webbuff, "<button id='backbtn' type='button' class=\"light\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 823 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr><td>");
jiang111 0:4c4775017e8a 824 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 825 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 826 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 827 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 828 break;
jiang111 0:4c4775017e8a 829 case AVOIDANCE: //障害物回避
jiang111 0:4c4775017e8a 830 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 831 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 832 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 833 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 834 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 835 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 836 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 837 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 838 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 839 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 840 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 841 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"light\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 842 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 843 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 844 break;
jiang111 0:4c4775017e8a 845 case LINE_TRACE: //ライントレース
jiang111 0:4c4775017e8a 846 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 847 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 848 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 849 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 850 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 851 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 852 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 853 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 854 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 855 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 856 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 857 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 858 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 859 strcat(webbuff, "<button id='tracebtn' type='button' class=\"light\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 860 break;
jiang111 0:4c4775017e8a 861 default: //その他
jiang111 0:4c4775017e8a 862 strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onClick='send_mes(this.value)'>GO");
jiang111 0:4c4775017e8a 863 strcat(webbuff, "</button></td><td></td></tr><tr><td>");
jiang111 0:4c4775017e8a 864 strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onClick='send_mes(this.value)' >LEFT");
jiang111 0:4c4775017e8a 865 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 866 strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onClick='send_mes(this.value)' >STOP");
jiang111 0:4c4775017e8a 867 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 868 strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onClick='send_mes(this.value)' >RIGHT");
jiang111 0:4c4775017e8a 869 strcat(webbuff, "</button></td></tr><td></td><td>");
jiang111 0:4c4775017e8a 870 strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onClick='send_mes(this.value)' >BACK");
jiang111 0:4c4775017e8a 871 strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
jiang111 0:4c4775017e8a 872 strcat(webbuff, "<strong>Mode</strong>");
jiang111 0:4c4775017e8a 873 strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onClick='send_mes(this.value)' >");
jiang111 0:4c4775017e8a 874 strcat(webbuff, "AVOIDANCE</button></td><td>");
jiang111 0:4c4775017e8a 875 strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onClick='send_mes(this.value)' >LINE_TRACE");
jiang111 0:4c4775017e8a 876 break;
jiang111 0:4c4775017e8a 877 }
jiang111 0:4c4775017e8a 878 strcat(webbuff, "</button></td></tr></table>");
jiang111 0:4c4775017e8a 879 strcat(webbuff, "<strong>Speed</strong>");
jiang111 0:4c4775017e8a 880 strcat(webbuff, "<table><tr><td>");
jiang111 0:4c4775017e8a 881 //ready示速度だけ点灯
jiang111 0:4c4775017e8a 882 switch (flag_sp) { //現在の速度のボタン表示
jiang111 0:4c4775017e8a 883 case 0: //ノーマル
jiang111 0:4c4775017e8a 884 strcat(webbuff, "<button id='sp1btn' type='button' class=\"light\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
jiang111 0:4c4775017e8a 885 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 886 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
jiang111 0:4c4775017e8a 887 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 888 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
jiang111 0:4c4775017e8a 889 break;
jiang111 0:4c4775017e8a 890 case 1: //ファスト
jiang111 0:4c4775017e8a 891 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
jiang111 0:4c4775017e8a 892 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 893 strcat(webbuff, "<button id='sp2btn' type='button' class=\"light\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
jiang111 0:4c4775017e8a 894 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 895 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
jiang111 0:4c4775017e8a 896 break;
jiang111 0:4c4775017e8a 897 case 2: //ベリーファスト
jiang111 0:4c4775017e8a 898 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
jiang111 0:4c4775017e8a 899 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 900 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
jiang111 0:4c4775017e8a 901 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 902 strcat(webbuff, "<button id='sp3btn' type='button' class=\"light\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
jiang111 0:4c4775017e8a 903 break;
jiang111 0:4c4775017e8a 904 default: //その他
jiang111 0:4c4775017e8a 905 strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onClick='send_mes_spe(this.value)' >Normal");
jiang111 0:4c4775017e8a 906 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 907 strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onClick='send_mes_spe(this.value)' >Fast");
jiang111 0:4c4775017e8a 908 strcat(webbuff, "</button></td><td>");
jiang111 0:4c4775017e8a 909 strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onClick='send_mes_spe(this.value)' >VeryFast");
jiang111 0:4c4775017e8a 910 break;
jiang111 0:4c4775017e8a 911 }
jiang111 0:4c4775017e8a 912 strcat(webbuff, "</button></td></tr></table>");
jiang111 0:4c4775017e8a 913
jiang111 0:4c4775017e8a 914 strcat(webbuff, "</center>");
jiang111 0:4c4775017e8a 915 strcat(webbuff, "</body>");
jiang111 0:4c4775017e8a 916 strcat(webbuff, "</html>");
jiang111 0:4c4775017e8a 917
jiang111 0:4c4775017e8a 918 strcat(webbuff, "<script language=\"javascript\" type=\"text/javascript\">"); //機能
jiang111 0:4c4775017e8a 919
jiang111 0:4c4775017e8a 920 strcat(webbuff, "var button_9 = document.getElementsByTagName(\"button\");");
jiang111 0:4c4775017e8a 921
jiang111 0:4c4775017e8a 922 //ボタン入力時それぞれの関数から呼び出されサーバーとの通信を行う
jiang111 0:4c4775017e8a 923 strcat(webbuff, "function htmlacs(url) {");
jiang111 0:4c4775017e8a 924 strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=true;}");
jiang111 0:4c4775017e8a 925 strcat(webbuff, "var xhr = new XMLHttpRequest();");
jiang111 0:4c4775017e8a 926 strcat(webbuff, "xhr.open(\"GET\", url);");
jiang111 0:4c4775017e8a 927 strcat(webbuff, "xhr.onreadystatechange = function(){");
jiang111 0:4c4775017e8a 928 strcat(webbuff, "if(this.readyState == 4 || this.status == 200){");
jiang111 0:4c4775017e8a 929 strcat(webbuff, "for(var m=0;m<11;m++){button_9[m].disabled=false;}");
jiang111 0:4c4775017e8a 930 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 931 strcat(webbuff, "};");
jiang111 0:4c4775017e8a 932 strcat(webbuff, "xhr.send(\"\");");
jiang111 0:4c4775017e8a 933 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 934
jiang111 0:4c4775017e8a 935 //mode変更ボタン入力時動作 //sendmes
jiang111 0:4c4775017e8a 936 strcat(webbuff, "function send_mes(btnval){"); //mode変更ボタン入力時の点灯消灯判定
jiang111 0:4c4775017e8a 937 strcat(webbuff, "console.log(btnval);");
jiang111 0:4c4775017e8a 938 strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
jiang111 0:4c4775017e8a 939 strcat(webbuff, "htmlacs(url);");
jiang111 0:4c4775017e8a 940 strcat(webbuff, "console.log(url);");
jiang111 0:4c4775017e8a 941 strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
jiang111 0:4c4775017e8a 942 strcat(webbuff, "for(var i=1;i<8;i++){");
jiang111 0:4c4775017e8a 943 strcat(webbuff, "if(buttons[i].value == btnval){");
jiang111 0:4c4775017e8a 944 strcat(webbuff, "buttons[i].className=\"light\";");
jiang111 0:4c4775017e8a 945 strcat(webbuff, "}else{");
jiang111 0:4c4775017e8a 946 strcat(webbuff, "buttons[i].className=\"noselect\";");
jiang111 0:4c4775017e8a 947 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 948 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 949 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 950
jiang111 0:4c4775017e8a 951 strcat(webbuff, "function send_mes_spe(btnval){"); //speed変更ボタン入力時の点灯消灯判定
jiang111 0:4c4775017e8a 952 strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
jiang111 0:4c4775017e8a 953 strcat(webbuff, "htmlacs(url);");
jiang111 0:4c4775017e8a 954 strcat(webbuff, "console.log(url);");
jiang111 0:4c4775017e8a 955 strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
jiang111 0:4c4775017e8a 956 strcat(webbuff, "for(var i=8;i<11;i++){");
jiang111 0:4c4775017e8a 957 strcat(webbuff, "if(buttons[i].value == btnval){");
jiang111 0:4c4775017e8a 958 strcat(webbuff, "buttons[i].className=\"light\";");
jiang111 0:4c4775017e8a 959 strcat(webbuff, "}else{");
jiang111 0:4c4775017e8a 960 strcat(webbuff, "buttons[i].className=\"noselect\";");
jiang111 0:4c4775017e8a 961 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 962 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 963 strcat(webbuff, "}");
jiang111 0:4c4775017e8a 964 strcat(webbuff, "</script>");
jiang111 0:4c4775017e8a 965
jiang111 0:4c4775017e8a 966
jiang111 0:4c4775017e8a 967 // end of WEB page data
jiang111 0:4c4775017e8a 968 bufl = strlen(webbuff); // get total page buffer length
jiang111 0:4c4775017e8a 969 //sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length.
jiang111 0:4c4775017e8a 970
jiang111 0:4c4775017e8a 971 sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl>2048?2048:bufl)); // send IPD link channel and buffer character length.
jiang111 0:4c4775017e8a 972 timeout=500;
jiang111 0:4c4775017e8a 973 getcount=40;
jiang111 0:4c4775017e8a 974 SendCMD();
jiang111 0:4c4775017e8a 975 getreply();
jiang111 0:4c4775017e8a 976 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 977 ////*-*-*-5("\n++++++++++ AT+CIPSENDBUF=%d,%d+++++++++\r\n", linkID, (bufl>2048?2048:bufl));
jiang111 0:4c4775017e8a 978
jiang111 0:4c4775017e8a 979 //*-*-*-5("\n++++++++++ bufl is %d ++++++++++\r\n",bufl);
jiang111 0:4c4775017e8a 980
jiang111 0:4c4775017e8a 981 //pastthrough mode
jiang111 0:4c4775017e8a 982 SendWEB(); // send web page
jiang111 0:4c4775017e8a 983 //*-*-*-5("\n++++++++++ webbuff clear ++++++++++\r\n");
jiang111 0:4c4775017e8a 984
jiang111 0:4c4775017e8a 985 memset(webbuff, '\0', sizeof(webbuff));
jiang111 0:4c4775017e8a 986 sendcheck();
jiang111 0:4c4775017e8a 987 }
jiang111 0:4c4775017e8a 988
jiang111 0:4c4775017e8a 989 // Large WEB buffer data send
jiang111 0:4c4775017e8a 990 void SendWEB()
jiang111 0:4c4775017e8a 991 {
jiang111 0:4c4775017e8a 992 int i=0;
jiang111 0:4c4775017e8a 993 if(esp.writeable()) {
jiang111 0:4c4775017e8a 994 while(webbuff[i]!='\0') {
jiang111 0:4c4775017e8a 995 esp.putc(webbuff[i]);
jiang111 0:4c4775017e8a 996
jiang111 0:4c4775017e8a 997 //****
jiang111 0:4c4775017e8a 998 //output at command when 2000
jiang111 0:4c4775017e8a 999 if(((i%2047)==0) && (i>0)) {
jiang111 0:4c4775017e8a 1000 //wait_ms(10);
jiang111 0:4c4775017e8a 1001 ThisThread::sleep_for(100);
jiang111 0:4c4775017e8a 1002 sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); // send IPD link channel and buffer character length.
jiang111 0:4c4775017e8a 1003 ////*-*-*-5("\r\n++++++++++ AT+CIPSENDBUF=%d,%d ++++++++++\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048));
jiang111 0:4c4775017e8a 1004 timeout=600;
jiang111 0:4c4775017e8a 1005 getcount=50;
jiang111 0:4c4775017e8a 1006 SendCMD();
jiang111 0:4c4775017e8a 1007 getreply();
jiang111 0:4c4775017e8a 1008 ////*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1009 ////*-*-*-5("\r\n+++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1010 }
jiang111 0:4c4775017e8a 1011 //****
jiang111 0:4c4775017e8a 1012 i++;
jiang111 0:4c4775017e8a 1013 ////*-*-*-5("%c",webbuff[i]);
jiang111 0:4c4775017e8a 1014 }
jiang111 0:4c4775017e8a 1015 }
jiang111 0:4c4775017e8a 1016 //*-*-*-5("\n++++++++++ send web i= %dinfo ++++++++++\r\n",i);
jiang111 0:4c4775017e8a 1017 }
jiang111 0:4c4775017e8a 1018
jiang111 0:4c4775017e8a 1019
jiang111 0:4c4775017e8a 1020 void sendcheck()
jiang111 0:4c4775017e8a 1021 {
jiang111 0:4c4775017e8a 1022 weberror=1;
jiang111 0:4c4775017e8a 1023 timeout=500;
jiang111 0:4c4775017e8a 1024 getcount=24;
jiang111 0:4c4775017e8a 1025 time2.reset();
jiang111 0:4c4775017e8a 1026 time2.start();
jiang111 0:4c4775017e8a 1027
jiang111 0:4c4775017e8a 1028 /*
jiang111 0:4c4775017e8a 1029 while(weberror==1 && time2.read() <5) {
jiang111 0:4c4775017e8a 1030 getreply();
jiang111 0:4c4775017e8a 1031 if (strstr(replybuff, "SEND OK") != NULL) {
jiang111 0:4c4775017e8a 1032 weberror=0; // wait for valid SEND OK
jiang111 0:4c4775017e8a 1033 }
jiang111 0:4c4775017e8a 1034 }
jiang111 0:4c4775017e8a 1035 */
jiang111 0:4c4775017e8a 1036 if(weberror==1) { // restart connection
jiang111 0:4c4775017e8a 1037 strcpy(cmdbuff, "AT+CIPMUX=1\r\n");
jiang111 0:4c4775017e8a 1038 timeout=500;
jiang111 0:4c4775017e8a 1039 getcount=10;
jiang111 0:4c4775017e8a 1040 SendCMD();
jiang111 0:4c4775017e8a 1041 getreply();
jiang111 0:4c4775017e8a 1042 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1043 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
jiang111 0:4c4775017e8a 1044 timeout=500;
jiang111 0:4c4775017e8a 1045 getcount=10;
jiang111 0:4c4775017e8a 1046 SendCMD();
jiang111 0:4c4775017e8a 1047 getreply();
jiang111 0:4c4775017e8a 1048 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1049 } else {
jiang111 0:4c4775017e8a 1050 sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection
jiang111 0:4c4775017e8a 1051 SendCMD();
jiang111 0:4c4775017e8a 1052 getreply();
jiang111 0:4c4775017e8a 1053 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1054 }
jiang111 0:4c4775017e8a 1055 time2.reset();
jiang111 0:4c4775017e8a 1056 }
jiang111 0:4c4775017e8a 1057
jiang111 0:4c4775017e8a 1058 // Reads and processes GET and POST web data
jiang111 0:4c4775017e8a 1059 void ReadWebData()
jiang111 0:4c4775017e8a 1060 {
jiang111 0:4c4775017e8a 1061 //*-*-*-5("+++++++++++++++++Read Web Data+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1062 ThisThread::sleep_for(200);
jiang111 0:4c4775017e8a 1063 esp.attach(NULL);
jiang111 0:4c4775017e8a 1064 ount=0;
jiang111 0:4c4775017e8a 1065 DataRX=0;
jiang111 0:4c4775017e8a 1066 weberror=0;
jiang111 0:4c4775017e8a 1067 memset(webdata, '\0', sizeof(webdata));
jiang111 0:4c4775017e8a 1068 int x = strcspn (webbuff,"+");
jiang111 0:4c4775017e8a 1069 if(x) {
jiang111 0:4c4775017e8a 1070 strcpy(webdata, webbuff + x);
jiang111 0:4c4775017e8a 1071 weberror=0;
jiang111 0:4c4775017e8a 1072 int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type);
jiang111 0:4c4775017e8a 1073 //int i=0;
jiang111 0:4c4775017e8a 1074 //*-*-*-5("+++++++++++++++++succed rec begin+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1075 //*-*-*-5("%s",webdata);
jiang111 0:4c4775017e8a 1076 //*-*-*-5("+++++++++++++++++succed rec end+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1077 if( strstr(webdata, "Normal") != NULL ) {
jiang111 0:4c4775017e8a 1078 //*-*-*-5("++++++++++++++++++Normal++++++++++++++++++++");
jiang111 0:4c4775017e8a 1079 mode = SPEED; // スピードモード
jiang111 0:4c4775017e8a 1080 flag_sp = 0;
jiang111 0:4c4775017e8a 1081 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1082 mode = beforeMode; // 現在のモードに前回のモードを設定
jiang111 0:4c4775017e8a 1083 }else if( strstr(webdata, "VeryFast") != NULL ) {
jiang111 0:4c4775017e8a 1084 //*-*-*-5("+++++++++++++++++++VeryFast+++++++++++++++++++");
jiang111 0:4c4775017e8a 1085 mode = SPEED; // スピードモード
jiang111 0:4c4775017e8a 1086 flag_sp = 2;
jiang111 0:4c4775017e8a 1087 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1088 mode = beforeMode; // 現在のモードに前回のモードを設定
jiang111 0:4c4775017e8a 1089 }else if( strstr(webdata, "Fast") != NULL ) {
jiang111 0:4c4775017e8a 1090 //*-*-*-5("++++++++++++++++++++Fast++++++++++++++++++");
jiang111 0:4c4775017e8a 1091 mode = SPEED; // スピードモード
jiang111 0:4c4775017e8a 1092 flag_sp = 1;
jiang111 0:4c4775017e8a 1093 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1094 mode = beforeMode; // 現在のモードに前回のモードを設定
jiang111 0:4c4775017e8a 1095 }else{
jiang111 0:4c4775017e8a 1096 beforeMode = mode;
jiang111 0:4c4775017e8a 1097 }
jiang111 0:4c4775017e8a 1098 if( strstr(webdata, "GO") != NULL ) {
jiang111 0:4c4775017e8a 1099 //*-*-*-5("+++++++++++++++++前進+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1100 //delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1101 //delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1102 run = ADVANCE; // 前進
jiang111 0:4c4775017e8a 1103 mode = ADVANCE; // モード変更
jiang111 0:4c4775017e8a 1104 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1105 }
jiang111 0:4c4775017e8a 1106
jiang111 0:4c4775017e8a 1107 if( strstr(webdata, "LEFT") != NULL ) {
jiang111 0:4c4775017e8a 1108 //*-*-*-5("+++++++++++++++++左折+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1109 //delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1110 //delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1111 run = LEFT; // 左折
jiang111 0:4c4775017e8a 1112 mode = LEFT; // モード変更
jiang111 0:4c4775017e8a 1113 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1114 }
jiang111 0:4c4775017e8a 1115
jiang111 0:4c4775017e8a 1116 if( strstr(webdata, "STOP") != NULL ) {
jiang111 0:4c4775017e8a 1117 //*-*-*-5("+++++++++++++++++停止+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1118 //delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1119 //delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1120 run = STOP; // 停止
jiang111 0:4c4775017e8a 1121 mode = STOP; // モード変更
jiang111 0:4c4775017e8a 1122 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1123 }
jiang111 0:4c4775017e8a 1124
jiang111 0:4c4775017e8a 1125 if( strstr(webdata, "RIGHT") != NULL ) {
jiang111 0:4c4775017e8a 1126 //*-*-*-5("+++++++++++++++++右折+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1127 //delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1128 //delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1129 run = RIGHT; // 右折
jiang111 0:4c4775017e8a 1130 mode = RIGHT; // モード変更
jiang111 0:4c4775017e8a 1131 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1132 }
jiang111 0:4c4775017e8a 1133
jiang111 0:4c4775017e8a 1134 if( strstr(webdata, "BACK") != NULL ) {
jiang111 0:4c4775017e8a 1135 //*-*-*-5("+++++++++++++++++後進+++++++++++++++++++++\r\n");
jiang111 0:4c4775017e8a 1136 //delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1137 //delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1138 run = BACK; // 後進
jiang111 0:4c4775017e8a 1139 mode = BACK; // モード変更
jiang111 0:4c4775017e8a 1140 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1141 }
jiang111 0:4c4775017e8a 1142 //*-*-*-5("+++++++++++++++++succed+++++++++++++++++++++");
jiang111 0:4c4775017e8a 1143
jiang111 0:4c4775017e8a 1144 if( strstr(webdata, "AVOIDANCE") != NULL ) {
jiang111 0:4c4775017e8a 1145 //*-*-*-5("+++++++++++++++++AVOIDANCE+++++++++++++++++++++");
jiang111 0:4c4775017e8a 1146 if(avoi_thread->get_state() == Thread::Deleted) {
jiang111 0:4c4775017e8a 1147 delete avoi_thread; //障害物回避スレッド停止
jiang111 0:4c4775017e8a 1148 avoi_thread = new Thread(avoidance);
jiang111 0:4c4775017e8a 1149 avoi_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 1150 }
jiang111 0:4c4775017e8a 1151 mode=AVOIDANCE;
jiang111 0:4c4775017e8a 1152 run = ADVANCE;
jiang111 0:4c4775017e8a 1153 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1154 }
jiang111 0:4c4775017e8a 1155 if( strstr(webdata, "LINE_TRACE") != NULL ) {
jiang111 0:4c4775017e8a 1156 //*-*-*-5("+++++++++++++++++LINET RACE+++++++++++++++++++++");
jiang111 0:4c4775017e8a 1157 //*-*-*-5("mode = LINE_TRACE\r\n");
jiang111 0:4c4775017e8a 1158 if(trace_thread->get_state() == Thread::Deleted) {
jiang111 0:4c4775017e8a 1159 delete trace_thread; //ライントレーススレッド停止
jiang111 0:4c4775017e8a 1160 trace_thread = new Thread(trace);
jiang111 0:4c4775017e8a 1161 trace_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 1162 }
jiang111 0:4c4775017e8a 1163 mode=LINE_TRACE;
jiang111 0:4c4775017e8a 1164 display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1165 }
jiang111 0:4c4775017e8a 1166 if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
jiang111 0:4c4775017e8a 1167 trace_thread->terminate();
jiang111 0:4c4775017e8a 1168 }
jiang111 0:4c4775017e8a 1169 if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
jiang111 0:4c4775017e8a 1170 avoi_thread->terminate();
jiang111 0:4c4775017e8a 1171 servo.pulsewidth_us(1450); // サーボを中央位置に戻す
jiang111 0:4c4775017e8a 1172 }
jiang111 0:4c4775017e8a 1173 sprintf(channel, "%d",linkID);
jiang111 0:4c4775017e8a 1174 if (strstr(webdata, "GET") != NULL) {
jiang111 0:4c4775017e8a 1175 servreq=1;
jiang111 0:4c4775017e8a 1176 }
jiang111 0:4c4775017e8a 1177 if (strstr(webdata, "POST") != NULL) {
jiang111 0:4c4775017e8a 1178 servreq=1;
jiang111 0:4c4775017e8a 1179 }
jiang111 0:4c4775017e8a 1180 webcounter++;
jiang111 0:4c4775017e8a 1181 sprintf(webcount, "%d",webcounter);
jiang111 0:4c4775017e8a 1182 } else {
jiang111 0:4c4775017e8a 1183 memset(webbuff, '\0', sizeof(webbuff));
jiang111 0:4c4775017e8a 1184 esp.attach(&callback);
jiang111 0:4c4775017e8a 1185 weberror=1;
jiang111 0:4c4775017e8a 1186 }
jiang111 0:4c4775017e8a 1187 }
jiang111 0:4c4775017e8a 1188 // Starts and restarts webserver if errors detected.
jiang111 0:4c4775017e8a 1189 void startserver()
jiang111 0:4c4775017e8a 1190 {
jiang111 0:4c4775017e8a 1191 //*-*-*-5("++++++++++ Resetting ESP ++++++++++\r\n");
jiang111 0:4c4775017e8a 1192 strcpy(cmdbuff,"AT+RST\r\n");
jiang111 0:4c4775017e8a 1193 timeout=8000;
jiang111 0:4c4775017e8a 1194 getcount=1000;
jiang111 0:4c4775017e8a 1195 SendCMD();
jiang111 0:4c4775017e8a 1196 getreply();
jiang111 0:4c4775017e8a 1197 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1198 //*-*-*-5("%d",ount);
jiang111 0:4c4775017e8a 1199 if (strstr(replybuff, "OK") != NULL) {
jiang111 0:4c4775017e8a 1200 //*-*-*-5("\n++++++++++ Starting Server ++++++++++\r\n");
jiang111 0:4c4775017e8a 1201 strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); // set multiple connections.
jiang111 0:4c4775017e8a 1202 timeout=500;
jiang111 0:4c4775017e8a 1203 getcount=20;
jiang111 0:4c4775017e8a 1204 SendCMD();
jiang111 0:4c4775017e8a 1205 getreply();
jiang111 0:4c4775017e8a 1206 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1207 sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
jiang111 0:4c4775017e8a 1208 timeout=500;
jiang111 0:4c4775017e8a 1209 getcount=20;
jiang111 0:4c4775017e8a 1210 SendCMD();
jiang111 0:4c4775017e8a 1211 getreply();
jiang111 0:4c4775017e8a 1212 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1213 ThisThread::sleep_for(500);
jiang111 0:4c4775017e8a 1214 sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout);
jiang111 0:4c4775017e8a 1215 timeout=500;
jiang111 0:4c4775017e8a 1216 getcount=50;
jiang111 0:4c4775017e8a 1217 SendCMD();
jiang111 0:4c4775017e8a 1218 getreply();
jiang111 0:4c4775017e8a 1219 //*-*-*-5(replybuff);
jiang111 0:4c4775017e8a 1220 ThisThread::sleep_for(5000);
jiang111 0:4c4775017e8a 1221 //*-*-*-5("\n Getting Server IP \r\n");
jiang111 0:4c4775017e8a 1222 strcpy(cmdbuff, "AT+CIFSR\r\n");
jiang111 0:4c4775017e8a 1223 timeout=2500;
jiang111 0:4c4775017e8a 1224 getcount=200;
jiang111 0:4c4775017e8a 1225 while(weberror==0) {
jiang111 0:4c4775017e8a 1226 SendCMD();
jiang111 0:4c4775017e8a 1227 getreply();
jiang111 0:4c4775017e8a 1228 if (strstr(replybuff, "0.0.0.0") == NULL) {
jiang111 0:4c4775017e8a 1229 weberror=1; // wait for valid IP
jiang111 0:4c4775017e8a 1230 }
jiang111 0:4c4775017e8a 1231 }
jiang111 0:4c4775017e8a 1232 //*-*-*-5("\n Enter WEB address (IP) found below in your browser \r\n\n");
jiang111 0:4c4775017e8a 1233 //*-*-*-5("\n The MAC address is also shown below,if it is needed \r\n\n");
jiang111 0:4c4775017e8a 1234 replybuff[strlen(replybuff)-1] = '\0';
jiang111 0:4c4775017e8a 1235 //char* IP = replybuff + 5;
jiang111 0:4c4775017e8a 1236 sprintf(webdata,"%s", replybuff);
jiang111 0:4c4775017e8a 1237 //*-*-*-5(webdata);
jiang111 0:4c4775017e8a 1238 led2=1;
jiang111 0:4c4775017e8a 1239 bufflen=200;
jiang111 0:4c4775017e8a 1240 //bufflen=100;
jiang111 0:4c4775017e8a 1241 ount=0;
jiang111 0:4c4775017e8a 1242 //*-*-*-5("\n\n++++++++++ Ready ++++++++++\r\n\n");
jiang111 0:4c4775017e8a 1243 setup();
jiang111 0:4c4775017e8a 1244 esp.attach(&callback);
jiang111 0:4c4775017e8a 1245 } else {
jiang111 0:4c4775017e8a 1246 //*-*-*-5("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n");
jiang111 0:4c4775017e8a 1247 led1=1;
jiang111 0:4c4775017e8a 1248 led2=1;
jiang111 0:4c4775017e8a 1249 led3=1;
jiang111 0:4c4775017e8a 1250 led4=1;
jiang111 0:4c4775017e8a 1251 while(1) {
jiang111 0:4c4775017e8a 1252 led1=!led1;
jiang111 0:4c4775017e8a 1253 led2=!led2;
jiang111 0:4c4775017e8a 1254 led3=!led3;
jiang111 0:4c4775017e8a 1255 led4=!led4;
jiang111 0:4c4775017e8a 1256 ThisThread::sleep_for(1000);
jiang111 0:4c4775017e8a 1257 }
jiang111 0:4c4775017e8a 1258 }
jiang111 0:4c4775017e8a 1259 time2.reset();
jiang111 0:4c4775017e8a 1260 time2.start();
jiang111 0:4c4775017e8a 1261 }
jiang111 0:4c4775017e8a 1262
jiang111 0:4c4775017e8a 1263 // ESP Command data send
jiang111 0:4c4775017e8a 1264 void SendCMD()
jiang111 0:4c4775017e8a 1265 {
jiang111 0:4c4775017e8a 1266 esp.printf("%s", cmdbuff);
jiang111 0:4c4775017e8a 1267 }
jiang111 0:4c4775017e8a 1268 // Get Command and ESP status replies
jiang111 0:4c4775017e8a 1269 void getreply()
jiang111 0:4c4775017e8a 1270 {
jiang111 0:4c4775017e8a 1271 memset(replybuff, '\0', sizeof(replybuff));
jiang111 0:4c4775017e8a 1272 time1.reset();
jiang111 0:4c4775017e8a 1273 time1.start();
jiang111 0:4c4775017e8a 1274 replycount=0;
jiang111 0:4c4775017e8a 1275 while(time1.read_ms()< timeout && replycount < getcount) {
jiang111 0:4c4775017e8a 1276 if(esp.readable()) {
jiang111 0:4c4775017e8a 1277 replybuff[replycount] = esp.getc();
jiang111 0:4c4775017e8a 1278 replycount++;
jiang111 0:4c4775017e8a 1279 }
jiang111 0:4c4775017e8a 1280 }
jiang111 0:4c4775017e8a 1281 time1.stop();
jiang111 0:4c4775017e8a 1282 }
jiang111 0:4c4775017e8a 1283
jiang111 0:4c4775017e8a 1284 /* mainスレッド */
jiang111 0:4c4775017e8a 1285 int main() {
jiang111 0:4c4775017e8a 1286 /* 初期設定 */
jiang111 0:4c4775017e8a 1287 //wifi_thread = new Thread(wifi);
jiang111 0:4c4775017e8a 1288 //wifi_thread -> set_priority(osPriorityHigh);
jiang111 0:4c4775017e8a 1289 motorR2.period_us(40);
jiang111 0:4c4775017e8a 1290 motorR1.period_us(40);
jiang111 0:4c4775017e8a 1291 motorL2.period_us(40);
jiang111 0:4c4775017e8a 1292 motorL1.period_us(40);
jiang111 0:4c4775017e8a 1293 setup();
jiang111 0:4c4775017e8a 1294 avoi_thread = new Thread(avoidance);
jiang111 0:4c4775017e8a 1295 avoi_thread->terminate();
jiang111 0:4c4775017e8a 1296 trace_thread = new Thread(trace);
jiang111 0:4c4775017e8a 1297 trace_thread->terminate();
jiang111 0:4c4775017e8a 1298 mode = READY; // 現在待ちモード
jiang111 0:4c4775017e8a 1299 beforeMode = READY; // 前回待ちモード
jiang111 0:4c4775017e8a 1300 run = STOP; // 停止
jiang111 0:4c4775017e8a 1301 flag_sp = NORMAL; // スピード(普通)
jiang111 0:4c4775017e8a 1302 lcd.setBacklight(TextLCD::LightOn); // バックライトON
jiang111 0:4c4775017e8a 1303 lcd.setAddress(0,1);
jiang111 0:4c4775017e8a 1304 lcd.printf("Mode:ready");
jiang111 0:4c4775017e8a 1305 // display(); // ディスプレイ表示
jiang111 0:4c4775017e8a 1306
jiang111 0:4c4775017e8a 1307 while(1){
jiang111 0:4c4775017e8a 1308 bChange(); // バッテリー残量更新
jiang111 0:4c4775017e8a 1309 }
jiang111 0:4c4775017e8a 1310 }