mbed_robotcar
linetrace saikyou
Dependencies: RemoteIR TextLCD
Diff: main.cpp
- Revision:
- 18:6cca64c7dbc3
- Parent:
- 17:f7259ab2fe86
- Child:
- 19:c6f9f010bd9e
--- a/main.cpp Tue Aug 04 08:43:58 2020 +0000
+++ b/main.cpp Wed Aug 05 01:09:50 2020 +0000
@@ -123,7 +123,6 @@
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){ // 受信成功
@@ -134,7 +133,6 @@
if(mode != SPEED){ // スピードモード以外なら
beforeMode=mode; // 前回のモードに現在のモードを設定
}
- //pc.printf("%0x\r\n",code);
switch(code){
case 0x40bf27d8: // クイック
//pc.printf("mode = SPEED\r\n");
@@ -160,6 +158,7 @@
avoi_thread = new Thread(avoidance);
avoi_thread -> set_priority(osPriorityHigh);
}
+ flag_a = 0;
mode=AVOIDANCE; // 障害物回避モード
run = ADVANCE; // 前進
display(); // ディスプレイ表示
@@ -210,14 +209,13 @@
viewTimer.reset(); // タイマーリセット
display(); // ディスプレイ表示
}
- ThisThread::sleep_for(60); // 90ms待つ
+ ThisThread::sleep_for(90); // 90ms待つ
}
}
/* モーター制御スレッド */
void motor(void const *argument){
while(1){
- //pc.printf("motor\r\n");
/* 走行状態の場合分け */
switch(run){
/* 前進 */
@@ -302,92 +300,57 @@
/* センサー値によって場合分け */
switch(sensorNum){
case 1:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- run = ADVANCE; // 低速で前進
- //}
- //pc.printf("ADVANCE");
+ run = ADVANCE; // 低速で前進
break;
case 2:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- run = RIGHT; // 低速で右折
- //}
- //pc.printf("RIGHT");
+ run = RIGHT; // 低速で右折
break;
case 3:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- run = LEFT; // 低速で左折
- //}
- //pc.printf("LEFT");
+ run = LEFT; // 低速で左折
break;
case 4:
- //if(mode==LINE_TRACE){ // 現在ライントレースモードなら
- flag_sp = flag_sp % 3 + 3;
- run = RIGHT; // 中速で右折
- //}
- //pc.printf("RIGHT");
+ flag_sp = flag_sp % 3 + 3;
+ run = RIGHT; // 中速で右折
break;
case 5:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- flag_sp = flag_sp % 3 + 3;
- run = LEFT; // 中速で左折
- //}
- //pc.printf("LEFT");
+ flag_sp = flag_sp % 3 + 3;
+ run = LEFT; // 中速で左折
break;
case 6:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- flag_sp = flag_sp % 3 + 6;
- run = RIGHT; // 高速で右折
- //}
- //pc.printf("RIGHT");
+ flag_sp = flag_sp % 3 + 6;
+ run = RIGHT; // 高速で右折
break;
case 7:
- //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
- flag_sp = flag_sp % 3 + 6;
- run = LEFT; // 高速で左折
- //}
- //pc.printf("LEFT");
+ flag_sp = flag_sp % 3 + 6;
+ run = 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; // 前進
+ watchsurrounding();
+ pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR);
+ if(flag_a == 0){ // 障害物がない場合
+ run = ADVANCE; // 前進
+ }
+ else{ // 障害物がある場合
+ i = 0;
+ if(SC < 15){ // 正面15cm以内に障害物が現れた場合
+ run = BACK; // 後退
+ while(watch() < limit){ // 正面20cm以内に障害物がある間
+ }
+ run = STOP; // 停止
}
- else{ // 障害物がある場合
- i = 0;
- if(SC < 15){ // 正面15cm以内に障害物が現れた場合
- run = BACK; // 後退
- while(watch() < limit){ // 正面20cm以内に障害物がある間
- /*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
- break;
- }*/
+ if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
+ run = LEFT; // 左折
+ while(i < 3){ // 進行方向確認
+ if(watch() > limit){
+ i++;
}
- run = STOP; // 停止
- }
- if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合
- run = LEFT; // 左折
- while(i < 3){ // 進行方向確認
- /*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
- break;
- } */
- if(watch() > limit){
- i++;
-
- }
else{
i = 0;
}
@@ -418,16 +381,11 @@
switch(houkou){ // 進行方向の場合分け
case 1: // 前の場合
run = ADVANCE; // 前進
- //pc.printf("Advance\r\n");
ThisThread::sleep_for(1000); // 1秒待つ
break;
case 2: // 左の場合
run = LEFT; // 左折
while(i < 3){ // 進行方向確認
- //pc.printf("left\r\n");
- /*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
- break;
- }*/
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
i++; // ループ+
}
@@ -440,10 +398,6 @@
case 3: // 右の場合
run = RIGHT; // 右折
while(i < 3){ // 進行方向確認
- //pc.printf("right\r\n");
- /*if(mode != AVOIDANCE){ // 現在障害物回避モードでないなら
- break;
- }*/
if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
i++; // ループ+
}
@@ -456,22 +410,16 @@
}
}
}
- //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待つ
+ ThisThread::sleep_for(5); // 15ms待つ
trig = 0;
timer.start();
t1=timer.read_ms();
@@ -501,7 +449,7 @@
if((t1-t2)>=10){
DT=100;
}
- pc.printf("私はDTである。%d\r\n",DT);
+ //pc.printf("私はDTである。%d\r\n",DT);
led1 = 0;
return DT;
}
@@ -517,7 +465,7 @@
return;
}
servo.pulsewidth_us(1925); // サーボを左に40度回転
- ThisThread::sleep_for(25); // 250ms待つ
+ ThisThread::sleep_for(140); // 250ms待つ
SLD = watch();
if(SLD < limit){ // 左前20cm以内に障害物がある場合
//if(mode == AVOIDANCE) // 現在障害物回避モードなら
@@ -526,7 +474,7 @@
//return;
}
servo.pulsewidth_us(2400); // サーボを左に90度回転
- ThisThread::sleep_for(25); // 250ms待つ
+ ThisThread::sleep_for(100); // 250ms待つ
SL = watch();
if(SL < limit){ // 左20cm以内に障害物がある場合
//if(mode == AVOIDANCE) // 現在障害物回避モードなら
@@ -534,10 +482,8 @@
//else
//return;
}
- servo.pulsewidth_us(1450); // サーボを中央位置に戻す
- ThisThread::sleep_for(10); // 100ms待つ
servo.pulsewidth_us(925); // サーボを右に40度回転
- ThisThread::sleep_for(25); // 250ms待つ
+ ThisThread::sleep_for(140); // 250ms待つ
SRD = watch();
if(SRD < limit){ // 右前20cm以内に障害物がある場合
//if(mode == AVOIDANCE) // 現在障害物回避モードなら
@@ -546,7 +492,7 @@
//return;
}
servo.pulsewidth_us(500); // サーボを右に90度回転
- ThisThread::sleep_for(25); // 250ms待つ
+ ThisThread::sleep_for(100); // 250ms待つ
SR = watch();
if(SR < limit){ // 右20cm以内に障害物がある場合
//if(mode == AVOIDANCE) // 現在障害物回避モードなら
@@ -555,7 +501,7 @@
//return;
}
servo.pulsewidth_us(1450); // サーボを中央位置に戻す
- ThisThread::sleep_for(10); // 100ms待つ
+ ThisThread::sleep_for(100); // 100ms待つ
if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
flag_a = 1; // 障害物有無フラグに1をセット
}
@@ -648,7 +594,6 @@
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なら
@@ -679,9 +624,6 @@
display(); // ディスプレイ表示
while(1){
- //pc.printf(" main1\r\n");
bChange(); // バッテリー残量更新
- //pc.printf(" main2\r\n");
- ThisThread::sleep_for(1); // 1ms待つ
}
}
\ No newline at end of file