Diff: main.cpp

Revision:

4:04ca6a211b85

Parent:

3:dd3b7c3c327b

Child:

5:ab74616c24b4

--- a/main.cpp	Thu Dec 07 00:37:17 2017 +0000
+++ b/main.cpp	Tue Dec 12 00:34:36 2017 +0000
@@ -12,8 +12,9 @@
 
 DigitalOut myled(LED1);
 
-HCSR04  usensor(D9,D10); // Trigger(DO), Echo(PWMIN)
-unsigned int dist;
+HCSR04  usensor1(D9,D10); // Trigger(DO), Echo(PWMIN)
+HCSR04  usensor2(D11,D12); // Trigger(DO), Echo(PWMIN)
+unsigned int dist1,dist2;
 
 
 #define STATE_A (0)
@@ -22,11 +23,14 @@
 #define STATE_D (3)
 #define STATE_E (4)
 #define STATE_A2 (5)
+#define STATE_A3 (6)
+
 
 char mode,fase;
 int wait_flag;
 int timer;
 
+
 void tflg(){
         timer = t.read(); 
         }
@@ -42,21 +46,26 @@
 mode = STATE_E;
 wait_flag = 0;
 
+fase = 0;
+
 mypwm4.period_ms(100);
 mypwm6.period_ms(100);
 mypwm2.period_ms(100);
 mypwm3.period_ms(100);
     
-timerflag.attach(&tflg, 0.1);
+timerflag.attach(&tflg, 1.0);
 /*flag.attach(&flg, 1.0);*/
 
       
 while(1){
     
-usensor.start();
+usensor1.start();
+usensor2.start();
         wait(0.2); 
-        dist=usensor.get_dist_cm();
-        /*printf("cm: %ld\n",dist); */
+        dist1=usensor1.get_dist_cm(); 
+        dist2=usensor2.get_dist_cm();
+        /*printf("dist1: %ld\n",dist1);  */     
+        printf("dist2: %ld\n",dist2); 
 
 
   /*  switch(timer)
@@ -95,26 +104,29 @@
 switch (fase)
         {
             case 0:
-                if(timer == 5){   
-                    mode = STATE_A;
+            /*printf("timer: %d\n", timer);*/
+                if(timer > 5){ 
+                    /*if(dist2 > 8){
+                        mode = STATE_A3; //hidariに
+                        }else if(dist2 < 10){
+                            mode = STATE_A2;  //migiに
+                            }else{mode = STATE_A;}
+                }*/if(dist1 > 45 && dist1<250){
+                    fase = 1;
                     t.stop();
                     t.reset();
-                    }
-                if(dist > 45){
-                    fase = 1;
                     t.start();
                 }
-                break;
-            
+                break;   
             case 1:
                 if(timer == 0){
                     wait_flag = 1;
                     mode = STATE_B;
-                    }else if(timer == 1.0){
+                    }else if(timer > 2 && dist1 > 60){
                     wait_flag = 1;
                     mode = STATE_A;
-                    t.stop();
                     t.reset();
+                     t.stop();
                     fase = 2;
                     t.start();
                     }
@@ -131,9 +143,10 @@
                             mode = STATE_D;
                             }else if(timer == 36.0){
                                 wait_flag = 1;
-                                mode = STATE_A;}else if(timer == 46.0){
+                                mode = STATE_A;
+                                }else if(timer == 46.0){
                                     wait_flag = 1;
-                                    mode = STATE_D;
+                                    mode = STATE_D;}
         }
 
 // 状態が切り替わるときは一時停止
@@ -164,14 +177,24 @@
                 mypwm2.pulsewidth_ms(70);
                 mypwm3.pulsewidth_ms(0);
                 break;
-                // STATE_A : 前進（左：正転　右：正転）
+                // STATE_A2 : 右に曲がる（左：正転　右：正転）
             case STATE_A2:
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(98);
+                mypwm4.pulsewidth_ms(50);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(100);
+                mypwm2.pulsewidth_ms(80);
+                mypwm3.pulsewidth_ms(0);
+                break;
+                // STATE_A3 : hidariに曲がる（左：正転　右：正転）
+            case STATE_A3:
+                myled = 1; // LED点灯
+                // 左モータの制御
+                mypwm4.pulsewidth_ms(80);
+                mypwm6.pulsewidth_ms(0);
+                // 右モータの制御
+                mypwm2.pulsewidth_ms(10);
                 mypwm3.pulsewidth_ms(0);
                 break;
                 
@@ -220,6 +243,4 @@
                 break;
         }
     }
-    
-}
-}
+    }