赤子を殺すより楽な作業よ
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 6:4817fe7b7654
- Parent:
- 5:5414f85108aa
- Child:
- 7:4a65f5a25c13
--- a/main.cpp Wed Jul 29 06:36:04 2020 +0000 +++ b/main.cpp Wed Jul 29 07:57:16 2020 +0000 @@ -15,11 +15,11 @@ /* マクロ定義、列挙型定義 */ #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 // フラグ値 +#define LOW 0 +#define HIGH 1 +#define NORMAL 0 +#define FAST 1 +#define VERYFAST 2 enum MODE{ READY = -1, @@ -109,24 +109,20 @@ void lcdBacklight(void const *argument); /* スレッド宣言 */ -Thread thread1(decodeIR, NULL, osPriorityRealtime);//+3 -Thread thread2(motor, NULL, osPriorityHigh);//+2 -Thread thread3(avoidance, NULL, osPriorityHigh);//+2 -Thread thread4(trace, NULL, osPriorityHigh);//+2 +Thread thread1(decodeIR, NULL, osPriorityRealtime); //+3 +Thread thread2(motor, NULL, osPriorityHigh); //+2 +Thread thread3(avoidance, NULL, osPriorityHigh); //+2 +Thread thread4(trace, NULL, osPriorityHigh); //+2 -/* タイマー割り込み */ +/* バックライト点滅タイマー割り込み */ RtosTimer bTimer(lcdBacklight, osTimerPeriodic); /* リモコン受信スレッド */ 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){ - // 受信成功 + 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))); @@ -135,49 +131,41 @@ beforeMode=mode; } switch(code){ - case 0x40bf27d8://クイック - pc.printf("mode = SPEED\r\n"); + case 0x40bf27d8: //クイック mode = SPEED; changeSpeed(); display(); mode = beforeMode; break; - case 0x40be34cb://レグザリンク - pc.printf("mode = LINE_TRACE\r\n"); + case 0x40be34cb: //レグザリンク mode=LINE_TRACE; display(); break; - case 0x40bf6e91://番組表 - pc.printf("mode = AVOIDANCE\r\n"); + case 0x40bf6e91: //番組表 mode=AVOIDANCE; display(); break; - case 0x40bf3ec1://↑ - pc.printf("mode = ADVANCE\r\n"); + case 0x40bf3ec1: //↑ mode = ADVANCE; run = ADVANCE; display(); break; - case 0x40bf3fc0://↓ - pc.printf("mode = BACK\r\n"); + case 0x40bf3fc0: //↓ mode = BACK; run = BACK; display(); break; - case 0x40bf5fa0://← - pc.printf("mode = LEFT\r\n"); + case 0x40bf5fa0: //← mode = LEFT; run = LEFT; display(); break; - case 0x40bf5ba4://→ - pc.printf("mode = RIGHT\r\n"); + case 0x40bf5ba4: //→ mode = RIGHT; run = RIGHT; display(); break; - case 0x40bf3dc2://決定 - pc.printf("mode = STOP\r\n"); + case 0x40bf3dc2: //決定 mode = STOP; run = STOP; display(); @@ -199,10 +187,6 @@ /* モータースレッド */ void motor(void const *argument){ while(1){ - pc.printf("motor\r\n"); - if(flag_sp >VERYFAST){ - flag_sp -= 3; - } switch(run){ case ADVANCE: motorR1 = motorSpeed[flag_sp]; @@ -235,18 +219,17 @@ motorL2 = LOW; break; } - pc.printf(" motor\r\n"); ThisThread::sleep_for(30); } } /* スピード変更関数 */ void changeSpeed(){ - if(flag_sp == VERYFAST){ - flag_sp = NORMAL; + if(flag_sp == VERYFAST){ // 最速の場合 + flag_sp = NORMAL; // 普通に設定 } - else{ - flag_sp = flag_sp + 1; + else{ // 最速以外の場合 + flag_sp = flag_sp + 1; // 1段階上げる } } @@ -254,7 +237,6 @@ void trace(void const *argument){ while(1){ if(mode==LINE_TRACE){ - pc.printf("line trace\r\n"); // センサー値読み取り int sensor1 = ss1; int sensor2 = ss2; @@ -292,7 +274,6 @@ run = LEFT; break; } - pc.printf(" line trace\r\n"); ThisThread::sleep_for(30); }else{ ThisThread::sleep_for(1); @@ -304,8 +285,7 @@ void avoidance(void const *argument){ while(1){ if(mode==AVOIDANCE){ - pc.printf("avoidance\r\n"); - if(flag_a == 0){ // 障害物がない場合 + if(flag_a == 0){ // 障害物がない場合 run = ADVANCE; // 前進 } else{ // 障害物がある場合 @@ -313,13 +293,18 @@ if(SC < 15){ // 正面15cm以内に障害物が現れた場合 run = BACK; // 後退 while(watch() < limit){ // 正面20cm以内に障害物がある間 - + if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら + break; // avoidanceから抜ける + } } run = STOP; // 停止 } if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 run = LEFT; // 左折 - while(i < 10){ // 進行方向確認 + while(i < 10){ // 進行方向確認 + if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら + break; // avoidanceから抜ける + } if(watch() > limit){ i++; } @@ -353,11 +338,14 @@ switch(houkou){ // 進行方向の場合分け case 1: // 前の場合 run = ADVANCE; // 前進 - thread_sleep_for(100); // 1秒待つ + ThisThread::sleep_for(1000); // 1秒待つ break; case 2: // 左の場合 run = LEFT; // 左折 - while(i < 10){ // 進行方向確認 + while(i < 10){ // 進行方向確認 + if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら + break; // avoidanceから抜ける + } if(watch() > (far - 2)){ i++; } @@ -369,7 +357,10 @@ break; case 3: // 右の場合 run = RIGHT; // 右折 - while(i < 10){ // 進行方向確認 + while(i < 10){ // 進行方向確認 + if(mode != AVOIDANCE){ // 障害物回避モードじゃなくなったら + braek; // avoidanceから抜ける + } if(watch() > (far - 2)){ i++; } @@ -384,50 +375,45 @@ } flag_a = 0; // 障害物有無フラグを0にセット watchsurrounding(); - pc.printf(" avoidance\r\n"); }else{ - ThisThread::sleep_for(1); + 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){ - //} + while(echo.read() == 0){ + } timer.start(); // 距離計測タイマースタート while(echo.read() == 1){ } timer.stop(); // 距離計測タイマーストップ DT = timer.read_us()*0.01657; // 距離計算 - if(DT > 400){ // 検知範囲外なら400cmに設定 - DT = 400; + if(DT > 100){ // 検知範囲外なら400cmに設定 + DT = 100; } timer.reset(); // 距離計測タイマーリセット - //ThisThread::sleep_for(30); // 30ms待つ - pc.printf(" watch\r\n"); return DT; } void watchsurrounding(){ - pc.printf("watchsurrounding\r\n"); SC = watch(); if(SC < limit){ // 正面20cm以内に障害物がある場合 run = STOP; // 停止 } servo.pulsewidth_us(1925); // サーボを左に40度回転 - ThisThread::sleep_for(250); // 250ms待つ + ThisThread::sleep_for(250); // 250ms待つ SLD = watch(); if(SLD < limit){ // 左前20cm以内に障害物がある場合 run = STOP; // 停止 } servo.pulsewidth_us(2400); // サーボを左に90度回転 - ThisThread::sleep_for(250); // 250ms待つ + ThisThread::sleep_for(250); // 250ms待つ SL = watch(); if(SL < limit){ // 左20cm以内に障害物がある場合 run = STOP; // 停止 @@ -435,23 +421,22 @@ servo.pulsewidth_us(1450); ThisThread::sleep_for(100); servo.pulsewidth_us(925); // サーボを右に40度回転 - ThisThread::sleep_for(250); // 250ms待つ + ThisThread::sleep_for(250); // 250ms待つ SRD = watch(); if(SRD < limit){ // 右前20cm以内に障害物がある場合 run = STOP; // 停止 } servo.pulsewidth_us(500); // サーボを右に90度回転 - ThisThread::sleep_for(250); // 250ms待つ + ThisThread::sleep_for(250); // 250ms待つ SR = watch(); if(SR < limit){ // 右20cm以内に障害物がある場合 run = STOP; // 停止 } servo.pulsewidth_us(1450); // サーボを中央位置に戻す - ThisThread::sleep_for(100); // 100ms待つ + ThisThread::sleep_for(100); // 100ms待つ if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 - flag = 1; // フラグに1をセット + flag_a = 1; // フラグに1をセット } - pc.printf(" watchsurrounding\r\n"); } /* ディスプレイ表示関数 */ @@ -515,9 +500,7 @@ void bChange(){ lcd.setBacklight(TextLCD::LightOn); while(1){ - pc.printf(" bChange1\r\n"); b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10; - pc.printf("%f",battery.read()); if(b < 0){//すべての機能停止(今はなし) /*lcd.setBacklight(TextLCD::LightOff); bTimer.stop(); @@ -530,7 +513,6 @@ mutex.lock(); lcd.setAddress(0,0); lcd.printf("Battery:%3d%%",b); - pc.printf(" bChange2\r\n"); mutex.unlock(); if(b <= 30){ if(flag_t == 0){ @@ -556,9 +538,7 @@ flag_sp = NORMAL; display(); while(1){ - pc.printf(" main1\r\n"); bChange(); - pc.printf(" main3\r\n"); ThisThread::sleep_for(1); } } \ No newline at end of file