Diff: main.cpp

Revision:

4:75090ff74dd0

Parent:

3:f70ecf1e21d5

Child:

5:9b5899b06e2e

diff -r f70ecf1e21d5 -r 75090ff74dd0 main.cpp
--- a/main.cpp	Sat Nov 23 10:06:03 2019 +0000
+++ b/main.cpp	Thu Nov 28 08:59:56 2019 +0000
@@ -4,7 +4,8 @@
 #include "TB6612.h"
 DigitalOut myled(LED1);
 US015 hs(p12,p11);
-DigitalOut thermo(p20);
+DigitalIn thermo(p20);
+DigitalOut Sb612switch(p15);//焦電スイッチ
 DigitalOut Ultra(p12);
 Serial pc(USBTX,USBRX); // tx, rx
 JPEGCamera camera(p9, p10); // TX, RX
@@ -18,30 +19,30 @@
   pc.printf("start\r\n");
   left = 100; //左モーター１００％
   right = 100;//右モーター１００％
-  bool detected=false;
-    thermo=0; //焦電off
+ 
+    Sb612switch=0; //焦電off
     Ultra=1;//超音波on
     
     while(1) {
-         printf("超音波on 焦電off" )  ;
+         printf("超音波on\r\n 焦電off\r\n" )  ;
          hs.TrigerOut();
          wait(1);
          int distance;
          distance = hs.GetDistance();
-         printf("%d\r\n",distance);//距離出力
+         printf("distance=%d\r\n",distance);//距離出力
         
-        if(distance<2000){//超音波反応
+        if(distance<=2000){//超音波反応
          left = 0; //左モーター０％
          right = 0;//右モーター０％
          printf("停止\n\r");
-         wait(1.5);
+         wait(5.0);
          
          Ultra=0;//超音波off
-         thermo=1;//焦電on
-          printf("超音波off焦電on  ");
-          if(true)
+         Sb612switch=1; //焦電on
+          printf("超音波off\r\n焦電on\r\n  ");
+           bool detected=false;
              th = thermo;
-             if(th=1 && !detected) {//焦電反応ありの場合
+             if(th==1 && !detected) {//焦電反応ありの場合
                i++; 
                detected=true;
                pc.printf("human\r\n");
@@ -73,17 +74,23 @@
                printf("time = %f\n", timer.read());
                
               }else{//焦電反応なしの場合
+              printf("not found!\r\n");
+               
+               left = 70; //左モーター１０％
+               right = 100;//右モーター１００％(右折)
+               Sb612switch=0;
+               Ultra=1;
                detected=false;
-               left = 20; //左モーター１０％
-               right = 100;//右モーター１００％(右折)
                printf("右折\n\r");
                wait(3.0);
                left = 100; //左モーター１００％
                right = 100;//右モーター１００％
               }           
        }else{//超音波distance>2000
+          printf("safety zone\r\n");
           left = 100; //左モーター100％
-          right = 20;//右モーター20％(右折)
+          right = 70;//右モーター70％(左折)
+          printf("左折\r\n");
          }
 
     }
@@ -93,4 +100,4 @@
 
  
   }
- 
+ 
\ No newline at end of file