File content as of revision 47:b77796718a9b:

#include "sensors.h"
#include "microinfinity.h"
#include "pinnames.h"
#include "moves.h"
int RightOrLeft = 0;

//lowpass関係
const float kOldWeight = 0;
const float kOldWeightLight = 0.3; //filterの重み.軽いver
const float kOutVal = 5000; //<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);
const float kDeg2Rad = M_pi/180.0;

//ec
const double ecsliprate=0.8647f;
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 switch_LR(pin_switch_LR,PullUp);
DigitalIn switch_modes[3]= {
    DigitalIn(pin_switch_modes[0],PullUp),
    DigitalIn(pin_switch_modes[1],PullUp),
    DigitalIn(pin_switch_modes[2],PullUp),
};
//InterruptIn mode4(pin_mode4,PullUp);
DigitalOut led4(LED4);
int hand_mode=G_CLOSE;

//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;
void FileCloseFiltered();

int FileOpen() //1:異常終了
{
    printf("file open\r\n");
    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();
    if(RightOrLeft) {
        //rightなら
        switch_LR.fall(FileCloseFiltered);//left(0)にしたらclose
    } else {
        //left
        switch_LR.rise(FileCloseFiltered);//left(0)にしたらclose
    }
    return 0;
}

void FileWrite()
{
    FileWrite("");
}

void FileWrite(const char *str)
{
    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,%s\r\n",
                filetimer.read(), degree0,
                hcsr04_val[0], hcsr04_val[1],
                motor_lo.getPosi(), motor_li.getPosi(),
                str);
    }
}
void FileClose()
{
    filetimer.stop();
    if(fp != NULL)
        fclose(fp);
};
void FileCloseFiltered()
{
    int is_ok = 0;
    wait(0.1);
    if(RightOrLeft) {    //rightならfallで取る
        if(switch_LR == 0)
            is_ok = 1;
    } else {//left
        if(switch_LR == 1)
            is_ok = 1;
    }

    if(is_ok == 1) {
        FileClose();
    }
}



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(60);//60
    sensor.start();
    wait_ms(30);
    float raw_data = sensor.get_dist_cm();
    if(raw_data < 20)//0.1以下なら前回の値を返す
        return kOutVal;
    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, int id)
{
    char data[2]= {0};
    int send=mode * 10 + (int)(duty * 10.0);
    data[0]=send & 0b11111111;
    data[1]=hand_mode;
    if(can1.write(CANMessage(id,data,2)))led4=1;
    else led4=0;
}

float Hcsr04BackWithEc()
{

    hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
    int is_ok = checkUW(hcsr04_val[1], degree0, ec_lo.getCount());//ECは内側を選んだ.適当
    if(is_ok == 0) {
        //もう一度とる
        sensor_back.start();
        wait_ms(50);
        hcsr04_val[1] = sensor_back.get_dist_cm();
//        FileWrite();
    }
    return hcsr04_val[1];
}
int checkUW(double uwdist,double deg,int eccount, int is_getlog)     //uwdist[cm],theta[deg],eccount[]
{
    const double oneloopdist=0.22f;//[m]
    const double errrange=0.5;//収束判定[m]
    //const double D=10;//機体中心から足まで距離[m]
    static double X=0,Y=0;  //初期位置を代入
    static double pre_theta=M_pi*0.5f,pre_eccount=0;

    double theta = deg * kDeg2Rad;
    theta+=M_pi*0.5f;
    double d_eccount=eccount-pre_eccount;
    double d_theta=theta-pre_theta;
    double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
    double rho=ecdist/d_theta;
    double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
    //double dy= rho*(cos(pre_theta)-cos(theta));//[m]
    double dy= rho*(cos(pre_theta)-cos(theta));//[m]
    X+=dx;//[m]
    Y+=dy;//[m]
    pre_theta=theta;
    pre_eccount=eccount;
//    printf("{X,Y}={%f,%f},uw=%f,",X,Y,uwdist);
//    fprintf(result,"%f,%f,%f,",X,Y,uwdist);
    int ret = 0;
    if(fabs(Y-uwdist*0.01f)<errrange)ret = 1;//Y-uwdist*0.01f*sin(theta)
    else ret = 0;
    if(is_getlog == 1) {
        char str[255];
        sprintf(str,"%f,%f,%d,%f,%f,%d",uwdist,deg,eccount,X,Y, ret);
        FileWrite(str);
    }
    return ret;
}