siqiang pan
2
Dependencies: RemoteIR TextLCD
main.cpp
- Committer:
- faker_71
- Date:
- 2020-09-28
- Revision:
- 73:8ce014cb07f1
- Parent:
- 72:d389adc0c780
File content as of revision 73:8ce014cb07f1:
/* mbed Microcontroller Library
* Copyright (c) 2019 ARM Limited
* SPDX-License-Identifier: Apache-2.0
*/
/*
Version 0.14
Date 2020/09/11
Uploader Taku Nishimura
更新するときにここも変える！
*/
// モーター電圧差チェック済み(PWM0.6)
// 2号機
// 4号機 右前進3.72V 右後退3.72V 左前進3.82V 左後退3.79V
// 6号機 右前進3.65V 右後退3.68V 左前進3.64V 左後退3.65V
// 9号機 右前進3.59V 右後退3.59V 左前進3.63V 左後退3.64V
#include "mbed.h"
#include "ReceiverIR.h"
#include "rtos.h"
#include <stdint.h>
#include "platform/mbed_thread.h"
#include "TextLCD.h"
RawSerial pc(USBTX, USBRX);
/* マクロ定義、列挙型定義 */
#define MIN_V 2.0 // 電圧の最小値
#define MAX_V 2.7 // 電圧の最大値
#define LOW 0 // モーターOFF
#define HIGH 1 // モーターON
#define NORMAL 0 // 普通
#define FAST 1 // 速い
#define VERYFAST 2 // とても速い
/* // 右回り37.21[s]
#define S 0.9
#define VF 1.2
#define REVERSE 0.5
#define MBED04 0.781 //Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
#define MBED05 0.953 //Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
#define MBED07 1
// MSR = Motor Speed Right
// MSL = Motor Speed Left
#define MSR0 0.5 // 基準速度Normal
#define MSR1 0.6 // 基準速度Fast
#define MSR2 0.7 // 基準速度VeryFast
*/
#define S 0.9 //ライントレースの旋回速度を遅くする
#define VF 1 //ライントレースの前進速度,旋回速度を速くする
#define REVERSE 0.5 //ライントレースの旋回速度(逆転のみ)を遅くする
// 使うロボットカーに合わせる
#define MBED01 1
#define MBED02 0.719 //Mbed02号機の左右のモーター速度比(R:L = 1: 0.719)
#define MBED03 1
#define MBED04 0.781 //Mbed04号機の左右のモーター速度比(R:L = 1: 0.781)
#define MBED05 0.947 //Mbed05号機の左右のモーター速度比(R:L = 1: 0.953)
#define MBED06 1
#define MBED07 1 //Mbed07号機の左右のモーター速度比(R:L = 1: 1.000)
#define MBED08 1
#define MBED09 1
#define MBED MBED05 //使うロボットカーの番号にする
// MSR = Motor Speed Right
// MSL = Motor Speed Left
#define MSR0 0.5 // 基準速度Normal PWM0.50
#define MSR1 0.65 // 基準速度Fast PWM0.65
#define MSR2 0.8 // 基準速度VeryFast PWM0.80
#define MSR3 MSR0*S // 低速旋回(Normalの時)
#define MSR4 MSR1*S*S // 低速旋回(Fastの時)
#define MSR5 MSR2*S*S // 低速旋回(VeryFastの時)
#define MSR6 MSR0*VF // 高速(Normalの時)
#define MSR7 MSR1*VF // 高速(Fastの時)
#define MSR8 MSR2*VF // 高速(VeryFastの時)
#define MSL0 MSR0*MBED
#define MSL1 MSR1*MBED
#define MSL2 MSR2*MBED
#define MSL3 MSR3*MBED
#define MSL4 MSR4*MBED
#define MSL5 MSR5*MBED
#define MSL6 MSR6*MBED
#define MSL7 MSR7*MBED
#define MSL8 MSR8*MBED
/* 操作モード定義 */
enum MODE{
READY = -1, // -1：待ち
ADVANCE = 1, // 1：前進
RIGHT, // 2：右折
LEFT, // 3：左折
BACK, // 4：後退
STOP, // 5：停止
LINE_TRACE, // 6：ライントレース
AVOIDANCE, // 7：障害物回避
SPEED, // 8：スピード制御
LT_R, // 9:低速右折(ライントレース時)
LT_L, // 10:低速左折(ライントレース時)
A1RIGHT,
A1LEFT,
A2RIGHT,
A2LEFT,
};
/* ピン配置 */
ReceiverIR ir(p5); // リモコン操作
DigitalOut trig(p6); // 超音波センサtrigger
DigitalIn echo(p7); // 超音波センサecho
DigitalIn ss1(p8); // ライントレースセンサ(左)
DigitalIn ss2(p9); // ライントレースセンサ
DigitalIn ss3(p10); // ライントレースセンサ
DigitalIn ss4(p11); // ライントレースセンサ
DigitalIn ss5(p12); // ライントレースセンサ(右)
RawSerial esp(p13, p14); // Wi-Fiモジュール(tx, rx)
AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
PwmOut motorR2(p22); // 右モーター後退
PwmOut motorR1(p21); // 右モーター前進
PwmOut motorL2(p23); // 左モーター後退
PwmOut motorL1(p24); // 左モーター前進
PwmOut servo(p25); // サーボ
I2C i2c_lcd(p28,p27); // LCD(tx, rx)
/* 変数宣言 */
MODE mode; // 操作モード
int run; // 走行状態
MODE beforeMode; // 前回のモード
int flag_sp = 0; // スピード変化フラグ
Timer viewTimer; // スピ―ド変更時に3秒計測タイマー
float motorSpeedR1[9] = {MSR0, MSR1, MSR2, MSR3 , MSR4 , MSR5 , MSR6 , MSR7 , MSR8 };
//float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6*REVERSE, MSR7*REVERSE, MSR8*REVERSE};
float motorSpeedR2[9] = {MSR0, MSR1, MSR2, MSR3*REVERSE, MSR4*REVERSE, MSR5*REVERSE, MSR6, MSR7, MSR8};
float motorSpeedL1[9] = {MSL0, MSL1, MSL2, MSL3 , MSL4 , MSL5 , MSL6 , MSL7 , MSL8 };
//float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6*REVERSE, MSL7*REVERSE, MSL8*REVERSE};
float motorSpeedL2[9] = {MSL0, MSL1, MSL2, MSL3*REVERSE, MSL4*REVERSE, MSL5*REVERSE, MSL6, MSL7, MSL8};
// モーター速度設定(3以降はライントレース用)
// 0,1,2:基準速度
// 3,4,5:低速
// 6,7,8:高速
// R1 : 右前, R2 : 右後, L1 : 左前, L2 : 左後
Mutex mutex; // ミューテックス
/* decodeIR用変数 */
RemoteIR::Format format;
uint8_t buf[32];
uint32_t bitcount;
uint32_t code;
/* bChange, lcdbacklight用変数 */
TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
int b10[10]; // バッテリー残量
int b = 0;
int k = 0;
int flag_b = 0; // バックライト点滅フラグ
int flag_t = 0; // バックライトタイマーフラグ
/* trace用変数 */
int sensArray[32] = {0,6,2,4,1,4,2,4, // ライントレースセンサパターン
3,4,1,6,3,1,1,6, // 0:前回動作継続
7,1,5,1,5,1,1,1, // 1:高速前進
5,1,7,1,5,1,7,1}; // 2:低速右折
// 3:低速左折
// 4:中速右折
// 5:中速左折
// 6:高速右折
// 7:高速左折
//int sensArray[32] = {0,6,0,4,1,0,1,4, // ライントレースセンサパターン
// 0,6,1,6,1,1,1,6, // 0:前回動作継続
// 7,1,7,1,0,1,1,1, // 1:高速前進
// 5,1,7,1,5,1,7,1}; // 2:低速右折
// // 3:低速左折
// // 4:中速右折
// // 5:中速左折
// // 6:高速右折
// // 7:高速左折
/* avoidance用変数 */
Timer timer; // 距離計測用タイマ
int DT; // 距離
int SC; // 正面
int SL; // 左
int SR; // 右
int SLD; // 左前
int SRD; // 右前
int flag_a = 0; // 障害物有無のフラグ
int limit = 25; // 障害物の距離のリミット(単位：cm)
int backlimit = 20;
int sidelimit = 18;
int far; // 最も遠い距離
int houkou; // 進行方向(1:前 2:左 3:右)
int t1 = 0;
/*WiFi用変数*/
Timer time1;
Timer time2;
int bufflen, DataRX, ount, getcount, replycount, servreq, timeout;
int bufl, ipdLen, linkID, weberror, webcounter,click_flag;
float R1=100000, R2=10000; // resistor values to give a 10:1 reduction of measured AnalogIn voltage
char webcount[8];
char type[16];
char channel[2];
char cmdbuff[32];
char replybuff[1024];
char webdata[1024]; // This may need to be bigger depending on WEB browser used
char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added
char httpbuff[4096];
int port =80; // set server port
int SERVtimeout =5; // set server timeout in seconds in case link breaks.
char ssid[32] = "mbed05"; // enter WiFi router ssid inside the quotes
char pwd [32] = "0123456789a"; // enter WiFi router password inside the quotes
bool click_log = false;
DigitalOut pred(p16);
DigitalOut pblue(p19);
/* プロトタイプ宣言 */
void decodeIR(/*void const *argument*/);
void motor(/*void const *argument*/);
void changeSpeed();
void avoidance(/*void const *argument*/);
void trace(/*void const *argument*/);
void trace_avoid();
void watchsurrounding3();
void watchsurrounding5();
int watch();
char battery_ch[8];
void bChange();
void display();
void lcdBacklight(void const *argument);
void SendCMD(),getreply(),ReadWebData(),startserver(),sendpage(),SendWEB(),sendcheck();
//void wifi(/*void const *argument*/);
Thread *deco_thread; // decodeIRをスレッド化 ：+3
//Thread *wifi_thread;
Thread *motor_thread; // motorをスレッド化 ：+2
RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定
Thread *avoi_thread; // avoidanceをスレッド化：+2
Thread *trace_thread; // traceをスレッド化 ：+2
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
void setup(){
deco_thread = new Thread(decodeIR);
deco_thread -> set_priority(osPriorityRealtime);
motor_thread = new Thread(motor);
motor_thread -> set_priority(osPriorityHigh);
//wifi_thread -> set_priority(osPriorityRealtime);
display();
}
/* リモコン受信スレッド */
void decodeIR(/*void const *argument*/){
while(1){
// 受信待ち
if (ir.getState() == ReceiverIR::Received){ // コード受信
bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
if(bitcount > 1){ // 受信成功
code=0;
for(int j = 0; j < 4; j++){
code+=(buf[j]<<(8*(3-j)));
}
if(mode != SPEED){ // スピードモード以外なら
beforeMode=mode; // 前回のモードに現在のモードを設定
}
switch(code){
case 0x40bf27d8: // クイック
mode = SPEED; // スピードモード
changeSpeed(); // 速度変更
display(); // ディスプレイ表示
mode = beforeMode; // 現在のモードに前回のモードを設定
break;
case 0x40be34cb: // レグザリンク
if(trace_thread->get_state() == Thread::Deleted){
delete trace_thread;
trace_thread = new Thread(trace);
trace_thread -> set_priority(osPriorityHigh);
}
mode=LINE_TRACE; // ライントレースモード
display(); // ディスプレイ表示
break;
case 0x40bf6e91: // 番組表
if(avoi_thread->get_state() == Thread::Deleted){
delete avoi_thread;
avoi_thread = new Thread(avoidance);
avoi_thread -> set_priority(osPriorityHigh);
}
flag_a = 0;
mode=AVOIDANCE; // 障害物回避モード
run = ADVANCE; // 前進
display(); // ディスプレイ表示
break;
case 0x40bf3ec1: // ↑
mode = ADVANCE; // 前進モード
run = ADVANCE; // 前進
display(); // ディスプレイ表示
break;
case 0x40bf3fc0: // ↓
mode = BACK; // 後退モード
run = BACK; // 後退
display(); // ディスプレイ表示
break;
case 0x40bf5fa0: // ←
mode = LEFT; // 左折モード
run = LEFT; // 左折
display(); // ディスプレイ表示
break;
case 0x40bf5ba4: // →
mode = RIGHT; // 右折モード
run = RIGHT; // 右折
display(); // ディスプレイ表示
break;
case 0x40bf3dc2: // 決定
mode = STOP; // 停止モード
run = STOP; // 停止
display(); // ディスプレイ表示
break;
default:
;
}
if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
trace_thread->terminate();
}
if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
avoi_thread->terminate();
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
}
}
}
if(viewTimer.read_ms()>=3000){ // スピードモードのまま3秒経過
viewTimer.stop(); // タイマーストップ
viewTimer.reset(); // タイマーリセット
display(); // ディスプレイ表示
}
switch(flag_sp){
case 1:
limit = 25;
backlimit = 20;
sidelimit = 18;
break;
case 2:
limit = 30;
backlimit = 18;
sidelimit = 21;
break;
case 3:
limit = 35;
backlimit = 15;
sidelimit = 24;
break;
}
ThisThread::sleep_for(90); // 90ms待つ
}
}
/* モーター制御スレッド */
void motor(/*void const *argument*/){
while(1){
/* 走行状態の場合分け */
if(mode != LINE_TRACE){ // スピード変更フラグが2より大きいなら
flag_sp %= 3; // スピード変更フラグ調整
}
//switch(flag_sp){
// case 1:
// limit = 25;
// backlimit = 20;
// break;
// case 2:
// limit = 30;
// backlimit = 18;
// break;
// case 3:
// limit = 35;
// backlimit = 15;
// break;
// }
switch(run){
/* 前進 */
case ADVANCE:
motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
/* 右折 */
case RIGHT:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
/* 左折 */
case LEFT:
motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
break;
/* 後退 */
case BACK:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = motorSpeedR2[flag_sp]; // 右後退モーターON
motorL1 = LOW; // 左前進モーターOFF
motorL2 = motorSpeedL2[flag_sp]; // 左後退モーターON
break;
/* 停止 */
case STOP:
motorR1 = LOW; // 右前進モーターOFF
motorR2 = LOW; // 右後退モーターOFF
motorL1 = LOW; // 左前進モーターOFF
motorL2 = LOW; // 左後退モーターOFF
break;
case A1RIGHT:
motorR1 = LOW;
motorR2 = 0.55;
motorL1 = 0.57;
motorL2 = LOW;
break;
case A1LEFT:
motorR1 = 0.57;
motorR2 = LOW;
motorL1 = LOW;
motorL2 = 0.55;
break;
case A2RIGHT:
motorR1 = LOW;
motorR2 = 0.33;
motorL1 = 0.35;
motorL2 = LOW;
break;
case A2LEFT:
motorR1 = 0.35;
motorR2 = LOW;
motorL1 = LOW;
motorL2 = 0.33;
break;
/* 低速右折 */
case LT_R:
motorR1 = motorSpeedR2[flag_sp]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL1[flag_sp]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
/* 低速左折 */
case LT_L:
motorR1 = motorSpeedR1[flag_sp]; // 右前進モーターON
motorR2 = LOW; // 右後退モーターOFF
motorL1 = motorSpeedL2[flag_sp]; // 左前進モーターON
motorL2 = LOW; // 左後退モーターOFF
break;
}
/*if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら
flag_sp %= 3; // スピード変更フラグ調整
}*/
ThisThread::sleep_for(3); // 30ms待つ
}
}
/* スピード変更関数 */
void changeSpeed(){
if(flag_sp%3 == 2){ // スピード変更フラグを3で割った余りが2なら
flag_sp = 0; // スピード変更フラグを0にする
}else{ // それ以外
flag_sp = flag_sp + 1; // スピード変更フラグを+1
}
}
/* ライントレーススレッド */
//void trace(){
// // PID用
// while(1){
// ThisThread::sleep_for(3);
// }
//}
void tran_RL(int tr){
if (tr==0){
//left
motorR1 = 0.2; // 右前進モーターON
motorR2 = 0; // 右後退モーターOFF
motorL1 = 0; // 左前進モーターON
motorL2 = 0.2; // 左後退モーターOFF
}else if(tr==3){
motorR1 = 0; // 右前進モーターON
motorR2 = 0; // 右後退モーターOFF
motorL1 = 0; // 左前進モーターON
motorL2 = 0; // 左後退モーターOFF
}else{
//right
motorR1 = 0; // 右前進モーターON
motorR2 = 0.2; // 右後退モーターOFF
motorL1 = 0.2; // 左前進モーターON
motorL2 = 0; // 左後退モーターOFF
}
}
int fond=0;
int DistanceMid,DistanceRM,DistanceLM,nowDistance;
int turn;//0 left 1 right
int mid_po=1400;
//0917
void trace_avoid(){
run=100;
//servo.pulsewidth_us(1450);
//ThisThread::sleep_for(100);
DistanceMid = watch();
if ((DistanceMid<20) && (DistanceMid>0)){
//run = STOP;
tran_RL(3);
led1!=led1;
led2!=led2;
led3!=led3;
//左前,右前
servo.pulsewidth_us(mid_po+300);
ThisThread::sleep_for(300);
DistanceLM=watch();
servo.pulsewidth_us(mid_po-300);//右qian
ThisThread::sleep_for(300);
DistanceRM=watch();
if(DistanceRM>DistanceLM){
//go through left side
turn=1;
servo.pulsewidth_us(mid_po+800);
ThisThread::sleep_for(300);
while(watch()>20 || watch()<0){
led1=!led1;
tran_RL(turn);
}
}else{
//go through right side+900
turn=0;
servo.pulsewidth_us(mid_po-830);//左直角
ThisThread::sleep_for(300);
while(watch()>25 || watch()<0){
tran_RL(turn);
}
}
//preDistance=watch();
while(1){
nowDistance=watch();
//if(watch()>nowDistance){
// nowDistance=watch();
ThisThread::sleep_for(10);
if(watch()>nowDistance){
nowDistance=watch();
ThisThread::sleep_for(10);
if(watch()>nowDistance){
tran_RL(3);
break;
}
}
//}
}
while(!fond){
while(watch()>0 && watch()<30){
pc.printf("++go through+++++++++++++++++%d\r\n",watch());
int sensor3 = ss3;
//直行
motorR1 = 0.2; // 右前進モーターON
motorR2 = 0; // 右後退モーターOFF
motorL1 = 0.2; // 左前進モーターON
motorL2 = 0; // 左後退モーターOFF
//sen3黑
if(sensor3==1){
fond=1;
tran_RL(3);
servo.pulsewidth_us(1450);
ThisThread::sleep_for(300000);
tran_RL(turn);
break;
}
if(watch()>40){
ThisThread::sleep_for(10);
if (watch()>40){
ThisThread::sleep_for(10);
if (watch()>40){
pc.printf("out is ok^^^^^^^^^^^^^^^^^^^^^^%d\r\n",watch());
}
}
}
}
while(!fond){
//缓慢右转
tran_RL(!turn);
nowDistance=watch();
ThisThread::sleep_for(10);
if(watch()>nowDistance){
nowDistance=watch();
ThisThread::sleep_for(10);
if(watch()>nowDistance){
//nowDistance=watch();
break;
/* if(watch()>nowDistance){
tran_RL(3);
break;
}*/
}
}
}
}
}
}
void trace(){
while(1){
/* 各センサー値読み取り */
int sensor1 = ss1;
int sensor2 = ss2;
int sensor3 = ss3;
int sensor4 = ss4;
int sensor5 = ss5;
int sensD = 0;
//led1=0;
//trace_avoid();
/* センサー値の決定 */
if(sensor1 > 0) sensD |= 0x10;
if(sensor2 > 0) sensD |= 0x08;
if(sensor3 > 0) sensD |= 0x04;
if(sensor4 > 0) sensD |= 0x02;
if(sensor5 > 0) sensD |= 0x01;
/* センサー値によって場合分け */
switch(sensArray[sensD]){
case 1:
flag_sp = flag_sp % 3 + 6;
run = ADVANCE; // 高速で前進
break;
case 2:
flag_sp = flag_sp % 3 + 3;
// run = RIGHT; // 低速で右折
run = LT_R; // 低速で右折
break;
case 3:
flag_sp = flag_sp % 3 + 3;
// run = LEFT; // 低速で左折
run = LT_L; // 低速で左折
break;
case 4:
flag_sp = flag_sp % 3;
run = RIGHT; // 中速で右折
break;
case 5:
flag_sp = flag_sp % 3;
run = LEFT; // 中速で左折
break;
case 6:
flag_sp = flag_sp % 3 + 6;
run = RIGHT; // 高速で右折
break;
case 7:
flag_sp = flag_sp % 3 + 6;
run = LEFT; // 高速で左折
break;
default:
break; // 前回動作を継続
}
// ThisThread::sleep_for(30); // 30ms待つ
ThisThread::sleep_for(3);
}
}
/* 障害物回避走行スレッド */
void avoidance(){
while(1){
watchsurrounding3();
if(flag_a == 0){ // 障害物がない場合
run = ADVANCE; // 前進
}else{ // 障害物がある場合
int i = 0;
int cnt_kyori;
int kyori;
if(SC < 15){
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
ThisThread::sleep_for(100); // 100ms待つ
run = STOP;
ThisThread::sleep_for(80);
run = BACK;
cnt_kyori = 0;
kyori = watch();
while(kyori < backlimit){ // 正面20cm以内に障害物がある間
if(kyori == -1){
cnt_kyori++;
if(cnt_kyori > 15){
cnt_kyori = 0;
break;
}
}
kyori = watch();
}
run = STOP;
}
watchsurrounding5();
if(SC < 27 && SLD < 38 && SL < 31 && SRD < 38 && SR < 31){ // 全方向に障害物がある場合
run = A1LEFT; // 左折
cnt_kyori = 0;
kyori = watch();
while(kyori < 53){
if(kyori == -1){
cnt_kyori++;
if(cnt_kyori > 25){
cnt_kyori = 0;
break;
}
}
kyori = watch();
}
// while(i < 15){ // 進行方向確認
// if(watch() > limit){
// break;
// }else{
// i++;
// }
// }
run = STOP; // 停止
}else { // 全方向以外
far = SC; // 正面を最も遠い距離に設定
houkou = 1; // 進行方向を前に設定
if(SC < 25){
if(far < SLD || far < SL){ // 左または左前がより遠い場合
if(SL < SLD){ // 左前が左より遠い場合
far = SLD; // 左前を最も遠い距離に設定
}else{ // 左が左前より遠い場合
far = SL; // 左を最も遠い距離に設定
}
houkou = 2; // 進行方向を左に設定
}
if(far < SRD || far < SR){ // 右または右前がより遠い場合
if(SR < SRD){ // 右前が右より遠い場合
far = SRD; // 右前を最も遠い距離に設定
}else{ // 右が右前よりも遠い場合
far = SR; // 右を最も遠い距離に設定
}
houkou = 3; // 進行方向を右に設定
}
}
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
switch(flag_sp){
case 0:
ThisThread::sleep_for(300);
break;
case 1:
ThisThread::sleep_for(200);
break;
case 2:
ThisThread::sleep_for(100);
break;
}
break;
case 2: // 左の場合
run = A1LEFT; // 左折
while(i < 15){ // 進行方向確認
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
break;
}else{
i++;
}
}
run = ADVANCE;
break;
case 3: // 右の場合
run = A1RIGHT; // 右折
while(i < 15){ // 進行方向確認
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
break;
}else{
i++;
}
}
run = ADVANCE;
break;
}
}
}
flag_a = 0; // 障害物有無フラグを0にセット
if(SLD < 30 && SRD > 30){
run = A2RIGHT; // 右折
ThisThread::sleep_for(100);
run = ADVANCE;
}else if(SRD < 30 && SLD > 30){
run = A2LEFT; // 左折
ThisThread::sleep_for(100);
run = ADVANCE;
}
}
}
/* 距離計測関数 */
int watchpsq(){
do{
trig = 0;
ThisThread::sleep_for(5); // 5ms待つ
trig = 1;
ThisThread::sleep_for(15); // 15ms待つ
trig = 0;
timer.start();
t1=timer.read_ms();
while(echo.read() == 0 && t1<10){
t1=timer.read_ms();
//led1 = 1;
}
timer.stop();
timer.reset();
}while(t1 >= 10);
timer.start(); // 距離計測タイマースタート
while(echo.read() == 1){
}
timer.stop(); // 距離計測タイマーストップ
DT = (int)(timer.read_us()*0.01657); // 距離計算
/*
if(timer.read_ms() > 1000){
DT = -1;
break;
}
*/
if(DT > 600){
DT = 600;
}
timer.reset();
//led1 = 0;
return DT;
}
/* 距離計測関数 */
int watch(){
do{
do{
trig = 0;
ThisThread::sleep_for(5); // 5ms待つ
trig = 1;
ThisThread::sleep_for(15); // 15ms待つ
trig = 0;
timer.start();
t1=timer.read_ms();
while(echo.read() == 0 && t1<10){
t1=timer.read_ms();
led1 = 1;
}
timer.stop();
timer.reset();
}while(t1 >= 10);
timer.start(); // 距離計測タイマースタート
while(echo.read() == 1){
}
timer.stop(); // 距離計測タイマーストップ
DT = (int)(timer.read_us()*0.01657); // 距離計算
if(timer.read_ms() > 1000){
DT = -1;
break;
}
}while(DT > 1000);
if(DT > 400){
DT = 400;
}
timer.reset();
led1 = 0;
return DT;
}
/* 障害物検知関数 */
void watchsurrounding3(){
SC = watch();
if(SC < limit){ // 正面20cm以内に障害物がある場合
if(SC!=-1){
run = STOP; // 停止
flag_a = 1;
return;
}
}
servo.pulsewidth_us(1930); // サーボを左に40度回転
ThisThread::sleep_for(100);
SLD = watch();
if(SLD < sidelimit){ // 左前20cm以内に障害物がある場合
run = STOP;
ThisThread::sleep_for(90);
run = BACK;
switch(flag_sp){
case 0:
ThisThread::sleep_for(200);
break;
case 1:
ThisThread::sleep_for(170);
break;
case 2:
ThisThread::sleep_for(140);
break;
}
run = A2RIGHT;
ThisThread::sleep_for(150);
return;
}
servo.pulsewidth_us(1450);
ThisThread::sleep_for(100);
SC = watch();
if(SC < limit){
if(SC != -1){
run = STOP; // 停止
flag_a = 1;
return;
}
}
servo.pulsewidth_us(920); // サーボを右に40度回転
ThisThread::sleep_for(100);
SRD = watch();
if(SRD < sidelimit){ // 右前20cm以内に障害物がある場合
run = STOP; // 停止
ThisThread::sleep_for(90);
run = BACK;
switch(flag_sp){
case 0:
ThisThread::sleep_for(200);
break;
case 1:
ThisThread::sleep_for(170);
break;
case 2:
ThisThread::sleep_for(140);
break;
}
run = A2LEFT;
ThisThread::sleep_for(150);
return;
}
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
ThisThread::sleep_for(100); // 100ms待つ
}
/* 障害物検知関数 */
void watchsurrounding5(){
SC = watch();
servo.pulsewidth_us(1927); // サーボを左に40度回転
ThisThread::sleep_for(100); // 250ms待つ
SLD = watch();
servo.pulsewidth_us(2400); // サーボを左に90度回転
ThisThread::sleep_for(100); // 250ms待つ
SL = watch();
servo.pulsewidth_us(1450);
ThisThread::sleep_for(250);
SC = watch();
servo.pulsewidth_us(923); // サーボを右に40度回転
ThisThread::sleep_for(100); // 250ms待つ
SRD = watch();
servo.pulsewidth_us(500); // サーボを右に90度回転
ThisThread::sleep_for(100); // 250ms待つ
SR = watch();
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
ThisThread::sleep_for(250); // 100ms待つ
}
/* ディスプレイ表示関数 */
void display(){
// pblue=!pblue;
mutex.lock(); // ミューテックスロック
lcd.setAddress(0,1);
/* 操作モードによる場合分け */
switch(mode){
/* 前進 */
case ADVANCE:
lcd.printf("Mode:Advance ");
break;
/* 右折 */
case RIGHT:
lcd.printf("Mode:TurnRight ");
break;
/* 左折 */
case LEFT:
lcd.printf("Mode:TurnLeft ");
break;
/* 後退 */
case BACK:
lcd.printf("Mode:Back ");
break;
/* 停止 */
case STOP:
lcd.printf("Mode:Stop ");
break;
/* 待ち */
case READY:
lcd.printf("Mode:Ready ");
break;
/* ライントレース */
case LINE_TRACE:
lcd.printf("Mode:LineTrace ");
break;
/* 障害物回避 */
case AVOIDANCE:
lcd.printf("Mode:Avoidance ");
break;
/* スピード制御 */
case SPEED:
/* スピードの状態で場合分け */
switch(flag_sp % 3){
/* 普通 */
case(NORMAL):
lcd.printf("Speed:Normal ");
break;
/* 速い */
case(FAST):
lcd.printf("Speed:Fast ");
break;
/* とても速い */
case(VERYFAST):
lcd.printf("Speed:VeryFast ");
break;
}
viewTimer.reset(); // タイマーリセット
viewTimer.start(); // タイマースタート
break;
default:
lcd.printf("Mode:ModeError ");
break;
}
mutex.unlock(); // ミューテックスアンロック
// pblue=!pblue;
}
/* バックライト点滅 */
void lcdBacklight(void const *argument){
if(flag_b == 1){ // バックライト点滅フラグが1なら
lcd.setBacklight(TextLCD::LightOn); // バックライトON
}else{ // バックライト点滅フラグが0なら
lcd.setBacklight(TextLCD::LightOff); // バックライトOFF
}
flag_b = !flag_b; // バックライト点滅フラグ切り替え
}
/* バッテリー残量更新関数 */
void bChange(){
//pc.printf(" bChange1\r\n");
b10[k++] = (int)(((battery.read() * 3.3 - MIN_V)/0.8)*10+0.5)*10;
if(k == 10){
k = 0;
}
b = 0;
for(int i=0;i<10;i++){
if(b<b10[i] && b10[i]<=100){
b = b10[i];
}
else if(b10[i] > 100){
b = 100;
}
}
if(b <= 0){ // バッテリー残量0%なら全ての機能停止
if(flag_t == 1){
bTimer.stop();
}
lcd.cls();
if(trace_thread->get_state() != Thread::Deleted){
trace_thread->terminate();
}
if(avoi_thread->get_state() != Thread::Deleted){
avoi_thread->terminate();
}
//if(wifi_thread->get_state() != Thread::Deleted){
// wifi_thread->terminate();
//}
if(deco_thread != NULL){
deco_thread->terminate();
}
run = STOP;
ThisThread::sleep_for(10);
if(motor_thread != NULL){
motor_thread -> terminate();
}
led1 = 0;
led2 = 0;
led3 = 0;
led4 = 0;
exit(0); // 電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。
}
mutex.lock(); // ミューテックスロック
lcd.setAddress(0,0);
lcd.printf("Battery:%3d%%",b); // バッテリー残量表示
mutex.unlock(); // ミューテックスアンロック
if(b <= 30){ // バッテリー残量30%以下なら
if(flag_t == 0){ // バックライトタイマーフラグが0なら
//bTimer.attach(lcdBacklight,0.5);
bTimer.start(500); // 0.5秒周期でバックライトタイマー割り込み
flag_t = 1; // バックライトタイマーフラグを1に切り替え
}
}else{
if(flag_t == 1){ // バックライトタイマーフラグが1なら
//bTimer.detach();
bTimer.stop(); // バックライトタイマーストップ
lcd.setBacklight(TextLCD::LightOn); // バックライトON
flag_t = 0; // バックライトタイマーフラグを0に切り替え
}
}
}
// Serial Interrupt read ESP data
void callback()
{
////*-*-*-5("\n\r------------ callback is being called --------------\n\r");
led3=1;
while (esp.readable()) {
webbuff[ount] = esp.getc();
ount++;
}
if(strlen(webbuff)>bufflen) {
// //*-*-*-5("\f\n\r------------ webbuff over bufflen --------------\n\r");
DataRX=1;
led3=0;
}
}
void wifi(/*void const *argument*/)
{
//*-*-*-5("\f\n\r------------ ESP8266 Hardware Reset psq --------------\n\r");
ThisThread::sleep_for(100);
led1=1,led2=0,led3=0;
timeout=6000;
getcount=500;
getreply();
esp.baud(115200); // ESP8266 baudrate. Maximum on KLxx' is 115200, 230400 works on K20 and K22F
startserver();
while(1) {
if(DataRX==1) {
//*-*-*-5("\f\n\r------------ main while > if --------------\n\r");
// click_flag = 1;
click_log=false;
ReadWebData();
//*-*-*-5("\f\n\r------------ click_flag=%d --------------\n\r",click_flag);
//if ((servreq == 1 && weberror == 0) && click_flag == 1) {
if (servreq == 1 && weberror == 0 && click_log == false) {
//*-*-*-5("\f\n\r------------ befor send page --------------\n\r");
sendpage(); }
//*-*-*-5("\f\n\r------------ send_check begin --------------\n\r");
//sendcheck();
//*-*-*-5("\f\n\r------------ ssend_check end--------------\n\r");
memset(webbuff, '\0', sizeof(webbuff));
esp.attach(&callback);
//*-*-*-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);
//*-*-*-5("\n\n HTTP Packet: \n\n%s\n", webdata);
//*-*-*-5(" Web Characters sent : %d\n\n", bufl);
//*-*-*-5(" -------------------------------------\n\n");
servreq=0;
}
ThisThread::sleep_for(100);
}
}
// Static WEB page
void sendpage()
{
// WEB page data
//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");
strcpy(webbuff, "<!DOCTYPE html>");
strcat(webbuff, "<html><head><title>RobotCarControl</title><meta name='viewport' content='width=device-width'/>");
strcat(webbuff, "<meta http-equiv=\"refresh\" content=\"10\"; >");
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>");
strcat(webbuff, "</head><body><center><p><strong>RobotCarControl");
strcat(webbuff, "</strong></p><td style='vertical-align:top;'><strong>Battery :");
if(b<=30){ //30%より下の場合残電量を赤文字
strcat(webbuff, "<font color=\"red\">");
sprintf(webbuff, "%s%3d", webbuff, b);
strcat(webbuff, "</font>");
}else{
sprintf(webbuff, "%s%3d", webbuff, b);
}
strcat(webbuff, " %</strong>");
strcat(webbuff, "<button id=\"reloadbtn\" type=\"button\" class=\"load\" onmousedown=\"location.reload()\">Reload</button>");
strcat(webbuff, "</td></p>");
strcat(webbuff, "<br>");
strcat(webbuff, "<table><tr><td></td><td>");
switch(mode) { //ブラウザ更新時の現在の車の状態からボタンの点灯判定
case ADVANCE: //前進
strcat(webbuff, "<button id='gobtn' type='button' class=\"light\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case LEFT: //左折
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"light\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case STOP: //停止
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"light\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case RIGHT: //右折
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"light\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case BACK: //後進
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"light\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case AVOIDANCE: //障害物回避
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"light\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
case LINE_TRACE: //ライントレース
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"light\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
default: //その他
strcat(webbuff, "<button id='gobtn' type='button' class=\"noselect\" value=\"GO\" onmousedown='send_mes(this.value)'>Go");
strcat(webbuff, "</button></td><td></td></tr><tr><td>");
strcat(webbuff, "<button id='leftbtn' type='button' class=\"noselect\" value=\"LEFT\" onmousedown='send_mes(this.value)' >Left");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='stopbtn' type='button' class=\"noselect\" value=\"STOP\" onmousedown='send_mes(this.value)' >Stop");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='rightbtn' type='button' class=\"noselect\" value=\"RIGHT\" onmousedown='send_mes(this.value)' >Right");
strcat(webbuff, "</button></td></tr><td></td><td>");
strcat(webbuff, "<button id='backbtn' type='button' class=\"noselect\" value=\"BACK\" onmousedown='send_mes(this.value)' >Back");
strcat(webbuff, "</button></td><td style='vertical-align:top; text-align:right;'></td></tr></table>");
strcat(webbuff, "<strong>Mode</strong>");
strcat(webbuff, "<table><tr><td><button id='avoidbtn' type='button' class=\"noselect\" value=\"AVOIDANCE\" onmousedown='send_mes(this.value)' >");
strcat(webbuff, "Avoidance</button></td><td>");
strcat(webbuff, "<button id='tracebtn' type='button' class=\"noselect\" value=\"LINE_TRACE\" onmousedown='send_mes(this.value)' >LineTrace");
break;
}
strcat(webbuff, "</button></td></tr></table>");
strcat(webbuff, "<strong>Speed</strong>");
strcat(webbuff, "<table><tr><td>");
//ready示速度だけ点灯
switch (flag_sp%3) { //現在の速度のボタン表示
case 0: //ノーマル
strcat(webbuff, "<button id='sp1btn' type='button' class=\"light\" value=\"Normal\" onmousedown='send_mes_spe(this.value)' >Normal");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onmousedown='send_mes_spe(this.value)' >Fast");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onmousedown='send_mes_spe(this.value)' >VeryFast");
break;
case 1: //ファスト
strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onmousedown='send_mes_spe(this.value)' >Normal");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp2btn' type='button' class=\"light\" value=\"Fast\" onmousedown='send_mes_spe(this.value)' >Fast");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onmousedown='send_mes_spe(this.value)' >VeryFast");
break;
case 2: //ベリーファスト
strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onmousedown='send_mes_spe(this.value)' >Normal");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onmousedown='send_mes_spe(this.value)' >Fast");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp3btn' type='button' class=\"light\" value=\"VeryFast\" onmousedown='send_mes_spe(this.value)' >VeryFast");
break;
default: //その他
strcat(webbuff, "<button id='sp1btn' type='button' class=\"noselect\" value=\"Normal\" onmousedown='send_mes_spe(this.value)' >Normal");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp2btn' type='button' class=\"noselect\" value=\"Fast\" onmousedown='send_mes_spe(this.value)' >Fast");
strcat(webbuff, "</button></td><td>");
strcat(webbuff, "<button id='sp3btn' type='button' class=\"noselect\" value=\"VeryFast\" onmousedown='send_mes_spe(this.value)' >VeryFast");
break;
}
strcat(webbuff, "</button></td></tr></table>");
strcat(webbuff, "</center>");
strcat(webbuff, "</body>");
strcat(webbuff, "</html>");
strcat(webbuff, "<script language=\"javascript\" type=\"text/javascript\">"); //機能
//ボタン入力時それぞれの関数から呼び出されサーバーとの通信を行う
strcat(webbuff, "function htmlacs(url) {");
strcat(webbuff, "var xhr = new XMLHttpRequest();");
strcat(webbuff, "xhr.open(\"GET\", url, true);");
strcat(webbuff, "xhr.send();");
strcat(webbuff, "setTimeout(function(){xhr.abort();},100);");
strcat(webbuff, "}");
//mode変更ボタン入力時動作 //sendmes
strcat(webbuff, "function send_mes(btnval){"); //mode変更ボタン入力時の点灯消灯判定
strcat(webbuff, "console.log(btnval);");
strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
strcat(webbuff, "htmlacs(url);");
strcat(webbuff, "console.log(url);");
strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
strcat(webbuff, "for(var i=1;i<8;i++){");
strcat(webbuff, "if(buttons[i].value == btnval){");
strcat(webbuff, "buttons[i].className=\"light\";");
strcat(webbuff, "}else{");
strcat(webbuff, "buttons[i].className=\"noselect\";");
strcat(webbuff, "}");
strcat(webbuff, "}");
strcat(webbuff, "}");
strcat(webbuff, "function send_mes_spe(btnval){"); //speed変更ボタン入力時の点灯消灯判定
strcat(webbuff, "var url = \"http://\" + window.location.hostname + \"/cargo?a=\" + btnval;");
strcat(webbuff, "htmlacs(url);");
strcat(webbuff, "console.log(url);");
strcat(webbuff, "var buttons = document.getElementsByTagName(\"button\");");
strcat(webbuff, "for(var i=8;i<11;i++){");
strcat(webbuff, "if(buttons[i].value == btnval){");
strcat(webbuff, "buttons[i].className=\"light\";");
strcat(webbuff, "}else{");
strcat(webbuff, "buttons[i].className=\"noselect\";");
strcat(webbuff, "}");
strcat(webbuff, "}");
strcat(webbuff, "}");
strcat(webbuff, "</script>");
bufl = strlen(webbuff); // get total page buffer length
sprintf(httpbuff, "%s",webbuff);
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);
//pc.printf("\r\n*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-\r\n%s\r\n",webbuff);
// end of WEB page data
bufl = strlen(webbuff); // get total page buffer length
//sprintf(cmdbuff,"AT+CIPSEND=%d,%d\r\n", linkID, bufl); // send IPD link channel and buffer character length.
sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl>2048?2048:bufl)); // send IPD link channel and buffer character length.
timeout=500;
getcount=40;
SendCMD();
getreply();
//*-*-*-5(replybuff);
////*-*-*-5("\n++++++++++ AT+CIPSENDBUF=%d,%d+++++++++\r\n", linkID, (bufl>2048?2048:bufl));
//*-*-*-5("\n++++++++++ bufl is %d ++++++++++\r\n",bufl);
//pastthrough mode
SendWEB(); // send web page
//*-*-*-5("\n++++++++++ webbuff clear ++++++++++\r\n");
memset(webbuff, '\0', sizeof(webbuff));
sendcheck();
}
// Large WEB buffer data send
void SendWEB()
{
int i=0;
if(esp.writeable()) {
while(webbuff[i]!='\0') {
esp.putc(webbuff[i]);
//****
//output at command when 2000
if(((i%2047)==0) && (i>0)) {
//wait_ms(10);
ThisThread::sleep_for(100);
sprintf(cmdbuff,"AT+CIPSENDBUF=%d,%d\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048)); // send IPD link channel and buffer character length.
////*-*-*-5("\r\n++++++++++ AT+CIPSENDBUF=%d,%d ++++++++++\r\n", linkID, (bufl-2048)>2048?2048:(bufl-2048));
timeout=600;
getcount=50;
SendCMD();
getreply();
////*-*-*-5(replybuff);
////*-*-*-5("\r\n+++++++++++++++++++\r\n");
}
//****
i++;
////*-*-*-5("%c",webbuff[i]);
}
}
//*-*-*-5("\n++++++++++ send web i= %dinfo ++++++++++\r\n",i);
}
void sendcheck()
{
weberror=1;
timeout=500;
getcount=24;
time2.reset();
time2.start();
/*
while(weberror==1 && time2.read() <5) {
getreply();
if (strstr(replybuff, "SEND OK") != NULL) {
weberror=0; // wait for valid SEND OK
}
}
*/
if(weberror==1) { // restart connection
strcpy(cmdbuff, "AT+CIPMUX=1\r\n");
timeout=500;
getcount=10;
SendCMD();
getreply();
//*-*-*-5(replybuff);
sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
timeout=500;
getcount=10;
SendCMD();
getreply();
//*-*-*-5(replybuff);
} else {
sprintf(cmdbuff, "AT+CIPCLOSE=%s\r\n",channel); // close current connection
SendCMD();
getreply();
//*-*-*-5(replybuff);
}
time2.reset();
}
// Reads and processes GET and POST web data
void ReadWebData()
{
//*-*-*-5("+++++++++++++++++Read Web Data+++++++++++++++++++++\r\n");
ThisThread::sleep_for(200);
esp.attach(NULL);
ount=0;
DataRX=0;
weberror=0;
memset(webdata, '\0', sizeof(webdata));
int x = strcspn (webbuff,"+");
if(x) {
strcpy(webdata, webbuff + x);
weberror=0;
int numMatched = sscanf(webdata,"+IPD,%d,%d:%s", &linkID, &ipdLen, type);
//int i=0;
//*-*-*-5("+++++++++++++++++succed rec begin+++++++++++++++++++++\r\n");
//*-*-*-5("%s",webdata);
//*-*-*-5("+++++++++++++++++succed rec end+++++++++++++++++++++\r\n");
beforeMode = mode;
if( strstr(webdata, "Normal") != NULL ) {
click_log=true;
//*-*-*-5("++++++++++++++++++Normal++++++++++++++++++++");
mode = SPEED; // スピードモード
flag_sp = 0;
display(); // ディスプレイ表示
mode = beforeMode; // 現在のモードに前回のモードを設定
}else if( strstr(webdata, "VeryFast") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++++VeryFast+++++++++++++++++++");
mode = SPEED; // スピードモード
flag_sp = 2;
display(); // ディスプレイ表示
mode = beforeMode; // 現在のモードに前回のモードを設定
}else if( strstr(webdata, "Fast") != NULL ) {
click_log=true;
//*-*-*-5("++++++++++++++++++++Fast++++++++++++++++++");
mode = SPEED; // スピードモード
flag_sp = 1;
display(); // ディスプレイ表示
mode = beforeMode; // 現在のモードに前回のモードを設定
}
if( strstr(webdata, "GO") != NULL ) {
pred=!pred;
click_log=true;
//*-*-*-5("+++++++++++++++++前進+++++++++++++++++++++\r\n");
run = ADVANCE; // 前進
mode = ADVANCE; // モード変更
display(); // ディスプレイ表示
}
if( strstr(webdata, "LEFT") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++左折+++++++++++++++++++++\r\n");
run = LEFT; // 左折
mode = LEFT; // モード変更
display(); // ディスプレイ表示
}
if( strstr(webdata, "STOP") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++停止+++++++++++++++++++++\r\n");
run = STOP; // 停止
mode = STOP; // モード変更
display(); // ディスプレイ表示
}
if( strstr(webdata, "RIGHT") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++右折+++++++++++++++++++++\r\n");
run = RIGHT; // 右折
mode = RIGHT; // モード変更
display(); // ディスプレイ表示
}
if( strstr(webdata, "BACK") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++後進+++++++++++++++++++++\r\n");
run = BACK; // 後進
mode = BACK; // モード変更
display(); // ディスプレイ表示
}
//*-*-*-5("+++++++++++++++++succed+++++++++++++++++++++");
if( strstr(webdata, "AVOIDANCE") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++AVOIDANCE+++++++++++++++++++++");
if(avoi_thread->get_state() == Thread::Deleted) {
delete avoi_thread; //障害物回避スレッド停止
avoi_thread = new Thread(avoidance);
avoi_thread -> set_priority(osPriorityHigh);
}
mode=AVOIDANCE;
run = ADVANCE;
display(); // ディスプレイ表示
}
if( strstr(webdata, "LINE_TRACE") != NULL ) {
click_log=true;
//*-*-*-5("+++++++++++++++++LINET RACE+++++++++++++++++++++");
//*-*-*-5("mode = LINE_TRACE\r\n");
if(trace_thread->get_state() == Thread::Deleted) {
delete trace_thread; //ライントレーススレッド停止
trace_thread = new Thread(trace);
trace_thread -> set_priority(osPriorityHigh);
}
mode=LINE_TRACE;
display(); // ディスプレイ表示
}
if(mode != LINE_TRACE && trace_thread->get_state() != Thread::Deleted){
trace_thread->terminate();
}
if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){
avoi_thread->terminate();
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
}
sprintf(channel, "%d",linkID);
if (strstr(webdata, "GET") != NULL) {
servreq=1;
}
if (strstr(webdata, "POST") != NULL) {
servreq=1;
}
webcounter++;
sprintf(webcount, "%d",webcounter);
} else {
memset(webbuff, '\0', sizeof(webbuff));
esp.attach(&callback);
weberror=1;
}
}
// Starts and restarts webserver if errors detected.
void startserver()
{
//*-*-*-5("++++++++++ Resetting ESP ++++++++++\r\n");
strcpy(cmdbuff,"AT+RST\r\n");
timeout=8000;
getcount=1000;
SendCMD();
getreply();
//*-*-*-5(replybuff);
//*-*-*-5("%d",ount);
if (strstr(replybuff, "OK") != NULL) {
//*-*-*-5("\n++++++++++ Starting Server ++++++++++\r\n");
strcpy(cmdbuff, "AT+CIPMUX=1\r\n"); // set multiple connections.
timeout=500;
getcount=20;
SendCMD();
getreply();
//*-*-*-5(replybuff);
sprintf(cmdbuff,"AT+CIPSERVER=1,%d\r\n", port);
timeout=500;
getcount=20;
SendCMD();
getreply();
//*-*-*-5(replybuff);
ThisThread::sleep_for(500);
sprintf(cmdbuff,"AT+CIPSTO=%d\r\n",SERVtimeout);
timeout=500;
getcount=50;
SendCMD();
getreply();
//*-*-*-5(replybuff);
ThisThread::sleep_for(5000);
//*-*-*-5("\n Getting Server IP \r\n");
strcpy(cmdbuff, "AT+CIFSR\r\n");
timeout=2500;
getcount=200;
while(weberror==0) {
SendCMD();
getreply();
if (strstr(replybuff, "0.0.0.0") == NULL) {
weberror=1; // wait for valid IP
}
}
//*-*-*-5("\n Enter WEB address (IP) found below in your browser \r\n\n");
//*-*-*-5("\n The MAC address is also shown below,if it is needed \r\n\n");
replybuff[strlen(replybuff)-1] = '\0';
//char* IP = replybuff + 5;
sprintf(webdata,"%s", replybuff);
//*-*-*-5(webdata);
led2=1;
bufflen=200;
//bufflen=100;
ount=0;
//*-*-*-5("\n\n++++++++++ Ready ++++++++++\r\n\n");
setup();
esp.attach(&callback);
} else {
//*-*-*-5("\n++++++++++ ESP8266 error, check power/connections ++++++++++\r\n");
led1=1;
led2=1;
led3=1;
led4=1;
while(1) {
led1=!led1;
led2=!led2;
led3=!led3;
led4=!led4;
mutex.lock(); // ミューテックスロック
lcd.setAddress(0,1);
lcd.printf("Mode:WiFiError ");
mutex.unlock();
ThisThread::sleep_for(1000);
}
}
time2.reset();
time2.start();
}
// ESP Command data send
void SendCMD()
{
esp.printf("%s", cmdbuff);
}
// Get Command and ESP status replies
void getreply()
{
memset(replybuff, '\0', sizeof(replybuff));
time1.reset();
time1.start();
replycount=0;
while(time1.read_ms()< timeout && replycount < getcount) {
if(esp.readable()) {
replybuff[replycount] = esp.getc();
replycount++;
}
}
time1.stop();
}
/* mainスレッド */
int main() {
/* 初期設定 */
for(int i=0;i<10;i++){
b10[i] = (int)(((battery.read() * 3.3 - MIN_V)/0.7)*10+0.5)*10;
};
//wifi_thread = new Thread(wifi);
//wifi_thread -> set_priority(osPriorityHigh);
setup();
avoi_thread = new Thread(avoidance);
avoi_thread->terminate();
trace_thread = new Thread(trace);
trace_thread->terminate();
mode = READY; // 現在待ちモード
beforeMode = READY; // 前回待ちモード
run = STOP; // 停止
flag_sp = NORMAL; // スピード(普通)
lcd.setBacklight(TextLCD::LightOn); // バックライトON
lcd.setAddress(0,1);
lcd.printf("Mode:SetUp");
//display(); // ディスプレイ表示
while(1){
bChange(); // バッテリー残量更新
}
}