Diff: sensors/sensors.cpp

Revision:

22:0ed9de464f40

Parent:

21:14133581387b

Child:

25:c740e6fd5dab

diff -r 14133581387b -r 0ed9de464f40 sensors/sensors.cpp
--- a/sensors/sensors.cpp	Fri May 03 02:21:47 2019 +0000
+++ b/sensors/sensors.cpp	Fri May 03 09:34:05 2019 +0000
@@ -1,29 +1,92 @@
 #include "sensors.h"
+#include "microinfinity.h"
+#include "pinnames.h"
+const float kOldWeight = 0.5;
+const float kOldWeightLight = 0.3; //filterの重み.軽いver
+
+const float kOutVal = 500; //<0.1が返ってきたときに返す値
 //filter [0]:forward, [1]: back
-const float kOldWeight = 0.5;
-const float kOutVal = 500; //<0.1が返ってきたときに返す値
 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
 
-HCSR04 sensor_forward(p11,p12);
-HCSR04 sensor_back(p17, p18);
+HCSR04 sensor_forward(pin_hcsr04[0][0],pin_hcsr04[0][1]);
+HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
+
+const int kResolution=500;
+Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
+Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
+
+
+DigitalIn bus_in(pin_bus);
+DigitalIn hand(pin_hand,PullUp);
+DigitalIn switch_lo(pin_switch_lo,PullUp);
+DigitalIn switch_li(pin_switch_li,PullUp);
+DigitalIn mode4(pin_mode4,PullUp);
+DigitalOut led4(LED4);
+
+int hand_mode=NORMAL;
 
+CAN can1(pin_can_rd,pin_can_td);
+
+Ticker ticker;
+const float kTicker_s = 0.2;
+void Hcsr04Start();
+float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
+
+void TickerSetUp()
+{
+    ticker.attach(Hcsr04Start,kTicker_s);
+}
+
+void Hcsr04Start()
+{
+    GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
+    GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
+}
 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
 {
+    wait_ms(60);
     sensor.start();
+    wait_ms(30);
     float raw_data = sensor.get_dist_cm();
-    if(raw_data < 0.1)//0.1以下なら前回の値を返す
+    if(raw_data < 20)//0.1以下なら前回の値を返す
         return kOutVal;
     filter.SetData(raw_data);
     float dist = filter.GetData();
-//    DEBUG("raw %.3f, filter %.3f\r\n", raw_data,dist);
+//    printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
     return dist;
 }
 
-double get_dist_forward()
+float get_dist_forward()
 {
     return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
 }
-double get_dist_back()
+float get_dist_back()
 {
     return GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
 }
+
+void set_gyro()
+{
+    device.baud(115200);
+    device.format(8,Serial::None,1);
+    device.attach(dev_rx, Serial::RxIrq);
+    wait(0.05);
+    theta0=degree0;
+    check_gyro();
+}
+void wait_gerege()
+{
+    int i = 0;
+    while(i!=100) {
+        if(hand.read()==0)i++;
+    }
+}
+void can_send(int mode, float duty)
+{
+    char data[2]= {0};
+    int send=mode * 10 + (int)(duty * 10.0);
+    data[0]=send & 0b11111111;
+    data[1]=hand_mode;
+    if(can1.write(CANMessage(0,data,2)))led4=1;
+    else led4=0;
+}