watch変えた

Dependencies:   RemoteIR TextLCD

Files at this revision

API Documentation at this revision

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