File content as of revision 26:5fb1aa9cb7f0:

#include "sensors.h"
#include "microinfinity.h"
#include "pinnames.h"
#include "moves.h"
//lowpass関係
const float kOldWeight = 0;
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]);
float hcsr04_val[2]  = {};
float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);

//ec
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);
InterruptIn mode4(pin_mode4,PullUp);
DigitalOut led4(LED4);
int hand_mode=NORMAL;

//can
CAN can1(pin_can_rd,pin_can_td);

//ticker
Ticker ticker;
//const float kTicker_s = 0.2;
void Hcsr04Start();//超音波をtickerでとる用


//file操作
FILE *fp = NULL;
LocalFileSystem local("local");
const char kFileName[] = "/local/sensors.csv";
Timer filetimer;

int FileOpen() //1:異常終了
{
    if ((fp = fopen(kFileName, "w")) == NULL) {
        printf("error : FileSave()\r\n");
        return 1;
    }
    fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
    filetimer.reset();
    filetimer.start();
    mode4.rise(FileClose);//mode4ピンを上げげればfileclose
    return 0;
}

void FileWrite()
{
    static int is_first = 0;//初回はtimerを0にする
    if(is_first ==0) {
        filetimer.reset();
        ++is_first;
    }
    if(fp == NULL) {
        is_first = 0;
    } else {
        fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
                filetimer.read(), degree0,
                hcsr04_val[0], hcsr04_val[1],
                motor_lo.getPosi(), motor_li.getPosi());
    }
}
void FileClose()
{
    filetimer.stop();
    if(fp != NULL)
        fclose(fp);
};


void TickerSetUp()
{
//    ticker.attach(FileWriteTicker,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(30);//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;
        */
    return raw_data;
}

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