![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
スレッド生成消去追加
Dependencies: RemoteIR TextLCD
main.cpp
- Committer:
- tomotsugu
- Date:
- 2020-07-30
- Revision:
- 10:d193030ce672
- Parent:
- 9:266198aae6c2
- Child:
- 11:b5ceaef70827
File content as of revision 10:d193030ce672:
/* mbed Microcontroller Library * Copyright (c) 2019 ARM Limited * SPDX-License-Identifier: Apache-2.0 */ #include "mbed.h" #include "ReceiverIR.h" #include "rtos.h" #include <stdint.h> #include "platform/mbed_thread.h" #include "TextLCD.h" Serial pc(USBTX, USBRX); /* マクロ定義、列挙型定義 */ #define MIN_V 2.0 // 電圧の最小値 #define MAX_V 2.67 // 電圧の最大値 #define LOW 0 // モーターOFF #define HIGH 1 // モーターON #define NORMAL 0 // 普通 #define FAST 1 // 速い #define VERYFAST 2 // とても速い /* 操作モード定義 */ enum MODE{ READY = -1, // -1:待ち ADVANCE = 1, // 1:前進 RIGHT, // 2:右折 LEFT, // 3:左折 BACK, // 4:後退 STOP, // 5:停止 LINE_TRACE, // 6:ライントレース AVOIDANCE, // 7:障害物回避 SPEED, // 8:スピード制御 }; /* ピン配置 */ 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); // ライントレースセンサ(右) 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) /* 変数宣言 */ int mode; // 操作モード int run; // 走行状態 int beforeMode; // 前回のモード int flag_sp = 0; // スピード変化フラグ Timer viewTimer; // スピ―ド変更時に3秒計測タイマー float motorSpeed[9] = {0.7, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0}; // モーター速度設定(後半はライントレース用) 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 b = 0; // バッテリー残量 int flag_b = 0; // バックライト点滅フラグ int flag_t = 0; // バックライトタイマーフラグ /* trace用変数 */ int sensArray[32] = {1,6,2,4,1,1,2,1, // ライントレースセンサパターン 3,1,1,1,3,1,1,1, 7,1,1,1,1,1,1,1, 5,1,1,1,1,1,1,1}; /* avoidance用変数 */ Timer timer; // 距離計測用タイマ int DT; // 距離 int SC; // 正面 int SL; // 左 int SR; // 右 int SLD; // 左前 int SRD; // 右前 int flag_a = 0; // 障害物有無のフラグ const int limit = 20; // 障害物の距離のリミット(単位:cm) int far; // 最も遠い距離 int houkou; // 進行方向(1:前 2:左 3:右) int i; // ループ変数 /* プロトタイプ宣言 */ void decodeIR(void const *argument); void motor(void const *argument); void changeSpeed(); void avoidance(void const *argument); void trace(void const *argument); void watchsurrounding(); int watch(); void bChange(); void display(); void lcdBacklight(void const *argument); Thread deco_thread(decodeIR, NULL, osPriorityRealtime); // decodeIRをスレッド化 :+3 Thread motor_thread(motor, NULL, osPriorityHigh); // motorをスレッド化 :+2 Thread avoi_thread(avoidance, NULL, osPriorityHigh); // avoidanceをスレッド化:+2 Thread trace_thread(trace, NULL, osPriorityHigh); // traceをスレッド化 :+2 RtosTimer bTimer(lcdBacklight, osTimerPeriodic); // lcdBacklightをタイマー割り込みで設定 DigitalOut led1(LED1); /* リモコン受信スレッド */ void decodeIR(void const *argument){ while(1){ // 受信待ち pc.printf("decodeIR\r\n"); 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; // 前回のモードに現在のモードを設定 } //pc.printf("%0x\r\n",code); switch(code){ case 0x40bf27d8: // クイック pc.printf("mode = SPEED\r\n"); mode = SPEED; // スピードモード changeSpeed(); // 速度変更 display(); // ディスプレイ表示 mode = beforeMode; // 現在のモードに前回のモードを設定 break; case 0x40be34cb: // レグザリンク pc.printf("mode = LINE_TRACE\r\n"); mode=LINE_TRACE; // ライントレースモード display(); // ディスプレイ表示 break; case 0x40bf6e91: // 番組表 pc.printf("mode = AVOIDANCE\r\n"); mode=AVOIDANCE; // 障害物回避モード display(); // ディスプレイ表示 break; case 0x40bf3ec1: // ↑ pc.printf("mode = ADVANCE\r\n"); mode = ADVANCE; // 前進モード run = ADVANCE; // 前進 display(); // ディスプレイ表示 break; case 0x40bf3fc0: // ↓ pc.printf("mode = BACK\r\n"); mode = BACK; // 後退モード run = BACK; // 後退 display(); // ディスプレイ表示 break; case 0x40bf5fa0: // ← pc.printf("mode = LEFT\r\n"); mode = LEFT; // 左折モード run = LEFT; // 左折 display(); // ディスプレイ表示 break; case 0x40bf5ba4: // → pc.printf("mode = RIGHT\r\n"); mode = RIGHT; // 右折モード run = RIGHT; // 右折 display(); // ディスプレイ表示 break; case 0x40bf3dc2: // 決定 pc.printf("mode = STOP\r\n"); mode = STOP; // 停止モード run = STOP; // 停止 display(); // ディスプレイ表示 break; default: ; } } } if(viewTimer.read_ms()>=3000){ // スピードモードのまま3秒経過 viewTimer.stop(); // タイマーストップ viewTimer.reset(); // タイマーリセット display(); // ディスプレイ表示 } ThisThread::sleep_for(90); // 90ms待つ } } /* モーター制御スレッド */ void motor(void const *argument){ while(1){ pc.printf("motor\r\n"); /* 走行状態の場合分け */ switch(run){ /* 前進 */ case ADVANCE: motorR1 = motorSpeed[flag_sp]; // 右前進モーターON motorR2 = LOW; // 右後退モーターOFF motorL1 = motorSpeed[flag_sp]; // 左前進モーターON motorL2 = LOW; // 左後退モーターOFF break; /* 右折 */ case RIGHT: motorR1 = LOW; // 右前進モーターOFF motorR2 = motorSpeed[flag_sp]; // 右後退モーターON motorL1 = motorSpeed[flag_sp]; // 左前進モーターON motorL2 = LOW; // 左後退モーターOFF break; /* 左折 */ case LEFT: motorR1 = motorSpeed[flag_sp]; // 右前進モーターON motorR2 = LOW; // 右後退モーターOFF motorL1 = LOW; // 左前進モーターOFF motorL2 = motorSpeed[flag_sp]; // 左後退モーターON break; /* 後退 */ case BACK: motorR1 = LOW; // 右前進モーターOFF motorR2 = motorSpeed[flag_sp]; // 右後退モーターON motorL1 = LOW; // 左前進モーターOFF motorL2 = motorSpeed[flag_sp]; // 左後退モーターON break; /* 停止 */ case STOP: motorR1 = LOW; // 右前進モーターOFF motorR2 = LOW; // 右後退モーターOFF motorL1 = LOW; // 左前進モーターOFF motorL2 = LOW; // 左後退モーターOFF break; } if(flag_sp > VERYFAST){ // スピード変更フラグが2より大きいなら flag_sp -= 3 * (flag_sp / 3); // スピード変更フラグ調整 } pc.printf(" motor\r\n"); ThisThread::sleep_for(30); // 30ms待つ } } /* スピード変更関数 */ void changeSpeed(){ if(flag_sp%3 == 2){ // スピード変更フラグを3で割った余りが2なら flag_sp -= 2; // スピード変更フラグを-2 }else{ // それ以外 flag_sp = flag_sp + 1; // スピード変更フラグを+1 } } /* ライントレーススレッド */ void trace(void const *argument){ while(1){ 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; pc.printf("%d %d %d %d %d \r\n",sensor1,sensor2,sensor3,sensor4,sensor5); 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: if(mode == LINE_TRACE){ // 現在ライントレースモードなら run = ADVANCE; // 低速で前進 } pc.printf("ADVANCE"); break; case 2: if(mode == LINE_TRACE){ // 現在ライントレースモードなら run = RIGHT; // 低速で右折 } pc.printf("RIGHT"); break; case 3: if(mode == LINE_TRACE){ // 現在ライントレースモードなら run = LEFT; // 低速で左折 } pc.printf("LEFT"); break; case 4: if(mode==LINE_TRACE){ // 現在ライントレースモードなら flag_sp %= 3 + 3; run = RIGHT; // 中速で右折 } pc.printf("RIGHT"); break; case 5: if(mode == LINE_TRACE){ // 現在ライントレースモードなら flag_sp = flag_sp % 3 + 3; run = LEFT; // 中速で左折 } pc.printf("LEFT"); break; case 6: if(mode == LINE_TRACE){ // 現在ライントレースモードなら flag_sp = flag_sp % 3 + 6; run = RIGHT; // 高速で右折 } pc.printf("RIGHT"); break; case 7: if(mode == LINE_TRACE){ // 現在ライントレースモードなら flag_sp = flag_sp % 3 + 6; run = LEFT; // 高速で左折 } pc.printf("LEFT"); break; } ThisThread::sleep_for(30); // 30ms待つ }else{ ThisThread::sleep_for(1); // 1ms待つ } } } /* 障害物回避走行スレッド */ void avoidance(void const *argument){ while(1){ if(mode == AVOIDANCE){ // 現在障害物回避モードなら pc.printf("avoidance\r\n"); watchsurrounding(); pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); if(flag_a == 0){ // 障害物がない場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = ADVANCE; // 前進 } else{ // 障害物がある場合 i = 0; if(SC < 15){ // 正面15cm以内に障害物が現れた場合 run = BACK; // 後退 while(watch() < limit){ // 正面20cm以内に障害物がある間 if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら break; } } run = STOP; // 停止 } if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 run = LEFT; // 左折 while(i < 10){ // 進行方向確認 if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら break; } 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; // 進行方向を左に設定 } if(far < SRD || far < SR){ // 右または右前がより遠い場合 if(SR < SRD){ // 右前が右より遠い場合 far = SRD; // 右前を最も遠い距離に設定 } else{ // 右が右前よりも遠い場合 far = SR; // 右を最も遠い距離に設定 } houkou = 3; // 進行方向を右に設定 } switch(houkou){ // 進行方向の場合分け case 1: // 前の場合 run = ADVANCE; // 前進 pc.printf("Advance\r\n"); ThisThread::sleep_for(1000); // 1秒待つ break; case 2: // 左の場合 run = LEFT; // 左折 while(i < 10){ // 進行方向確認 pc.printf("left\r\n"); if(mode!=AVOIDANCE){ break; } if(watch() > (far - 2)){ i++; } else{ i = 0; } } run = STOP; // 停止 break; case 3: // 右の場合 run = RIGHT; // 右折 while(i < 10){ // 進行方向確認 pc.printf("right\r\n"); if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら break; } if(watch() > (far - 2)){ i++; } else{ i = 0; } } run = STOP; // 停止 break; } } } pc.printf("こんにちは!\r\n"); flag_a = 0; // 障害物有無フラグを0にセット pc.printf(" avoidance\r\n"); }else{ ThisThread::sleep_for(1); // 1ms待つ } } } /* 距離計測関数 */ int watch(){ pc.printf("watch\r\n"); trig = 0; ThisThread::sleep_for(5); // 5ms待つ trig = 1; ThisThread::sleep_for(15); // 15ms待つ trig = 0; while(echo.read() == 0){ led1 = 1; if(mode!=AVOIDANCE){ // 現在障害物回避モードでないなら break; } } timer.start(); // 距離計測タイマースタート while(echo.read() == 1){ } timer.stop(); // 距離計測タイマーストップ DT = (int)(timer.read_us()*0.01657); // 距離計算 if(DT > 100){ // 検知範囲外なら100cmに設定 DT = 100; } timer.reset(); // 距離計測タイマーリセット pc.printf("私はDTである。%d\r\n",DT); led1 = 0; return DT; } /* 障害物検知関数 */ void watchsurrounding(){ pc.printf("watchsurrounding\r\n"); SC = watch(); if(SC < limit){ // 正面20cm以内に障害物がある場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = STOP; // 停止 else return; } servo.pulsewidth_us(1925); // サーボを左に40度回転 ThisThread::sleep_for(250); // 250ms待つ SLD = watch(); if(SLD < limit){ // 左前20cm以内に障害物がある場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = STOP; // 停止 else return; } servo.pulsewidth_us(2400); // サーボを左に90度回転 ThisThread::sleep_for(250); // 250ms待つ SL = watch(); if(SL < limit){ // 左20cm以内に障害物がある場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = STOP; // 停止 else return; } servo.pulsewidth_us(1450); // サーボを中央位置に戻す ThisThread::sleep_for(100); // 100ms待つ servo.pulsewidth_us(925); // サーボを右に40度回転 ThisThread::sleep_for(250); // 250ms待つ SRD = watch(); if(SRD < limit){ // 右前20cm以内に障害物がある場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = STOP; // 停止 else return; } servo.pulsewidth_us(500); // サーボを右に90度回転 ThisThread::sleep_for(250); // 250ms待つ SR = watch(); if(SR < limit){ // 右20cm以内に障害物がある場合 if(mode == AVOIDANCE) // 現在障害物回避モードなら run = STOP; // 停止 else return; } servo.pulsewidth_us(1450); // サーボを中央位置に戻す ThisThread::sleep_for(100); // 100ms待つ if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 flag_a = 1; // 障害物有無フラグに1をセット } pc.printf(" watchsurrounding\r\n"); } /* ディスプレイ表示関数 */ void display(){ 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){ /* 普通 */ 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){ // バックライト点滅フラグが1なら lcd.setBacklight(TextLCD::LightOn); // バックライトON }else{ // バックライト点滅フラグが0なら lcd.setBacklight(TextLCD::LightOff); // バックライトOFF } flag_b = !flag_b; // バックライト点滅フラグ切り替え } /* バッテリー残量更新関数 */ void bChange(){ pc.printf(" bChange1\r\n"); b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10; if(b <= 0){ // バッテリー残量0%なら全ての機能停止 b = 0; //lcd.setBacklight(TextLCD::LightOff); //run = STOP; //exit(1); // 電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。 }else if(b >= 100){ // バッテリー残量100%以上なら b = 100; // 100% } mutex.lock(); // ミューテックスロック lcd.setAddress(0,0); lcd.printf("Battery:%3d%%",b); // バッテリー残量表示 pc.printf(" bChange2\r\n"); mutex.unlock(); // ミューテックスアンロック if(b <= 30){ // バッテリー残量30%以下なら if(flag_t == 0){ // バックライトタイマーフラグが0なら bTimer.start(500); // 0.5秒周期でバックライトタイマー割り込み flag_t = 1; // バックライトタイマーフラグを1に切り替え } }else{ if(flag_t == 1){ // バックライトタイマーフラグが1なら bTimer.stop(); // バックライトタイマーストップ lcd.setBacklight(TextLCD::LightOn); // バックライトON flag_t = 0; // バックライトタイマーフラグを0に切り替え } } } /* mainスレッド */ int main() { /* 初期設定 */ mode = READY; // 現在待ちモード beforeMode = READY; // 前回待ちモード run = STOP; // 停止 flag_sp = NORMAL; // スピード(普通) lcd.setBacklight(TextLCD::LightOn); // バックライトON display(); // ディスプレイ表示 while(1){ pc.printf(" main1\r\n"); bChange(); // バッテリー残量更新 pc.printf(" main2\r\n"); ThisThread::sleep_for(1); // 1ms待つ } }