Diff: main.cpp

Revision:

1:9787fdc9b191

Parent:

0:5d6b3a226854

Child:

2:3a891c8d5b82

--- a/main.cpp	Wed Jul 22 02:08:10 2020 +0000
+++ b/main.cpp	Wed Jul 22 03:14:33 2020 +0000
@@ -15,9 +15,8 @@
 int SRD;      // 右前
 int flag = 0; // 障害物有無のフラグ
 const int limit = 20;;        // 障害物の距離のリミット(単位：cm)
-const int sublimit = 15;      // 急な障害物の距離のリミット(単位：cm)
 int DT;       // 距離
-int houkou;   // 進行方向(1:前進 2:左折 3:右折)
+int houkou;   // 進行方向(1:前 2:左 3:右)
 int max;      // 最も遠い距離
 int i;        // ループ変数
 
@@ -43,7 +42,7 @@
     return DT;
 }
 
-/* サーボ関数 */
+/* サーボ制御スレッド */
 void watchsurrounding(void const *n){
     while(1){
         Thread::signal_wait(0x1);       // シグナル待ち
@@ -88,81 +87,83 @@
 /* 障害物回避走行 */
 void avoidance(){
     while(1){
-        if(flag == 0){
-            run = ADVANCE;
+        if(flag == 0){          // 障害物がない場合
+            run = ADVANCE;      // 前進
         }
-        else{
+        else{                   // 障害物がある場合                   
             i = 0;
-            if(SC < 15){
-                run = BACK;
-                while(watch() < limit){
+            if(SC < 15){                    // 正面15cm以内に障害物が現れた場合
+                run = BACK;                 // 後退
+                while(watch() < limit){     // 正面20cm以内に障害物がある間
                 }
-                run = STOP;
+                run = STOP;                 // 停止                 
             }
-            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){
-                run = LEFT;
-                while(i < 100){
-                    if(watch() > limit){
-                        i++;
+            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
+                run = LEFT;                 // 左折                   
+                while(i < 100){             // 進行方向確認
+                    if(watch() > limit){    
+                        i++;                
                     }
-                    else{
-                        i = 0;
+                    else{                   
+                        i = 0;              
                     }
                 }
-                run = STOP;
-            }
-            max = SC;
-            houkou = 1;
-            if(max < SLD || max < SL){
-                if(SL < SLD){
-                    max = SLD;
-                }
-                else{
-                    max = SL;
-                }
-                houkou = 2;
-            }
-            if(max < SRD || max < SR){
-                if(SR < SRD){
-                    max = SRD;
-                }
-                else{
-                    max = SR;
-                }
-                houkou = 3;
+                run = STOP;                 // 停止
             }
-            switch(houkou){
-                case 1:
-                    run = ADVANCE;
-                    break;
-                case 2:
-                    run = LEFT;
-                    while(i < 100){
-                        if(watch() > (max - 2)){
-                            i++;
-                        }
-                        else{
-                            i = 0;
-                        }
+            else {                          // 全方向以外                          
+                max = SC;                   // 正面を最も遠い距離に設定
+                houkou = 1;                 // 進行方向を前に設定
+                if(max < SLD || max < SL){  // 左または左前がより遠い場合
+                    if(SL < SLD){           // 左前が左より遠い場合
+                        max = SLD;          // 左前を最も遠い距離に設定
+                    }
+                    else{                   // 左が左前より遠い場合
+                        max = SL;           // 左を最も遠い距離に設定
+                    }
+                    houkou = 2;             // 進行方向を左に設定
+                }
+                if(max < SRD || max < SR){  // 右または右前がより遠い場合
+                    if(SR < SRD){           // 右前が右より遠い場合
+                        max = SRD;          // 右前を最も遠い距離に設定
+                    }
+                    else{                   // 右が右前よりも遠い場合
+                        max = SR;           // 右を最も遠い距離に設定
                     }
-                    run = STOP;
-                    break;
-                case 3:
-                    run = RIGHT;
-                    while(i < 100){
-                        if(watch() > (max - 2)){
-                            i++;
+                    houkou = 3;             // 進行方向を右に設定
+                }
+                switch(houkou){                         // 進行方向の場合分け
+                    case 1:                             // 前の場合
+                        run = ADVANCE;                  // 前進
+                        thread_sleep_for(100);          // 1秒待つ
+                        break;
+                    case 2:                             // 左の場合
+                        run = LEFT;                     // 左折
+                        while(i < 100){                 // 進行方向確認
+                            if(watch() > (max - 2)){    
+                                i++;
+                            }
+                            else{
+                                i = 0;
+                            }
                         }
-                        else{
-                            i = 0;
+                        run = STOP;                     // 停止
+                        break;
+                    case 3:                             // 右の場合
+                        run = RIGHT;                    // 右折
+                        while(i < 100){                 // 進行方向確認
+                            if(watch() > (max - 2)){
+                                i++;
+                            }
+                            else{
+                                i = 0;
+                            }
                         }
-                    }
-                    run = STOP;
-                    break;
+                        run = STOP;                     // 停止
+                        break;
+                }
             }
         }
-        flag = 0;
-        Thread::signal_set(0x1);
+        flag = 0;                   // フラグを0にセット
+        Thread::signal_set(0x1);    // watchsurroundingのシグナルをセット
     }
-}  
-        
\ No newline at end of file
+}          
\ No newline at end of file