Diff: main.cpp

Revision:

6:08b8f6b05c22

Parent:

1:9d2b2b5ec36f

diff -r 9d2b2b5ec36f -r 08b8f6b05c22 main.cpp
--- a/main.cpp	Wed Mar 04 05:31:39 2020 +0000
+++ b/main.cpp	Wed Mar 04 06:06:07 2020 +0000
@@ -4,6 +4,7 @@
 #define RESOLUTION 500
 #include "math.h"
 #include "R1370P.h"
+#include"hcsr04.h"
 
 //PwmOut motor_1F(PA_5);//1Forward Right motor Forward
 //PwmOut motor_1B(PC_7);//Forward Right motor Back
@@ -30,7 +31,11 @@
                        SpeedControl(PA_9,PA_7,50,ec[3])
                       };
 
-//R1370P gyro(PA_11,PA_12);
+HCSR04 echo[]= {HCSR04(PC_0,PC_12),//A
+                HCSR04(PA_15,PB_7),//A
+                HCSR04(PH_1,PB_0),// B
+                HCSR04(PC_3,PB_10),//B
+               };
 
 DigitalIn button(USER_BUTTON);
 Serial pc(USBTX, USBRX); // tx, rx
@@ -115,6 +120,25 @@
         motor[i].Sc((int)omega[i]);
     }
 }
+void echo_()
+{
+    double a=0,b=0,c=0,d=0;
+    for(int i=0; i<4; i++) {
+        echo[i].start();
+    }
+    wait(1);//minimum wait is 30msec
+    a=echo[0].get_dist_cm();
+    b=echo[1].get_dist_cm();
+    c=echo[2].get_dist_cm();
+    d=echo[3].get_dist_cm();
+    printf("a:%f b:%f c:%f d:%f\r\n",a,b,c,d);
+    if(a<5||b<5||c<5||d<5) {
+        for(int j=0; j<4; j++) {
+            motor[j].stop();
+        }
+    }
+}
+
 int main()
 {
     gyro.initialize();    //main関数の最初に一度だけ実行
@@ -137,6 +161,7 @@
     motor[3].setPDparam( 0.004801, 0.026645 );
 
     int n=0,dx,dy,aimX,aimY;
+    double a=0,b=0,c=0,d=0;
     Location location;
     while(1) {
         //自己位置取得