Diff: main.cpp

Revision:

2:38825726cb1b

Parent:

1:5bb497a38344

Child:

3:2ae6218973be

diff -r 5bb497a38344 -r 38825726cb1b main.cpp
--- a/main.cpp	Wed Jul 22 07:43:56 2020 +0000
+++ b/main.cpp	Mon Jul 27 08:49:28 2020 +0000
@@ -8,11 +8,18 @@
 #include "rtos.h"
 #include <stdint.h>
 #include "platform/mbed_thread.h"
+#include "TextLCD.h"
+#define MIN_V 2.23
+#define MAX_V 3.3
+
+//bChangeのいろいろ
+I2C i2c_lcd(p9, p10);
+TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
+int b = 0;
+AnalogIn battery(p15);
+double test;
 
 Serial pc(USBTX, USBRX);
-
-
-DigitalIn button(p15);
 // ポートp15を赤外線受信モジュールの接続先に指定
 ReceiverIR ir_rx(p15);
 RemoteIR::Format format;
@@ -20,11 +27,53 @@
 uint32_t bitcount;
 uint32_t code;
 
-void rimokon();
+//障害物回避の設定
+DigitalOut trig(p6);    // 超音波センサのtrigピンをp6に接続
+DigitalIn  echo(p7);    // 超音波センサのechoピンをp7に接続
+PwmOut   servo(p25);    // サーボコントロールピン(p25)
+
+enum Mode{
+    ADVANCE,
+    RIGHT,
+    LEFT,
+    BACK,
+    STOP,
+    LINE_TRACE,
+    AVOIDANCE,
+    READY
+};
+
+Mode run;
+Mode mode;
+
+Timer timer;            // 距離計測用タイマー
 
-void rimokon(/*void const *argument*/){  
-        
-       
+/* 障害物検知用の変数設定 */
+int SC;                 // 正面   
+int SL;                 // 左
+int SR;                 // 右
+int SLD;                // 左前
+int SRD;                // 右前
+int flag = 0;           // 障害物有無のフラグ
+const int limit = 20;   // 障害物の距離のリミット(単位：cm)
+int DT;                 // 距離
+int houkou;             // 進行方向(1:前 2:左 3:右)
+int far;                // 最も遠い距離
+int i;                  // ループ変数
+
+void rimokon(void const *argument);
+void avoidance(void const *argument);
+void motor(void const *argument);
+void watchsurrounding();
+int watch();
+void bChange(void const *argument);
+Thread thread1(rimokon, NULL, osPriorityRealtime);
+Thread thread2(motor, NULL, osPriorityHigh);
+Thread thread3(avoidance, NULL, osPriorityAboveNormal);
+Thread thread4(bChange, NULL, osPriorityBelowNormal);
+
+void rimokon(void const *argument){  
+    while(1){  
         // 受信待ち
         if (ir_rx.getState() == ReceiverIR::Received) {
             // コード受信
@@ -132,9 +181,11 @@
                         pc.printf("↓\r\n");
                         break;
                     case 0x40bf5fa0://←
+                        mode=LINE_TRACE;
                         pc.printf("←\r\n");
                         break;
                     case 0x40bf5ba4://→
+                        mode=AVOIDANCE;
                         pc.printf("→\r\n");
                         break;
                     case 0x40bf3dc2://決定
@@ -171,17 +222,184 @@
                 }
             }
         }
-        
+        ThisThread::sleep_for(10);
+    }        
+}
+void motor(void const *argument){
+    while(1){
+        pc.printf("motor\r\n"); 
+        ThisThread::sleep_for(20);
+    }
+}
+/* 障害物回避走行 */
+void avoidance(void const *argument){
+    
+    while(1){
+    if(mode==AVOIDANCE){
+        pc.printf("avoidance\r\n"); 
+        if(flag == 0){                      // 障害物がない場合
+            run = ADVANCE;                  // 前進
+        }
+     /*   else{                               // 障害物がある場合                   
+            i = 0;
+            if(SC < 15){                    // 正面15cm以内に障害物が現れた場合
+                run = BACK;                 // 後退
+                while(watch() < limit){     // 正面20cm以内に障害物がある間
+                    
+                }
+                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;                 // 停止
+            }
+            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;                  // 前進
+                        thread_sleep_for(100);          // 1秒待つ
+                        break;
+                    case 2:                             // 左の場合
+                        run = LEFT;                     // 左折
+                        while(i < 100){                 // 進行方向確認
+                            if(watch() > (far - 2)){    
+                                i++;
+                            }
+                            else{
+                                i = 0;
+                            }
+                        }
+                        run = STOP;                     // 停止
+                        break;
+                    case 3:                             // 右の場合
+                        run = RIGHT;                    // 右折
+                        while(i < 100){                 // 進行方向確認
+                            if(watch() > (far - 2)){
+                                i++;
+                            }
+                            else{
+                                i = 0;
+                            }
+                        }
+                        run = STOP;                     // 停止
+                        break;
+                }
+            }
+        }*/
+        flag = 0;                   // フラグを0にセット
+        watchsurrounding();
+    }
+    }   
+}
+int watch(){
+    pc.printf("watch\r\n");
+    trig = 0;
+    ThisThread::sleep_for(5);           // 5ms待つ
+    trig = 1;
+    ThisThread::sleep_for(15);          // 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();                  // 距離計測タイマーリセット
+    ThisThread::sleep_for(30);      // 30ms待つ
+    return DT;
 }
 
+void watchsurrounding(){
+    pc.printf("watchsurrounding\r\n");
+    SC = watch();
+    if(SC < limit){                 // 正面20cm以内に障害物がある場合
+        run = STOP;                 // 停止
+    }
+    servo.pulsewidth_us(1925);      // サーボを左に40度回転
+    ThisThread::sleep_for(250);           // 250ms待つ
+    SLD = watch();
+    if(SLD < limit){                // 左前20cm以内に障害物がある場合
+        run = STOP;                 // 停止
+    }
+    servo.pulsewidth_us(2400);      // サーボを左に90度回転
+    ThisThread::sleep_for(250);           // 250ms待つ
+    SL = watch();
+    if(SL < limit){                 // 左20cm以内に障害物がある場合
+        run = STOP;                 // 停止
+    }
+    servo.pulsewidth_us(1450);
+    ThisThread::sleep_for(100);
+    servo.pulsewidth_us(925);       // サーボを右に40度回転
+    ThisThread::sleep_for(250);           // 250ms待つ
+    SRD = watch();
+    if(SRD < limit){                // 右前20cm以内に障害物がある場合
+        run = STOP;                 // 停止
+    }
+    servo.pulsewidth_us(500);       // サーボを右に90度回転
+    ThisThread::sleep_for(250);           // 250ms待つ
+    SR = watch();
+    if(SR < limit){                 // 右20cm以内に障害物がある場合
+        run = STOP;                 // 停止
+    }
+    servo.pulsewidth_us(1450);      // サーボを中央位置に戻す
+    ThisThread::sleep_for(100);           // 100ms待つ
+    if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
+        flag = 1;                   // フラグに1をセット
+    }
+}
+
+
+void bChange(void const *argument){
+    while(1){
+        //lcd.setBacklight(TextLCD::LightOn);
+        test = battery.read()*MAX_V;
+        b = (int)((test - MIN_V)/0.107 + 0.5)*10;
+        lcd.locate(0,0);
+        lcd.printf("Battery:%3d%%",b);
+        pc.printf("bChange\r\n");
+    }
+}
 int main() {
     
-    //RtosTimer rimokon_timer(rimokon, osTimerPeriodic, (void *)0); // set RTOS timer for sensor
-    //rimokon_timer.start(10);
-    //Thread thread1(rimokon , NULL , osPriorityHigh);
+    
+    
     while(1){
-        if(button.read()==0){
+        /*if(button.read()==0){
             rimokon();
-        }    
+        }    */
+        pc.printf("main\r\n"); 
+        ThisThread::sleep_for(40);
     }
 }
\ No newline at end of file