Diff: servo_and_movefunc.cpp

Revision:

50:36741e8ab197

Parent:

43:2ed84f3558c1

--- a/servo_and_movefunc.cpp	Mon Mar 11 06:36:56 2019 +0000
+++ b/servo_and_movefunc.cpp	Mon Mar 11 10:38:07 2019 +0000
@@ -6,6 +6,7 @@
 #include "KondoServo.h"
 #include "pinnames.h"
 #include "can.h"
+#include "hcsr04.h"
 #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
 #endif
 FILE *fp;
@@ -33,6 +34,7 @@
     KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
 };
 DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
+HCSR04 usensor(p25, p24); // Trigger(DO), Echo(PWMIN)
 #endif
 //一サイクル分進む.return 1:異常終了
 int MoveOneCycle(Walk walkway, OneLeg leg[4])
@@ -114,3 +116,8 @@
     fprintf(fp, "time[s],x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[3]\r\n");
     return 0;
 }
+unsigned int GetDist_cm()
+{
+    usensor.start();
+    return usensor.get_dist_cm();
+}
\ No newline at end of file