![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
スレッド生成消去追加
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