File content as of revision 22:0ed9de464f40:

#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
LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};

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 < 20)//0.1以下なら前回の値を返す
        return kOutVal;
    filter.SetData(raw_data);
    float dist = filter.GetData();
//    printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
    return dist;
}

float get_dist_forward()
{
    return GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
}
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;
}