Takekazu Kato / Mbed 2 deprecated SC2018-3

Dependencies:   mbed

main.cpp

Committer:
TakekazuKATO
Date:
2018-08-24
Revision:
0:bdb7452d5dd9

File content as of revision 0:bdb7452d5dd9:

#include <mbed.h>
#include "utils.h"
Timer timer;
int fall_time[3] = {0, 0, 0};
int pulse_width[3] = {0, 0, 0};
int irpower[3] = {0, 0, 0};
float alpha = 0.03;
int hyst = 10;
//信号の立ち下がりの処理
void irfall(int n) {
  int t = timer.read_us();
  if (t - fall_time[n] > 1000) {
    //1000us以上パルスを感知していない場合は0
    irpower[n] = 0;
  } else if (t - fall_time[n] > 800) {
    //正常ならパルスの間隔は833usになる.間隔が800us以下の場合は無視する.

    //ノイズによる時間のゆらぎを平滑化
    irpower[n] = (1.0 - alpha) * irpower[n] + alpha * pulse_width[n];
  }
  //立ち下がり時刻を記録
  fall_time[n] = t;
}
//信号の立ち上がりの処理
void irrise(int n) {
  //立ち下がりパルスの幅を記録
  pulse_width[n] = timer.read_us() - fall_time[n];
}
void irright_fall() { irfall(0); }
void irright_rise() { irrise(0); }
void ircenter_fall() { irfall(1); }
void ircenter_rise() { irrise(1); }
void irleft_fall() { irfall(2); }
void irleft_rise() { irrise(2); }

int main() {
  // A0, A1, A2ピンをパルス検出に使用
  InterruptIn irright(A0);
  InterruptIn ircenter(A1);
  InterruptIn irleft(A2);

  timer.start();
  irright.fall(irright_fall);
  irright.rise(irright_rise);
  ircenter.fall(ircenter_fall);
  ircenter.rise(ircenter_rise);
  irleft.fall(irleft_fall);
  irleft.rise(irleft_rise);

  int hyst = 10;
  //左右のモーターの速さを設定
  left_speed = 0.4;
  right_speed = 0.4;
  while (1) {
    printf("%d,%d,%d\n", irpower[0], irpower[1], irpower[2]);
    if (irpower[1] > irpower[0] + hyst && irpower[1] > irpower[2] + hyst) {
      //右のセンサが一番強く反応する場合
      turn_right();
    } else if (irpower[2] > irpower[1] + hyst &&
               irpower[2] > irpower[0] + hyst) {
      //左のセンサが一番強く反応する場合
      turn_left();
    } else if (irpower[0] > irpower[1] + hyst &&
               irpower[0] > irpower[2] + hyst) {
      //正面のセンサが一番強く反応する場合
      go_straight();
    }
  }
}