siqiang pan / Mbed OS robotcar-team-0928

2

Dependencies:   RemoteIR TextLCD

main.cpp@73:8ce014cb07f1, 2020-09-28 (annotated)

Committer:
faker_71
Date:
Mon Sep 28 00:52:36 2020 +0000
Revision:
73:8ce014cb07f1
Parent:
72:d389adc0c780 
0928

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