mbed_robotcar / Mbed OS saikyo

linetrace saikyou

Dependencies:   RemoteIR TextLCD

main.cpp@54:f862caf08a7f, 2020-09-02 (annotated)

Committer:
nishimura_taku_pet
Date:
Wed Sep 02 07:20:41 2020 +0000
Revision:
54:f862caf08a7f
Parent:
53:debc8879ba5d
Child:
55:d3c19efa1ef9
bug fix 2

Who changed what in which revision?

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