watch変えた
Dependencies: RemoteIR TextLCD
Revision 24:cc88728885ca, committed 2020-08-06
- Comitter:
- yangtzuli
- Date:
- Thu Aug 06 01:36:00 2020 +0000
- Parent:
- 23:8c862b55fa1f
- Commit message:
- fhgfxh
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8c862b55fa1f -r cc88728885ca main.cpp --- a/main.cpp Wed Aug 05 07:18:08 2020 +0000 +++ b/main.cpp Thu Aug 06 01:36:00 2020 +0000 @@ -201,6 +201,7 @@ } if(mode != AVOIDANCE && avoi_thread->get_state() != Thread::Deleted){ avoi_thread->terminate(); + servo.pulsewidth_us(1450); // サーボを中央位置に戻す } } } @@ -336,8 +337,7 @@ pc.printf("%d %d %d %d %d \r\n",SL,SLD,SC,SRD,SR); if(flag_a == 0){ // 障害物がない場合 run = ADVANCE; // 前進 - } - else{ // 障害物がある場合 + }else{ // 障害物がある場合 i = 0; if(SC < 15){ // 正面15cm以内に障害物が現れた場合 run = BACK; // 後退 @@ -345,72 +345,71 @@ } run = STOP; // 停止 } - if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 + if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){ // 全方向に障害物がある場合 run = LEFT; // 左折 - while(i < 3){ // 進行方向確認 + while(i < 1){ // 進行方向確認 + flag_sp = flag_sp % 3 + 6; 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; // 前進 - ThisThread::sleep_for(1000); // 1秒待つ - break; - case 2: // 左の場合 - run = LEFT; // 左折 - while(i < 3){ // 進行方向確認 - if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - i++; // ループ+ - } - else{ - i = 0; - } - } - run = STOP; // 停止 - break; - case 3: // 右の場合 - run = RIGHT; // 右折 - while(i < 3){ // 進行方向確認 - if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) - i++; // ループ+ - } - else{ - i = 0; - } - } - run = STOP; // 停止 - break; + }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; // 前進 + ThisThread::sleep_for(1000); // 1秒待つ + break; + case 2: // 左の場合 + + run = LEFT; // 左折 + while(i < 1){ // 進行方向確認 + flag_sp = flag_sp % 3 + 6; + if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) + i++; // ループ+ + }else{ + i = 0; + } + } + run = STOP; // 停止 + break; + case 3: // 右の場合 + + run = RIGHT; // 右折 + while(i < 1){ // 進行方向確認 + flag_sp = flag_sp % 3 + 6; + if(watch() > (far - 2)){ // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm) + i++; // ループ+ + }else{ + i = 0; + } + } + run = STOP; // 停止 + break; + } } - flag_a = 0; // 障害物有無フラグを0にセット + } + flag_a = 0; // 障害物有無フラグを0にセット } } @@ -431,16 +430,17 @@ } timer.stop(); timer.reset(); - if((t1-t2) >= 10){ - run = STOP; - } + /*if((t1-t2) >= 10){ + run = STOP;*/ }while((t1-t2) >= 10); timer.start(); // 距離計測タイマースタート while(echo.read() == 1){ } timer.stop(); // 距離計測タイマーストップ DT = (int)(timer.read_us()*0.01657); // 距離計算 - if(DT > 150){ // 検知範囲外なら100cmに設定 + if(DT > 1000){ + DT = -1; + }else if(DT > 150){ // 検知範囲外なら100cmに設定 DT = 150; } timer.reset(); // 距離計測タイマーリセット @@ -450,47 +450,57 @@ /* 障害物検知関数 */ void watchsurrounding(){ - servo.pulsewidth_us(1450); // サーボを中央位置に戻す - ThisThread::sleep_for(90); // 100ms待つ + //servo.pulsewidth_us(1450); // サーボを中央位置に戻す + //ThisThread::sleep_for(200); // 100ms待つ SC = watch(); - if(SC < limit){ // 正面20cm以内に障害物がある場合 - run = STOP; // 停止 + if(SC < limit){ // 正面20cm以内に障害物がある場合 + if(SC!=-1){ + run = STOP; // 停止 + flag_a = 1; + } } servo.pulsewidth_us(1925); // サーボを左に40度回転 - ThisThread::sleep_for(200); // 250ms待つ + ThisThread::sleep_for(100); // 250ms待つ SLD = watch(); if(SLD < limit){ // 左前20cm以内に障害物がある場合 run = STOP; // 停止 + flag_a = 1; } servo.pulsewidth_us(2400); // サーボを左に90度回転 - ThisThread::sleep_for(200); // 250ms待つ + ThisThread::sleep_for(100); // 250ms待つ SL = watch(); if(SL < limit){ // 左20cm以内に障害物がある場合 run = STOP; // 停止 + flag_a = 1; } servo.pulsewidth_us(1450); - ThisThread::sleep_for(90); + ThisThread::sleep_for(250); SC = watch(); if(SC < limit){ - run = STOP; + if(SC!=-1){ + run = STOP; // 停止 + flag_a = 1; + } } servo.pulsewidth_us(925); // サーボを右に40度回転 - ThisThread::sleep_for(200); // 250ms待つ + ThisThread::sleep_for(100); // 250ms待つ SRD = watch(); if(SRD < limit){ // 右前20cm以内に障害物がある場合 run = STOP; // 停止 + flag_a = 1; } servo.pulsewidth_us(500); // サーボを右に90度回転 - ThisThread::sleep_for(200); // 250ms待つ + ThisThread::sleep_for(100); // 250ms待つ SR = watch(); if(SR < limit){ // 右20cm以内に障害物がある場合 - run = STOP; // 停止 + run = STOP; // 停止 + flag_a = 1; } servo.pulsewidth_us(1450); // サーボを中央位置に戻す - ThisThread::sleep_for(80); // 100ms待つ - if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 + ThisThread::sleep_for(250); // 100ms待つ + /*if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合 flag_a = 1; // 障害物有無フラグに1をセット - } + }*/ } /* ディスプレイ表示関数 */