mbed_robotcar
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 |
--- 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をセット
- }
+ }*/
}
/* ディスプレイ表示関数 */