Diff: main.cpp

Revision:

7:19ebd0851f49

Parent:

6:800a745c7f2e

Child:

8:a47dbf4fa455

--- a/main.cpp	Thu Jul 30 02:18:48 2020 +0000
+++ b/main.cpp	Thu Jul 30 04:40:26 2020 +0000
@@ -15,8 +15,8 @@
 /* マクロ定義、列挙型定義 */
 #define MIN_V 2.0
 #define MAX_V 2.67
-#define     LOW     0       // 
-#define     HIGH    1       // 
+#define     LOW     0        
+#define     HIGH    1        
 #define     NORMAL   0       // フラグ値
 #define     FAST   1       // フラグ値
 #define     VERYFAST   2       // フラグ値
@@ -268,35 +268,49 @@
     
             switch(sensorNum){
                 case 1:
-                    run = ADVANCE;
+                    if(mode==LINE_TRACE){
+                        run = ADVANCE;
+                    }
                     pc.printf("ADVANCE");
                     break;
                 case 2:
-                    run = RIGHT;
+                    if(mode==LINE_TRACE){
+                        run = RIGHT;
+                    }
                     pc.printf("RIGHT");
                     break;
                 case 3:
-                    run = LEFT;
+                    if(mode==LINE_TRACE){
+                        run = LEFT;
+                    }
                     pc.printf("LEFT");
                     break;
                 case 4:
-                    flag_sp %= 3 + 3;
-                    run = RIGHT;
+                    if(mode==LINE_TRACE){
+                        flag_sp %= 3 + 3;
+                        run = RIGHT;
+                    }
                     pc.printf("RIGHT");
                     break;
                 case 5:
-                    flag_sp %= 3 + 3;
-                    run = LEFT;
+                    if(mode==LINE_TRACE){
+                        flag_sp %= 3 + 3;
+                        run = LEFT;
+                    }
                     pc.printf("LEFT");
                     break;
                 case 6:
-                    flag_sp %= 3 + 6;
-                    run = RIGHT;
+                    if(mode==LINE_TRACE){
+                        flag_sp %= 3 + 6;
+                        run = RIGHT;
+                    }
                     pc.printf("RIGHT");
                     break;
                 case 7:
-                    flag_sp %= 3 + 6;
-                    run = LEFT;
+                    if(mode==LINE_TRACE){
+                        flag_sp %= 3 + 6;
+                        run = LEFT;
+                    }
                     pc.printf("LEFT");
                     break;
             }
@@ -313,10 +327,12 @@
     while(1){  
         if(mode==AVOIDANCE){
             pc.printf("avoidance\r\n");
-            pc.printf("%d  %d  %d  %d  %d  \r\n",SL,SLD,SC,SRD,SR); 
+            watchsurrounding();
+            pc.printf("%d  %d  %d  %d  %d  \r\n",SL,SLD,SC,SRD,SR); a:
             
             if(flag_a == 0){                      // 障害物がない場合
-                run = ADVANCE;                  // 前進
+                if(mode==AVOIDANCE)
+                    run = ADVANCE;                  // 前進
             }
             else{                               // 障害物がある場合                   
                 i = 0;
@@ -411,7 +427,6 @@
             }
             pc.printf("こんにちは！\r\n");
             flag_a = 0;                   // フラグを0にセット
-            watchsurrounding();
             pc.printf("                avoidance\r\n");
         }else{  
             ThisThread::sleep_for(1);
@@ -452,19 +467,28 @@
     pc.printf("watchsurrounding\r\n");
     SC = watch();
     if(SC < limit){                 // 正面20cm以内に障害物がある場合
-        run = STOP;                 // 停止
+        if(mode==AVOIDANCE)
+            run = STOP;                 // 停止
+        else
+            return;          
     }
     servo.pulsewidth_us(1925);      // サーボを左に40度回転
     ThisThread::sleep_for(250);           // 250ms待つ
     SLD = watch();
     if(SLD < limit){                // 左前20cm以内に障害物がある場合
-        run = STOP;                 // 停止
+        if(mode==AVOIDANCE)
+            run = STOP;                 // 停止
+        else
+            return; 
     }
     servo.pulsewidth_us(2400);      // サーボを左に90度回転
     ThisThread::sleep_for(250);           // 250ms待つ
     SL = watch();
     if(SL < limit){                 // 左20cm以内に障害物がある場合
-        run = STOP;                 // 停止
+        if(mode==AVOIDANCE)
+            run = STOP;                 // 停止
+        else
+            return; 
     }
     servo.pulsewidth_us(1450);
     ThisThread::sleep_for(100);
@@ -472,13 +496,19 @@
     ThisThread::sleep_for(250);           // 250ms待つ
     SRD = watch();
     if(SRD < limit){                // 右前20cm以内に障害物がある場合
-        run = STOP;                 // 停止
+        if(mode==AVOIDANCE)
+            run = STOP;                 // 停止
+        else
+            return; 
     }
     servo.pulsewidth_us(500);       // サーボを右に90度回転
     ThisThread::sleep_for(250);           // 250ms待つ
     SR = watch();
     if(SR < limit){                 // 右20cm以内に障害物がある場合
-        run = STOP;                 // 停止
+        if(mode==AVOIDANCE)
+            run = STOP;                 // 停止
+        else
+            return;     
     }
     servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
     ThisThread::sleep_for(100);           // 100ms待つ