Diff: main.cpp

Revision:

0:3a6873f917b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 13 05:38:57 2017 +0000
@@ -0,0 +1,90 @@
+#include "mbed.h"
+
+DigitalOut myled(LED3); // 確認用LED
+
+AnalogIn photoref_L(A1); // フォトリフレクタ左
+AnalogIn photoref_M(A2); // フォトリフレクタ中央
+AnalogIn photoref_R(A3); // フォトリフレクタ右
+
+PwmOut mypwm4(D4);
+PwmOut mypwm5(D6);
+PwmOut mypwm2(D2);
+PwmOut mypwm3(D3);
+
+#define STATE_A (0)
+#define STATE_B (1)
+#define STATE_C (2)
+#define STATE_D (3)
+#define STATE_E (4)
+
+void Motor(char);
+
+int main() {
+  char mode = STATE_A;
+  while(1) {
+    if(photoref_M > 0.5) { // ライン中央：白
+      myled = 1;
+      mode = STATE_E;
+      Motor(mode);
+    }
+    else {                 // ライン中央：黒
+      myled = 0;
+      mode = STATE_A;
+      Motor(mode);
+    }
+  }
+}
+
+void Motor(char mode) {
+  switch(mode) {
+    // STATE_A : 前進（左：正転　右：正転）
+    case STATE_A:
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(8);
+      mypwm5.pulsewidth_ms(0);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(8);
+      mypwm3.pulsewidth_ms(0);
+      break;
+
+      // STATE_B : 右旋回（左：正転　右：逆転）
+      case STATE_B:
+         // 左モータの制御
+         mypwm4.pulsewidth_ms(5);
+         mypwm5.pulsewidth_ms(0);
+         // 右モータの制御
+         mypwm2.pulsewidth_ms(0);
+         mypwm3.pulsewidth_ms(5);
+         break;
+
+      // STATE_C : 左旋回（左：逆転　右：正転）
+     case STATE_C:
+       // 左モータの制御
+       mypwm4.pulsewidth_ms(0);
+       mypwm5.pulsewidth_ms(5);
+       // 右モータの制御
+       mypwm2.pulsewidth_ms(5);
+       mypwm3.pulsewidth_ms(0);
+       break;
+
+     // STATE_D : 後退（左：逆転　右：逆転）
+     case STATE_D:
+       // 左モータの制御
+       mypwm4.pulsewidth_ms(0);
+       mypwm5.pulsewidth_ms(5);
+       // 右モータの制御
+       mypwm2.pulsewidth_ms(0);
+       mypwm3.pulsewidth_ms(5);
+       break;
+
+     // STATE_E : ブレーキ（左：ブレーキ　右：ブレーキ）
+     case STATE_E:
+       // 左モータの制御
+       mypwm4.pulsewidth_ms(5);
+       mypwm5.pulsewidth_ms(5);
+       // 右モータの制御
+       mypwm2.pulsewidth_ms(5);
+       mypwm3.pulsewidth_ms(5);
+       break;
+  }
+}
\ No newline at end of file