siqiang pan / Mbed OS robotcar-team-0928

2

Dependencies:   RemoteIR TextLCD

main.cpp@67:0333997269dc, 2020-09-09 (annotated)

Committer:
nishimura_taku_pet
Date:
Wed Sep 09 00:21:38 2020 +0000
Revision:
67:0333997269dc
Parent:
66:2e2091dc7932
Child:
68:d9ce7c09b127
v 0.10 wifi fix

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