Diff: main.cpp

Revision:

0:bdb7452d5dd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 24 13:04:00 2018 +0000
@@ -0,0 +1,69 @@
+#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以上パルスを感知していない場合は０
+    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();
+    }
+  }
+}
\ No newline at end of file