Diff: main.cpp

Revision:

3:dd3b7c3c327b

Parent:

2:f568ec2e6955

Child:

4:04ca6a211b85

--- a/main.cpp	Mon Dec 04 05:11:53 2017 +0000
+++ b/main.cpp	Thu Dec 07 00:37:17 2017 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "hcsr04.h"
 
 PwmOut mypwm4(D4);
 PwmOut mypwm6(D6);
@@ -6,10 +7,14 @@
 PwmOut mypwm3(D3);
 
 Ticker flag;
+Ticker timerflag;
 Timer t;
 
 DigitalOut myled(LED1);
 
+HCSR04  usensor(D9,D10); // Trigger(DO), Echo(PWMIN)
+unsigned int dist;
+
 
 #define STATE_A (0)
 #define STATE_B (1)
@@ -18,13 +23,18 @@
 #define STATE_E (4)
 #define STATE_A2 (5)
 
-char mode;
+char mode,fase;
 int wait_flag;
 int timer;
 
-void flg(){
+void tflg(){
         timer = t.read(); 
         }
+/*void flg(){
+        usensor.start();
+        dist=usensor.get_dist_cm();
+        printf("dist:%u\n", dist);
+        }*/
 
 int main() {
     
@@ -37,23 +47,29 @@
 mypwm2.period_ms(100);
 mypwm3.period_ms(100);
     
-flag.attach(&flg, 1.0);
+timerflag.attach(&tflg, 0.1);
+/*flag.attach(&flg, 1.0);*/
+
+      
+while(1){
+    
+usensor.start();
+        wait(0.2); 
+        dist=usensor.get_dist_cm();
+        /*printf("cm: %ld\n",dist); */
 
 
-        
-while(1){
-    
-    switch(timer)
+  /*  switch(timer)
 {
     case 8.0:
         wait_flag = 1;
         mode = STATE_A;
         break;
-    case 13.0:
+    case 12.0:
         wait_flag = 1;
         mode = STATE_B;
         break;
-    case 17.0:
+    case 15.5:
         wait_flag = 1;
         mode = STATE_A2;
         break;
@@ -74,7 +90,51 @@
         mode = STATE_A;
         break; 
         
-}
+}*/
+
+switch (fase)
+        {
+            case 0:
+                if(timer == 5){   
+                    mode = STATE_A;
+                    t.stop();
+                    t.reset();
+                    }
+                if(dist > 45){
+                    fase = 1;
+                    t.start();
+                }
+                break;
+            
+            case 1:
+                if(timer == 0){
+                    wait_flag = 1;
+                    mode = STATE_B;
+                    }else if(timer == 1.0){
+                    wait_flag = 1;
+                    mode = STATE_A;
+                    t.stop();
+                    t.reset();
+                    fase = 2;
+                    t.start();
+                    }
+                break;
+            case 2:
+                if(timer == 6.0){
+                    wait_flag = 1;
+                    mode = STATE_D;
+                    }else if(timer == 16.0){
+                        wait_flag = 1;
+                        mode = STATE_A;
+                        }else if(timer == 26.0){
+                            wait_flag = 1;
+                            mode = STATE_D;
+                            }else if(timer == 36.0){
+                                wait_flag = 1;
+                                mode = STATE_A;}else if(timer == 46.0){
+                                    wait_flag = 1;
+                                    mode = STATE_D;
+        }
 
 // 状態が切り替わるときは一時停止
         if(wait_flag==1)
@@ -91,7 +151,7 @@
             // 500ms待機する
             wait(0.5);
         }
-
+        
 switch (mode)
         {
             // STATE_A : 前進（左：正転　右：正転）
@@ -101,7 +161,7 @@
                 mypwm4.pulsewidth_ms(70);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(75);
+                mypwm2.pulsewidth_ms(70);
                 mypwm3.pulsewidth_ms(0);
                 break;
                 // STATE_A : 前進（左：正転　右：正転）
@@ -119,11 +179,11 @@
             case STATE_B:
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(50);
+                mypwm4.pulsewidth_ms(100);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
                 mypwm2.pulsewidth_ms(0);
-                mypwm3.pulsewidth_ms(50);
+                mypwm3.pulsewidth_ms(100);
                 break;
                 
             // STATE_C : 左旋回（左：逆転　右：正転）
@@ -131,9 +191,9 @@
                 myled = 1; // LED点灯
                 // 左モータの制御
                 mypwm4.pulsewidth_ms(0);
-                mypwm6.pulsewidth_ms(50);
+                mypwm6.pulsewidth_ms(100);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(50);
+                mypwm2.pulsewidth_ms(100);
                 mypwm3.pulsewidth_ms(0);
                 break;
                 
@@ -162,3 +222,4 @@
     }
     
 }
+}