sensors.cpp

00001 #include "sensors.h"
00002 #include "microinfinity.h"
00003 #include "pinnames.h"
00004 #include "moves.h"
00005 int RightOrLeft = 0;
00006 
00007 //lowpass関係
00008 const float kOldWeight = 0;
00009 const float kOldWeightLight = 0.3; //filterの重み.軽いver
00010 const float kOutVal = 500; //<0.1が返ってきたときに返す値
00011 //filter [0]:forward, [1]: back
00012 LowPassFilter lowpassfilter[2] = {LowPassFilter(kOldWeight),LowPassFilter(kOldWeight),};
00013 
00014 //超音波
00015 HCSR04 sensor_forward(pin_hcsr04[0][0],pin_hcsr04[0][1]);
00016 HCSR04 sensor_back(pin_hcsr04[1][0], pin_hcsr04[1][1]);
00017 float hcsr04_val[2]  = {};
00018 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter);
00019 const float kDeg2Rad = M_pi/180.0;
00020 int checkUW(double uwdist,double theta,int eccount);
00021 
00022 //ec
00023 const int kResolution=500;
00024 Ec ec_lo(pin_ec[0][0],pin_ec[0][1],NC,kResolution,0.01);
00025 Ec ec_li(pin_ec[1][0],pin_ec[1][1],NC,kResolution,0.01);
00026 
00027 //スイッチ
00028 DigitalIn bus_in(pin_bus);
00029 DigitalIn hand(pin_hand,PullUp);
00030 DigitalIn switch_lo(pin_switch_lo,PullUp);
00031 DigitalIn switch_li(pin_switch_li,PullUp);
00032 InterruptIn switch_LR(pin_switch_LR,PullUp);
00033 DigitalIn switch_modes[3]= {
00034     DigitalIn(pin_switch_modes[0],PullUp),
00035     DigitalIn(pin_switch_modes[1],PullUp),
00036     DigitalIn(pin_switch_modes[2],PullUp),
00037 };
00038 //InterruptIn mode4(pin_mode4,PullUp);
00039 DigitalOut led4(LED4);
00040 int hand_mode=G_CLOSE;
00041 
00042 //can
00043 CAN can1(pin_can_rd,pin_can_td);
00044 
00045 //ticker
00046 Ticker ticker;
00047 //const float kTicker_s = 0.2;
00048 void Hcsr04Start();//超音波をtickerでとる用
00049 
00050 
00051 //file操作
00052 FILE *fp = NULL;
00053 LocalFileSystem local("local");
00054 const char kFileName[] = "/local/sensors.csv";
00055 Timer filetimer;
00056 
00057 int FileOpen() //1:異常終了
00058 {
00059     printf("file open\r\n");
00060     if ((fp = fopen(kFileName, "w")) == NULL) {
00061         printf("error : FileSave()\r\n");
00062         return 1;
00063     }
00064     fprintf(fp, "time[s], gyro[deg], hcsr04_forward[cm], hcsr04_back[cm], motor_lo[deg], motor_li[deg]\r\n");
00065     filetimer.reset();
00066     filetimer.start();
00067     if(RightOrLeft) {
00068         //rightなら
00069         switch_LR.fall(FileClose);//left(0)にしたらclose
00070     } else {
00071         //left
00072         switch_LR.rise(FileClose);//left(0)にしたらclose
00073     }
00074     return 0;
00075 }
00076 
00077 void FileWrite()
00078 {
00079     static int is_first = 0;//初回はtimerを0にする
00080     if(is_first ==0) {
00081         filetimer.reset();
00082         ++is_first;
00083     }
00084     if(fp == NULL) {
00085         is_first = 0;
00086     } else {
00087         fprintf(fp, "%f, %f, %f, %f, %f, %f\r\n",
00088                 filetimer.read(), degree0,
00089                 hcsr04_val[0], hcsr04_val[1],
00090                 motor_lo.getPosi(), motor_li.getPosi());
00091     }
00092 }
00093 void FileClose()
00094 {
00095     filetimer.stop();
00096     if(fp != NULL)
00097         fclose(fp);
00098 };
00099 
00100 
00101 
00102 void TickerSetUp()
00103 {
00104 //    ticker.attach(FileWriteTicker,kTicker_s);
00105 }
00106 
00107 void Hcsr04Start()
00108 {
00109     GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
00110     GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
00111 }
00112 float GetFilteredDist_cm(HCSR04 & sensor, LowPassFilter & filter)
00113 {
00114     wait_ms(90);//60
00115     sensor.start();
00116     wait_ms(30);
00117     float raw_data = sensor.get_dist_cm();
00118     if(raw_data < 20)//0.1以下なら前回の値を返す
00119         return kOutVal;
00120     /*    filter.SetData(raw_data);
00121         float dist = filter.GetData();
00122     //    printf("raw %.3f, filter %.3f\r\n", raw_data,dist);
00123         return dist;
00124         */
00125     return raw_data;
00126 }
00127 
00128 float get_dist_forward()
00129 {
00130     hcsr04_val[0] = GetFilteredDist_cm(sensor_forward,lowpassfilter[0]);
00131     return hcsr04_val[0];
00132 }
00133 float get_dist_back()
00134 {
00135     hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
00136     return hcsr04_val[1];
00137 }
00138 
00139 void set_gyro()
00140 {
00141     device.baud(115200);
00142     device.format(8,Serial::None,1);
00143     device.attach(dev_rx, Serial::RxIrq);
00144     wait(0.05);
00145     theta0=degree0;
00146     check_gyro();
00147 }
00148 void wait_gerege()
00149 {
00150     int i = 0;
00151     while(i!=100) {
00152         if(hand.read()==0)i++;
00153     }
00154 }
00155 void can_send(int mode, float duty)
00156 {
00157     char data[2]= {0};
00158     int send=mode * 10 + (int)(duty * 10.0);
00159     data[0]=send & 0b11111111;
00160     data[1]=hand_mode;
00161     if(can1.write(CANMessage(0,data,2)))led4=1;
00162     else led4=0;
00163 }
00164 
00165 float Hcsr04BackWithEc()
00166 {
00167 
00168     hcsr04_val[1] = GetFilteredDist_cm(sensor_back,lowpassfilter[1]);
00169     FileWrite();
00170     int is_ok = checkUW(hcsr04_val[1], degree0*kDeg2Rad, ec_li.getCount());//ECは内側を選んだ.適当
00171     if(is_ok != 1) {
00172         //もう一度とる
00173         sensor_back.start();
00174         wait_ms(50);
00175         hcsr04_val[1] = sensor_back.get_dist_cm();
00176         FileWrite();
00177     }
00178     return hcsr04_val[1];
00179 }
00180 int checkUW(double uwdist,double theta,int eccount)     //uwdist[cm],theta[rad],eccount[]
00181 {
00182     const double oneloopdist=0.22f;//[m]
00183     const double ecsliprate=0.7f;
00184     const double errrange=0.5;//収束判定[m]
00185     //const double D=10;//機体中心から足まで距離[m]
00186     static double X=0,Y=0;  //初期位置を代入
00187     static double pre_theta=M_pi*0.5f,pre_eccount=0;
00188     theta+=M_pi*0.5f;
00189     double d_eccount=eccount-pre_eccount;
00190     double d_theta=theta-pre_theta;
00191     double ecdist=oneloopdist*d_eccount/1000*ecsliprate;
00192     double rho=ecdist/d_theta;
00193     double dx=-rho*(sin(pre_theta)-sin(theta));//[m]
00194     double dy= rho*(cos(pre_theta)-cos(theta));//[m]
00195     X+=dx;//[m]
00196     Y+=dy;//[m]
00197     pre_theta=theta;
00198     pre_eccount=eccount;
00199     if(fabs(Y-uwdist*0.01f)<errrange)return 1;//Y-uwdist*0.01f*sin(theta)
00200     else return 0;
00201 }