siqiang pan
ボタンを押すと、 バッテリ更新を停止し、 他のボタンもロックさせる
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 3:2ae6218973be
- Parent:
- 2:38825726cb1b
- Child:
- 4:3f80c0180e2f
--- a/main.cpp Mon Jul 27 08:49:28 2020 +0000
+++ b/main.cpp Wed Jul 29 01:24:51 2020 +0000
@@ -9,46 +9,79 @@
#include <stdint.h>
#include "platform/mbed_thread.h"
#include "TextLCD.h"
-#define MIN_V 2.23
-#define MAX_V 3.3
-
-//bChangeのいろいろ
-I2C i2c_lcd(p9, p10);
-TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
-int b = 0;
-AnalogIn battery(p15);
-double test;
Serial pc(USBTX, USBRX);
-// ポートp15を赤外線受信モジュールの接続先に指定
-ReceiverIR ir_rx(p15);
-RemoteIR::Format format;
-uint8_t buf[32];
-uint32_t bitcount;
-uint32_t code;
-//障害物回避の設定
-DigitalOut trig(p6); // 超音波センサのtrigピンをp6に接続
-DigitalIn echo(p7); // 超音波センサのechoピンをp7に接続
-PwmOut servo(p25); // サーボコントロールピン(p25)
+/* マクロ定義、列挙型定義 */
+#define MIN_V 2.0
+#define MAX_V 2.67
+#define LOW 0 //
+#define HIGH 1 //
+#define NORMAL 0 // フラグ値
+#define FAST 1 // フラグ値
+#define VERYFAST 2 // フラグ値
-enum Mode{
- ADVANCE,
+enum MODE{
+ READY = -1,
+ ADVANCE = 1,
RIGHT,
LEFT,
BACK,
STOP,
LINE_TRACE,
AVOIDANCE,
- READY
+ SPEED,
};
-Mode run;
-Mode mode;
+/* ピン配置 */
+ReceiverIR ir(p5); // リモコン操作
+DigitalOut trig(p6); // 超音波センサtrigger
+DigitalOut echo(p7); // 超音波センサecho
+DigitalIn ss1(p8); // ライントレースセンサ(左)
+DigitalIn ss2(p9); // ライントレースセンサ
+DigitalIn ss3(p10); // ライントレースセンサ
+DigitalIn ss4(p11); // ライントレースセンサ
+DigitalIn ss5(p12); // ライントレースセンサ(右)
+Serial esp(p13, p14); // Wi-Fiモジュール(tx, rx)
+AnalogIn battery(p15); // 電池残量読み取り(Max 3.3V)
+PwmOut motorR2(p21); // 右モーター後退
+PwmOut motorR1(p22); // 右モーター前進
+PwmOut motorL2(p23); // 左モーター後退
+PwmOut motorL1(p24); // 左モーター前進
+PwmOut servo(p25); // サーボ
+I2C i2c_lcd(p28,p27); // LCD(tx, rx)
-Timer timer; // 距離計測用タイマー
+/* 変数宣言 */
+int mode; // 操作モード
+int run; // 走行状態
+int beforeMode; // 前のモード
+int flag_sp = 0; // スピード変化フラグ
+Timer viewTimer; // スピ―ド変更時に3秒計測
+float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
+ // モーター速度設定(後半はライントレース用)
+Mutex mutex;
+// ポートp15を赤外線受信モジュールの接続先に指定
+RemoteIR::Format format;
+uint8_t buf[32];
+uint32_t bitcount;
+uint32_t code;
+
+//bChangeのいろいろ
+TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
+int b = 0;
+double test;
+int flag_b = 0;
+int flag_t = 0;
+
+// ライントレース
+int sensArray[32] = {0,0,0,0,0,0,0,0, // ライントレースセンサパターン
+ 0,0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,0};
/* 障害物検知用の変数設定 */
+Timer timer; // 距離計測用タイマ
+int DT; // 距離
int SC; // 正面
int SL; // 左
int SR; // 右
@@ -56,167 +89,93 @@
int SRD; // 右前
int flag = 0; // 障害物有無のフラグ
const int limit = 20; // 障害物の距離のリミット(単位：cm)
-int DT; // 距離
+int far; // 最も遠い距離
int houkou; // 進行方向(1:前 2:左 3:右)
-int far; // 最も遠い距離
int i; // ループ変数
-void rimokon(void const *argument);
+
+
+/* プロトタイプ宣言 */
+void decodeIR(void const *argument);
+void motor(void const *argument);
+void changeSpeed();
void avoidance(void const *argument);
-void motor(void const *argument);
+void trace(void const *argument);
void watchsurrounding();
int watch();
void bChange(void const *argument);
-Thread thread1(rimokon, NULL, osPriorityRealtime);
-Thread thread2(motor, NULL, osPriorityHigh);
-Thread thread3(avoidance, NULL, osPriorityAboveNormal);
-Thread thread4(bChange, NULL, osPriorityBelowNormal);
+void display();
+void lcdBacklight(void const *argument);
+Thread thread1(decodeIR, NULL, osPriorityAboveNormal);
+Thread thread2(motor, NULL, osPriorityNormal);
+Thread thread3(avoidance, NULL, osPriorityLow);
+Thread thread4(trace, NULL, osPriorityLow);
+Thread thread5(bChange, NULL, osPriorityIdle);
+RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
-void rimokon(void const *argument){
+
+void decodeIR(void const *argument){
while(1){
// 受信待ち
- if (ir_rx.getState() == ReceiverIR::Received) {
+ if (ir.getState() == ReceiverIR::Received) {
// コード受信
- bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+ 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)));
}
- pc.printf("%0x\r\n",code);
+ if(mode!=SPEED){
+ beforeMode=mode;
+ }
+
+ //pc.printf("%0x\r\n",code);
switch(code){
- case 0x40bf0ff0://入力切換
- pc.printf("入力切換\r\n");
- break;
- case 0x40bf12ed://電源
- pc.printf("電源\r\n");
- break;
- case 0x40bf7b84://地アナ
- pc.printf("地アナ\r\n");
- break;
- case 0x40bf7a85://地デジ
- pc.printf("地デジ\r\n");
- break;
- case 0x40bf7c83://BS
- pc.printf("BS\r\n");
- break;
- case 0x40bf7d82://CS
- pc.printf("CS\r\n");
- break;
-
- case 0x40bf01fe://1
- pc.printf("1\r\n");
- break;
- case 0x40bf02fd://2
- pc.printf("2\r\n");
- break;
- case 0x40bf03fc://3
- pc.printf("3\r\n");
- break;
- case 0x40bf04fb://4
- pc.printf("4\r\n");
- break;
- case 0x40bf05fa://5
- pc.printf("5\r\n");
- break;
- case 0x40bf06f9://6
- pc.printf("6\r\n");
- break;
- case 0x40bf07f8://7
- pc.printf("7\r\n");
- break;
- case 0x40bf08f7://8
- pc.printf("8\r\n");
- break;
- case 0x40bf09f6://9
- pc.printf("9\r\n");
- break;
- case 0x40bf0af5://10
- pc.printf("10\r\n");
- break;
- case 0x40bf0bf4://11
- pc.printf("11\r\n");
- break;
- case 0x40bf0cf3://12
- pc.printf("12\r\n");
- break;
- case 0x40bf1be4://チャンネル↑
- pc.printf("チャンネル↑\r\n");
- break;
- case 0x40bf1fe0://チャンネル↓
- pc.printf("チャンネル↓\r\n");
- break;
- case 0x40bf1ce3://画面表示
- pc.printf("画面表示\r\n");
- break;
- case 0x40bf10ef://消音
- pc.printf("消音\r\n");
- break;
case 0x40bf27d8://クイック
- pc.printf("クイック\r\n");
- break;
- case 0x40bf1ae5://音量↑
- pc.printf("音量↑\r\n");
- break;
- case 0x40bf1ee1://音量↓
- pc.printf("音量↓\r\n");
+ pc.printf("mode = SPEED\r\n");
+ mode = SPEED;
break;
case 0x40be34cb://レグザリンク
- pc.printf("レグザリンク\r\n");
+ pc.printf("mode = INE_TRACE\r\n");
+ mode=LINE_TRACE;
+ display();
break;
case 0x40bf6e91://番組表
- pc.printf("番組表\r\n");
- break;
- case 0x40bf3bc4://戻る
- pc.printf("戻る\r\n");
- break;
- case 0x40bf3cc3://終了
- pc.printf("終了\r\n");
+ pc.printf("mode = AVOIDANCE\r\n");
+ mode=AVOIDANCE;
+ display();
break;
case 0x40bf3ec1://↑
- pc.printf("↑\r\n");
+ pc.printf("mode = ADVANCE\r\n");
+ mode = ADVANCE;
+ run = ADVANCE;
+ display();
break;
case 0x40bf3fc0://↓
- pc.printf("↓\r\n");
+ pc.printf("mode = BACK\r\n");
+ mode = BACK;
+ run = BACK;
+ display();
break;
case 0x40bf5fa0://←
- mode=LINE_TRACE;
- pc.printf("←\r\n");
+ pc.printf("mode = LEFT\r\n");
+ mode = LEFT;
+ run = LEFT;
+ display();
break;
case 0x40bf5ba4://→
- mode=AVOIDANCE;
- pc.printf("→\r\n");
+ pc.printf("mode = RIGHT\r\n");
+ mode = RIGHT;
+ run = RIGHT;
+ display();
break;
case 0x40bf3dc2://決定
- pc.printf("決定\r\n");
- break;
- case 0x40bf738c://青
- pc.printf("青\r\n");
- break;
- case 0x40bf748b://赤
- pc.printf("赤\r\n");
- break;
- case 0x40bf758a://緑
- pc.printf("緑\r\n");
- break;
- case 0x40bf7689://黄
- pc.printf("黄\r\n");
+ pc.printf("mode = STOP\r\n");
+ mode = STOP;
+ run = STOP;
+ display();
break;
- case 0x43bc14eb://dデータ
- pc.printf("dデータ\r\n");
- break;
- case 0x40bf50af://静止
- pc.printf("静止\r\n");
- break;
- case 0x40bf59a6://おまかせ映像
- pc.printf("おまかせ映像\r\n");
- break;
- case 0x40bf13ec://音声切換
- pc.printf("音声切換\r\n");
- break;
-
-
default:
;
}
@@ -226,97 +185,192 @@
}
}
void motor(void const *argument){
- while(1){
- pc.printf("motor\r\n");
- ThisThread::sleep_for(20);
+ while(true){
+ pc.printf("motor\r\n");
+ if(flag_sp >VERYFAST){
+ flag_sp -= 3;
+ }
+ switch(run){
+ case ADVANCE:
+ motorR1 = motorSpeed[flag_sp];
+ motorR2 = LOW;
+ motorL1 = motorSpeed[flag_sp];
+ motorL2 = LOW;
+ break;
+ case RIGHT:
+ motorR1 = LOW;
+ motorR2 = motorSpeed[flag_sp];
+ motorL1 = motorSpeed[flag_sp];
+ motorL2 = LOW;
+ break;
+ case LEFT:
+ motorR1 = motorSpeed[flag_sp];
+ motorR2 = LOW;
+ motorL1 = LOW;
+ motorL2 = motorSpeed[flag_sp];
+ break;
+ case BACK:
+ motorR1 = LOW;
+ motorR2 = motorSpeed[flag_sp];
+ motorL1 = LOW;
+ motorL2 = motorSpeed[flag_sp];
+ break;
+ case STOP:
+ motorR1 = LOW;
+ motorR2 = LOW;
+ motorL1 = LOW;
+ motorL2 = LOW;
+ break;
+ }
+ pc.printf(" motor\r\n");
+ ThisThread::sleep_for(50);
}
}
+void changeSpeed(){
+ if(flag_sp == VERYFAST){
+ flag_sp = NORMAL;
+
+ }else{
+ flag_sp = flag_sp + 1;
+ }
+}
+void trace(void const *argument){
+ while(true){
+ if(mode==LINE_TRACE){
+ pc.printf("line trace\r\n");
+ // センサー値読み取り
+ int sensor1 = ss1;
+ int sensor2 = ss2;
+ int sensor3 = ss3;
+ int sensor4 = ss4;
+ int sensor5 = ss5;
+
+ int sensD = 0;
+ int sensorNum;
+
+ 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;
+
+ sensorNum = sensArray[sensD];
+
+ switch(sensorNum){
+ case 1:
+ run = ADVANCE;
+ break;
+ case 2:
+ run = RIGHT;
+ break;
+ case 3:
+ run = LEFT;
+ break;
+ case 4:
+ flag_sp += 3;
+ run = RIGHT;
+ break;
+ case 5:
+ flag_sp += 3;
+ run = LEFT;
+ break;
+ }
+ pc.printf(" line trace\r\n");
+ ThisThread::sleep_for(50);
+ }else{
+ ThisThread::sleep_for(50);
+ }
+ }
+}
+
/* 障害物回避走行 */
void avoidance(void const *argument){
-
- while(1){
- if(mode==AVOIDANCE){
- pc.printf("avoidance\r\n");
- if(flag == 0){ // 障害物がない場合
- run = ADVANCE; // 前進
- }
- /* else{ // 障害物がある場合
- i = 0;
- if(SC < 15){ // 正面15cm以内に障害物が現れた場合
- run = BACK; // 後退
- while(watch() < limit){ // 正面20cm以内に障害物がある間
+ while(1){
+ if(mode==AVOIDANCE){
+ pc.printf("avoidance\r\n");
+ if(flag == 0){ // 障害物がない場合
+ run = ADVANCE; // 前進
+ }
+ /*else{ // 障害物がある場合
+ i = 0;
+ if(SC < 15){ // 正面15cm以内に障害物が現れた場合
+ run = BACK; // 後退
+ while(watch() < limit){ // 正面20cm以内に障害物がある間
+ }
+ run = STOP; // 停止
}
- run = STOP; // 停止
- }
- if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
- run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
- if(watch() > limit){
- i++;
+ if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
+ run = LEFT; // 左折
+ while(i < 100){ // 進行方向確認
+ if(watch() > limit){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ }
+ else { // 全方向以外
+ far = SC; // 正面を最も遠い距離に設定
+ houkou = 1; // 進行方向を前に設定
+ if(far < SLD || far < SL){ // 左または左前がより遠い場合
+ if(SL < SLD){ // 左前が左より遠い場合
+ far = SLD; // 左前を最も遠い距離に設定
+ }
+ else{ // 左が左前より遠い場合
+ far = SL; // 左を最も遠い距離に設定
+ }
+ houkou = 2; // 進行方向を左に設定
}
- else{
- i = 0;
+ if(far < SRD || far < SR){ // 右または右前がより遠い場合
+ if(SR < SRD){ // 右前が右より遠い場合
+ far = SRD; // 右前を最も遠い距離に設定
+ }
+ else{ // 右が右前よりも遠い場合
+ far = SR; // 右を最も遠い距離に設定
+ }
+ houkou = 3; // 進行方向を右に設定
+ }
+ switch(houkou){ // 進行方向の場合分け
+ case 1: // 前の場合
+ run = ADVANCE; // 前進
+ thread_sleep_for(100); // 1秒待つ
+ break;
+ case 2: // 左の場合
+ run = LEFT; // 左折
+ while(i < 100){ // 進行方向確認
+ if(watch() > (far - 2)){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ break;
+ case 3: // 右の場合
+ run = RIGHT; // 右折
+ while(i < 100){ // 進行方向確認
+ if(watch() > (far - 2)){
+ i++;
+ }
+ else{
+ i = 0;
+ }
+ }
+ run = STOP; // 停止
+ break;
}
}
- run = STOP; // 停止
- }
- else { // 全方向以外
- far = SC; // 正面を最も遠い距離に設定
- houkou = 1; // 進行方向を前に設定
- 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; // 前進
- thread_sleep_for(100); // 1秒待つ
- break;
- case 2: // 左の場合
- run = LEFT; // 左折
- while(i < 100){ // 進行方向確認
- if(watch() > (far - 2)){
- i++;
- }
- else{
- i = 0;
- }
- }
- run = STOP; // 停止
- break;
- case 3: // 右の場合
- run = RIGHT; // 右折
- while(i < 100){ // 進行方向確認
- if(watch() > (far - 2)){
- i++;
- }
- else{
- i = 0;
- }
- }
- run = STOP; // 停止
- break;
- }
- }
- }*/
- flag = 0; // フラグを0にセット
- watchsurrounding();
- }
+ }*/
+ flag = 0; // フラグを0にセット
+ watchsurrounding();
+ pc.printf(" avoidance\r\n");
+ }else{
+ ThisThread::sleep_for(50);
+ }
}
}
int watch(){
@@ -337,7 +391,8 @@
DT = 400;
}
timer.reset(); // 距離計測タイマーリセット
- ThisThread::sleep_for(30); // 30ms待つ
+ //ThisThread::sleep_for(30); // 30ms待つ
+ pc.printf(" watch\r\n");
return DT;
}
@@ -378,28 +433,176 @@
if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
flag = 1; // フラグに1をセット
}
+ pc.printf(" watchsurrounding\r\n");
+}
+
+void defaultView(){
+ lcd.setAddress(0,0);
+ lcd.printf("Battery:");
+ lcd.setAddress(0,1);
+ lcd.printf("Mode:");
}
+void display(){
+ mutex.lock();
+ lcd.setAddress(0,1);
+ switch(mode){
+ case ADVANCE:
+ //strcpy(DispMode,"Mode:Advance");
+ lcd.printf("Mode:Advance ");
+ break;
+ case RIGHT:
+ //strcpy(DispMode,"Mode:Right");
+ lcd.printf("Mode:Right ");
+ break;
+ case LEFT:
+ //strcpy(DispMode,"Mode:Left");
+ lcd.printf("Mode:Left ");
+ break;
+ case BACK:
+ //strcpy(DispMode,"Mode:Back");
+ lcd.printf("Mode:Back ");
+ break;
+ case STOP:
+ //strcpy(DispMode,"Mode:Stop");
+ lcd.printf("Mode:Stop ");
+ break;
+ case READY:
+ //strcpy(DispMode,"Mode:Stop");
+ lcd.printf("Mode:Ready ");
+ break;
+ case LINE_TRACE:
+ //strcpy(DispMode,"Mode:LineTrace");
+ lcd.printf("Mode:LineTrace ");
+ break;
+ case AVOIDANCE:
+ //strcpy(DispMode,"Mode:Avoidance");
+ lcd.setAddress(0,1);
+ lcd.printf("Mode:Avoidance ");
+ break;
+ case SPEED:
+ switch(flag_sp){
+ 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;
+ }
+ mutex.unlock();
+}
+
+void lcdBacklight(void const *argument){
+ if(flag_b == 1){
+ lcd.setBacklight(TextLCD::LightOn);
+ }else{
+ lcd.setBacklight(TextLCD::LightOff);
+ }
+ flag_b = !flag_b;
+}
void bChange(void const *argument){
- while(1){
- //lcd.setBacklight(TextLCD::LightOn);
+ /*while(1){
+ pc.printf(" bChange1\r\n");
+ lcd.setBacklight(TextLCD::LightOn);
test = battery.read()*MAX_V;
b = (int)((test - MIN_V)/0.107 + 0.5)*10;
- lcd.locate(0,0);
+ if(b < 0){
+ b = 0;
+ }else if(b > 100){
+ b = 100;
+ }
+ mutex.lock();
+ lcd.setAddress(0,0);
+ lcd.printf("Battery:%3d%% ",b);
+ pc.printf(" bChange2\r\n");
+ mutex.unlock();
+ }*/
+ lcd.setBacklight(TextLCD::LightOn);
+ while(1){
+ pc.printf(" bChange1\r\n");
+ b = (int)((battery.read()* MAX_V - MIN_V)/0.067 + 0.5)*10;
+ if(b < 0){//すべての機能停止
+ lcd.setBacklight(TextLCD::LightOff);
+ bTimer.stop();
+ exit(1);
+ }else if(b > 100){
+ b = 100;
+ }
+ mutex.lock();
+ lcd.setAddress(0,0);
lcd.printf("Battery:%3d%%",b);
- pc.printf("bChange\r\n");
+ pc.printf(" bChange2\r\n");
+ mutex.unlock();
+ if(b <= 30){
+ if(flag_t == 0){
+ bTimer.start(500);
+ flag_t = 1;
+ }
+ }else{
+ if(flag_t == 1){
+ bTimer.stop();
+ lcd.setBacklight(TextLCD::LightOn);
+ flag_t = 0;
+ }
+ }
}
+
}
int main() {
-
-
+ mode = READY;
+ beforeMode = READY;
+ run = STOP;
+ flag_sp = NORMAL;
+ //defaultView();
while(1){
- /*if(button.read()==0){
- rimokon();
- } */
- pc.printf("main\r\n");
- ThisThread::sleep_for(40);
+ pc.printf(" main1\r\n");
+ switch(mode){
+ /*case ADVANCE:
+ run = ADVANCE;
+ break;
+ case RIGHT:
+ run = RIGHT;
+ break;
+ case LEFT:
+ run = LEFT;
+ break;
+ case BACK:
+ run = BACK;
+ break;
+ case STOP:
+ run = STOP;
+ break;
+ case READY:
+ run = STOP;
+ break;*/
+ case SPEED:
+ changeSpeed();
+ display();
+ mode = beforeMode;
+ break;
+ }
+
+ if(viewTimer.read_ms()>=3000){
+ pc.printf("a");
+ viewTimer.stop();
+ //defaultView();
+ viewTimer.reset();
+ //flaga=0;
+ }
+ pc.printf(" main2\r\n");
+ if(viewTimer.read_ms()==0){
+ display();
+ }
+ pc.printf(" main3\r\n");
+ ThisThread::sleep_for(10);
}
}
\ No newline at end of file