Diff: main.cpp

Revision:

0:5d6b3a226854

Child:

1:9787fdc9b191

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 22 02:08:10 2020 +0000
@@ -0,0 +1,168 @@
+#include "mbed.h"
+#include "rtos.h"
+
+DigitalOut trig(p6);    // 超音波センサのtrigピンをp6に接続
+DigitalIn  echo(p7);    // 超音波センサのechoピンをp7に接続
+PwmOut   servo(p25);    // サーボコントロールピン(p25)
+
+Timer timer;            // 距離計測用タイマー
+
+/* 障害物検知用の変数設定 */
+int SC;       // 正面   
+int SL;       // 左
+int SR;       // 右
+int SLD;      // 左前
+int SRD;      // 右前
+int flag = 0; // 障害物有無のフラグ
+const int limit = 20;;        // 障害物の距離のリミット(単位：cm)
+const int sublimit = 15;      // 急な障害物の距離のリミット(単位：cm)
+int DT;       // 距離
+int houkou;   // 進行方向(1:前進 2:左折 3:右折)
+int max;      // 最も遠い距離
+int i;        // ループ変数
+
+/* 超音波センサ関数 */
+int watch(){
+    trig = 0;
+    thread_sleep_for(0.5);          // 5ms待つ
+    trig = 1;
+    thread_sleep_for(1.5);          // 15ms待つ
+    trig = 0;
+    while(echo.read() == 0){
+    }
+    timer.start();                  // 距離計測タイマースタート
+    while(echo.read() == 1){
+    }
+    timer.stop();                   // 距離計測タイマーストップ
+    DT = timer.read_us()*0.01657;   // 距離計算
+    if(DT > 400){                   // 検知範囲外なら400cmに設定
+        DT = 400;
+    }
+    timer.reset();                  // 距離計測タイマーリセット
+    thread_sleep_for(3);            // 30ms待つ
+    return DT;
+}
+
+/* サーボ関数 */
+void watchsurrounding(void const *n){
+    while(1){
+        Thread::signal_wait(0x1);       // シグナル待ち
+        SC = watch();
+        if(SC < limit){                 // 前方20cm以内に障害物がある場合
+            run = STOP;                 // 停止
+        }
+        servo.pulsewidth_us(1925);      // サーボを左に40度回転
+        thread_sleep_for(25);           // 250ms待つ
+        SLD = watch();
+        if(SLD < limit){                // 左斜め20cm以内に障害物がある場合
+            run = STOP;                 // 停止
+        }
+        servo.pulsewidth_us(2400);      // サーボを左に90度回転
+        thread_sleep_for(25);           // 250ms待つ
+        SL = watch();
+        if(SL < limit){                 // 左20cm以内に障害物がある場合
+            run = STOP;                 // 停止
+        }
+        servo.pulsewidth_us(1450);
+        thread_sleep_for(10);
+        servo.pulsewidth_us(925);       // サーボを右に40度回転
+        thread_sleep_for(25);           // 250ms待つ
+        SRD = watch();
+        if(SRD < limit){                // 右斜め20cm以内に障害物がある場合
+            run = STOP;                 // 停止
+        }
+        servo.pulsewidth_us(500);       // サーボを右に90度回転
+        thread_sleep_for(25);           // 250ms待つ
+        SR = watch();
+        if(SR < limit){                 // 右20cm以内に障害物がある場合
+            run = STOP;                 // 停止
+        }
+        servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
+        thread_sleep_for(10);           // 100ms待つ
+        if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
+            flag = 1;                   // フラグに1をセット
+        }
+    }
+}
+
+/* 障害物回避走行 */
+void avoidance(){
+    while(1){
+        if(flag == 0){
+            run = ADVANCE;
+        }
+        else{
+            i = 0;
+            if(SC < 15){
+                run = BACK;
+                while(watch() < limit){
+                }
+                run = STOP;
+            }
+            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){
+                run = LEFT;
+                while(i < 100){
+                    if(watch() > limit){
+                        i++;
+                    }
+                    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;
+            }
+            switch(houkou){
+                case 1:
+                    run = ADVANCE;
+                    break;
+                case 2:
+                    run = LEFT;
+                    while(i < 100){
+                        if(watch() > (max - 2)){
+                            i++;
+                        }
+                        else{
+                            i = 0;
+                        }
+                    }
+                    run = STOP;
+                    break;
+                case 3:
+                    run = RIGHT;
+                    while(i < 100){
+                        if(watch() > (max - 2)){
+                            i++;
+                        }
+                        else{
+                            i = 0;
+                        }
+                    }
+                    run = STOP;
+                    break;
+            }
+        }
+        flag = 0;
+        Thread::signal_set(0x1);
+    }
+}  
+        
\ No newline at end of file