main.cpp

00001 #include <mbed.h>
00002 #include "utils.h"
00003 Timer timer;
00004 int fall_time[3] = {0, 0, 0};
00005 int pulse_width[3] = {0, 0, 0};
00006 int irpower[3] = {0, 0, 0};
00007 float alpha = 0.03;
00008 int hyst = 10;
00009 //信号の立ち下がりの処理
00010 void irfall(int n) {
00011   int t = timer.read_us();
00012   if (t - fall_time[n] > 1000) {
00013     //1000us以上パルスを感知していない場合は０
00014     irpower[n] = 0;
00015   } else if (t - fall_time[n] > 800) {
00016     //正常ならパルスの間隔は833usになる．間隔が800us以下の場合は無視する．
00017 
00018     //ノイズによる時間のゆらぎを平滑化
00019     irpower[n] = (1.0 - alpha) * irpower[n] + alpha * pulse_width[n];
00020   }
00021   //立ち下がり時刻を記録
00022   fall_time[n] = t;
00023 }
00024 //信号の立ち上がりの処理
00025 void irrise(int n) {
00026   //立ち下がりパルスの幅を記録
00027   pulse_width[n] = timer.read_us() - fall_time[n];
00028 }
00029 void irright_fall() { irfall(0); }
00030 void irright_rise() { irrise(0); }
00031 void ircenter_fall() { irfall(1); }
00032 void ircenter_rise() { irrise(1); }
00033 void irleft_fall() { irfall(2); }
00034 void irleft_rise() { irrise(2); }
00035 
00036 int main() {
00037   // A0, A1, A2ピンをパルス検出に使用
00038   InterruptIn irright(A0);
00039   InterruptIn ircenter(A1);
00040   InterruptIn irleft(A2);
00041 
00042   timer.start();
00043   irright.fall(irright_fall);
00044   irright.rise(irright_rise);
00045   ircenter.fall(ircenter_fall);
00046   ircenter.rise(ircenter_rise);
00047   irleft.fall(irleft_fall);
00048   irleft.rise(irleft_rise);
00049 
00050   int hyst = 10;
00051   //左右のモーターの速さを設定
00052   left_speed = 0.4;
00053   right_speed = 0.4;
00054   while (1) {
00055     printf("%d,%d,%d\n", irpower[0], irpower[1], irpower[2]);
00056     if (irpower[1] > irpower[0] + hyst && irpower[1] > irpower[2] + hyst) {
00057       //右のセンサが一番強く反応する場合
00058       turn_right();
00059     } else if (irpower[2] > irpower[1] + hyst &&
00060                irpower[2] > irpower[0] + hyst) {
00061       //左のセンサが一番強く反応する場合
00062       turn_left();
00063     } else if (irpower[0] > irpower[1] + hyst &&
00064                irpower[0] > irpower[2] + hyst) {
00065       //正面のセンサが一番強く反応する場合
00066       go_straight();
00067     }
00068   }
00069 }