Diff: main.cpp

Revision:

18:6cca64c7dbc3

Parent:

17:f7259ab2fe86

Child:

19:c6f9f010bd9e

diff -r f7259ab2fe86 -r 6cca64c7dbc3 main.cpp
--- a/main.cpp	Tue Aug 04 08:43:58 2020 +0000
+++ b/main.cpp	Wed Aug 05 01:09:50 2020 +0000
@@ -123,7 +123,6 @@
 void decodeIR(void const *argument){ 
    while(1){ 
         // 受信待ち
-        //pc.printf("decodeIR\r\n");
         if (ir.getState() == ReceiverIR::Received){ // コード受信
             bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
             if(bitcount > 1){        // 受信成功
@@ -134,7 +133,6 @@
                 if(mode != SPEED){   // スピードモード以外なら
                     beforeMode=mode; // 前回のモードに現在のモードを設定       
                 }
-                //pc.printf("%0x\r\n",code);
                 switch(code){
                     case 0x40bf27d8:    // クイック
                         //pc.printf("mode = SPEED\r\n");
@@ -160,6 +158,7 @@
                             avoi_thread = new Thread(avoidance);
                             avoi_thread -> set_priority(osPriorityHigh);
                         }
+                        flag_a = 0;
                         mode=AVOIDANCE;     // 障害物回避モード
                         run = ADVANCE;      // 前進
                         display();          // ディスプレイ表示
@@ -210,14 +209,13 @@
             viewTimer.reset();  // タイマーリセット
             display();          // ディスプレイ表示
         }
-        ThisThread::sleep_for(60);  // 90ms待つ
+        ThisThread::sleep_for(90);  // 90ms待つ
     }       
 }
 
 /* モーター制御スレッド */
 void motor(void const *argument){
     while(1){
-        //pc.printf("motor\r\n");
         /* 走行状態の場合分け */
         switch(run){
             /* 前進 */
@@ -302,92 +300,57 @@
             /* センサー値によって場合分け */
             switch(sensorNum){
                 case 1:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        run = ADVANCE;      // 低速で前進
-                    //}
-                    //pc.printf("ADVANCE");
+                    run = ADVANCE;      // 低速で前進
                     break;
                 case 2:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        run = RIGHT;        // 低速で右折
-                    //}
-                    //pc.printf("RIGHT");
+                    run = RIGHT;        // 低速で右折
                     break;
                 case 3:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        run = LEFT;         // 低速で左折
-                    //}
-                    //pc.printf("LEFT");
+                    run = LEFT;         // 低速で左折
                     break;
                 case 4:
-                    //if(mode==LINE_TRACE){   // 現在ライントレースモードなら
-                        flag_sp = flag_sp % 3 + 3;   
-                        run = RIGHT;        // 中速で右折
-                    //}
-                    //pc.printf("RIGHT");
+                    flag_sp = flag_sp % 3 + 3;   
+                    run = RIGHT;        // 中速で右折
                     break;
                 case 5:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        flag_sp = flag_sp % 3 + 3;
-                        run = LEFT;         // 中速で左折
-                    //}
-                    //pc.printf("LEFT");
+                    flag_sp = flag_sp % 3 + 3;
+                    run = LEFT;         // 中速で左折
                     break;
                 case 6:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        flag_sp = flag_sp % 3 + 6;
-                        run = RIGHT;        // 高速で右折
-                    //}
-                    //pc.printf("RIGHT");
+                    flag_sp = flag_sp % 3 + 6;
+                    run = RIGHT;        // 高速で右折
                     break;
                 case 7:
-                    //if(mode == LINE_TRACE){ // 現在ライントレースモードなら
-                        flag_sp = flag_sp % 3 + 6;
-                        run = LEFT;         // 高速で左折
-                    //}
-                    //pc.printf("LEFT");
+                    flag_sp = flag_sp % 3 + 6;
+                    run = LEFT;         // 高速で左折
                     break;
             }
             ThisThread::sleep_for(30);      // 30ms待つ
-        /*}else{          
-            ThisThread::sleep_for(1);       // 1ms待つ
-        }*/
-    }
+        }
 }
 
 /* 障害物回避走行スレッド */
 void avoidance(/*void const *argument*/){
     while(1){  
-        //if(mode == AVOIDANCE){        // 現在障害物回避モードなら
-            //pc.printf("avoidance\r\n");
-            watchsurrounding();
-            pc.printf("%d  %d  %d  %d  %d  \r\n",SL,SLD,SC,SRD,SR);
-            
-            if(flag_a == 0){                     // 障害物がない場合
-                //if(mode == AVOIDANCE)            // 現在障害物回避モードなら
-                    run = ADVANCE;               // 前進
+        watchsurrounding();
+        pc.printf("%d  %d  %d  %d  %d  \r\n",SL,SLD,SC,SRD,SR);
+        if(flag_a == 0){                     // 障害物がない場合
+            run = ADVANCE;               // 前進
+        }
+        else{                                // 障害物がある場合                   
+            i = 0;
+            if(SC < 15){                     // 正面15cm以内に障害物が現れた場合
+                run = BACK;                  // 後退
+                while(watch() < limit){      // 正面20cm以内に障害物がある間
+                }
+                    run = STOP;                  // 停止                 
             }
-            else{                                // 障害物がある場合                   
-                i = 0;
-                if(SC < 15){                     // 正面15cm以内に障害物が現れた場合
-                    run = BACK;                  // 後退
-                    while(watch() < limit){      // 正面20cm以内に障害物がある間
-                        /*if(mode != AVOIDANCE){   // 現在障害物回避モードでないなら
-                            break;    
-                        }*/
+            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
+                run = LEFT;                  // 左折                   
+                while(i < 3){               // 進行方向確認 
+                    if(watch() > limit){    
+                        i++;
                     }
-                    run = STOP;                  // 停止                 
-                }
-                if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
-                    run = LEFT;                  // 左折                   
-                    while(i < 3){               // 進行方向確認
-                        /*if(mode != AVOIDANCE){   // 現在障害物回避モードでないなら
-                            break;    
-                        } */  
-                        if(watch() > limit){    
-                            i++;
-                                         
-                        }
                         else{                   
                             i = 0;              
                         }
@@ -418,16 +381,11 @@
                     switch(houkou){                        // 進行方向の場合分け
                         case 1:                            // 前の場合
                             run = ADVANCE;                 // 前進
-                            //pc.printf("Advance\r\n");
                             ThisThread::sleep_for(1000);   // 1秒待つ
                             break;
                         case 2:                            // 左の場合
                             run = LEFT;                    // 左折
                             while(i < 3){                 // 進行方向確認
-                                //pc.printf("left\r\n");
-                                /*if(mode != AVOIDANCE){     // 現在障害物回避モードでないなら
-                                        break;    
-                                }*/
                                 if(watch() > (far - 2)){   // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
                                     i++;                   // ループ+  
                                 }
@@ -440,10 +398,6 @@
                         case 3:                            // 右の場合
                             run = RIGHT;                   // 右折
                             while(i < 3){                 // 進行方向確認
-                                //pc.printf("right\r\n");
-                                /*if(mode != AVOIDANCE){     // 現在障害物回避モードでないなら
-                                    break;    
-                                }*/
                                 if(watch() > (far - 2)){   // 正面の計測距離と最も遠い距離が一致したら(誤差-2cm)
                                     i++;                   // ループ+  
                                 }
@@ -456,22 +410,16 @@
                     }
                 }
             }
-            //pc.printf("こんにちは！\r\n");
             flag_a = 0;                   // 障害物有無フラグを0にセット
-            //pc.printf("                avoidance\r\n");
-        /*}else{  
-            ThisThread::sleep_for(1);     // 1ms待つ
-        }*/
     }   
 }
 
 /* 距離計測関数 */
 int watch(){
-    //pc.printf("watch\r\n");
     trig = 0;
     ThisThread::sleep_for(5);       // 5ms待つ
     trig = 1;
-    ThisThread::sleep_for(15);      // 15ms待つ
+    ThisThread::sleep_for(5);      // 15ms待つ
     trig = 0;
     timer.start();
     t1=timer.read_ms();
@@ -501,7 +449,7 @@
     if((t1-t2)>=10){
         DT=100;
     }
-    pc.printf("私はDTである。%d\r\n",DT);
+    //pc.printf("私はDTである。%d\r\n",DT);
     led1 = 0;
     return DT;
 }
@@ -517,7 +465,7 @@
             return;          
     }
     servo.pulsewidth_us(1925);      // サーボを左に40度回転
-    ThisThread::sleep_for(25);     // 250ms待つ
+    ThisThread::sleep_for(140);     // 250ms待つ
     SLD = watch();
     if(SLD < limit){                // 左前20cm以内に障害物がある場合
         //if(mode == AVOIDANCE)       // 現在障害物回避モードなら
@@ -526,7 +474,7 @@
             //return; 
     }
     servo.pulsewidth_us(2400);      // サーボを左に90度回転
-    ThisThread::sleep_for(25);     // 250ms待つ
+    ThisThread::sleep_for(100);     // 250ms待つ
     SL = watch();
     if(SL < limit){                 // 左20cm以内に障害物がある場合
         //if(mode == AVOIDANCE)       // 現在障害物回避モードなら
@@ -534,10 +482,8 @@
         //else
             //return; 
     }
-    servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
-    ThisThread::sleep_for(10);     // 100ms待つ
     servo.pulsewidth_us(925);       // サーボを右に40度回転
-    ThisThread::sleep_for(25);     // 250ms待つ
+    ThisThread::sleep_for(140);     // 250ms待つ
     SRD = watch();
     if(SRD < limit){                // 右前20cm以内に障害物がある場合
         //if(mode == AVOIDANCE)       // 現在障害物回避モードなら
@@ -546,7 +492,7 @@
             //return; 
     }
     servo.pulsewidth_us(500);       // サーボを右に90度回転
-    ThisThread::sleep_for(25);     // 250ms待つ
+    ThisThread::sleep_for(100);     // 250ms待つ
     SR = watch();
     if(SR < limit){                 // 右20cm以内に障害物がある場合
         //if(mode == AVOIDANCE)       // 現在障害物回避モードなら
@@ -555,7 +501,7 @@
             //return;     
     }
     servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
-    ThisThread::sleep_for(10);     // 100ms待つ
+    ThisThread::sleep_for(100);     // 100ms待つ
     if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
         flag_a = 1;                 // 障害物有無フラグに1をセット
     }
@@ -648,7 +594,6 @@
     mutex.lock();                    // ミューテックスロック
     lcd.setAddress(0,0);
     lcd.printf("Battery:%3d%%",b);   // バッテリー残量表示
-    //pc.printf("                                                                              bChange2\r\n");
     mutex.unlock();                  // ミューテックスアンロック                 
     if(b <= 30){                     // バッテリー残量30%以下なら
         if(flag_t == 0){             // バックライトタイマーフラグが0なら
@@ -679,9 +624,6 @@
     display();              // ディスプレイ表示
     
     while(1){
-        //pc.printf("                                    main1\r\n");
         bChange();          // バッテリー残量更新
-        //pc.printf("                                    main2\r\n");
-        ThisThread::sleep_for(1); // 1ms待つ
     }
 }
\ No newline at end of file