Diff: main.cpp

Revision:

2:7b84f6d2cf63

Parent:

1:3af3d6834515

--- a/main.cpp	Mon Nov 13 07:19:51 2017 +0000
+++ b/main.cpp	Mon Dec 04 05:57:40 2017 +0000
@@ -1,7 +1,5 @@
 #include "mbed.h"
 
-DigitalOut myled(LED3); // 確認用LED
-
 AnalogIn photoref_L(A1); // フォトリフレクタ左
 AnalogIn photoref_M(A2); // フォトリフレクタ中央
 AnalogIn photoref_R(A3); // フォトリフレクタ右
@@ -20,27 +18,26 @@
 #define STATE_G (6) // ブレーキ
 
 void Motor(char);
+void LineTrace(void);
 
 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_F; // 右前進
-      Motor(mode);
-    }
+    LineTrace();
   }
 }
 
+void LineTrace(void) {   // ラインの右側でライントレース
+  if(photoref_M > 0.5) { // センサ中央：白
+    Motor(STATE_E);      // 左前進
+  } else {               // センサ中央：黒
+    Motor(STATE_F);      // 右前進
+  }
+  wait_ms(50);
+}
+
 void Motor(char mode) {
   switch(mode) {
-    // STATE_A : 前進（左：正転　右：正転）
-    case STATE_A:
+    case STATE_A: // 前進
       // 左モータの制御
       mypwm4.pulsewidth_ms(8);
       mypwm5.pulsewidth_ms(0);
@@ -48,65 +45,53 @@
       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(0);
-       break;
-       
-     // STATE_F : 右前進（左：正転　右：ブレーキ）
-     case STATE_F:
-       // 左モータの制御
-       mypwm4.pulsewidth_ms(5);
-       mypwm5.pulsewidth_ms(0);
-       // 右モータの制御
-       mypwm2.pulsewidth_ms(5);
-       mypwm3.pulsewidth_ms(5);
-       break;
-
-     // STATE_G : ブレーキ（左：ブレーキ　右：ブレーキ）
-     case STATE_G:
-       // 左モータの制御
-       mypwm4.pulsewidth_ms(5);
-       mypwm5.pulsewidth_ms(5);
-       // 右モータの制御
-       mypwm2.pulsewidth_ms(5);
-       mypwm3.pulsewidth_ms(5);
-       break;
+    case STATE_B: // 右旋回
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(5);
+      mypwm5.pulsewidth_ms(0);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(0);
+      mypwm3.pulsewidth_ms(5);
+      break;
+    case STATE_C: // 左旋回
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(0);
+      mypwm5.pulsewidth_ms(5);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(5);
+      mypwm3.pulsewidth_ms(0);
+      break;
+    case STATE_D: // 後退
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(0);
+      mypwm5.pulsewidth_ms(5);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(0);
+      mypwm3.pulsewidth_ms(5);
+      break;
+    case STATE_E: // 左前進
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(5);
+      mypwm5.pulsewidth_ms(5);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(5);
+      mypwm3.pulsewidth_ms(0);
+      break;
+    case STATE_F: // 右前進
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(5);
+      mypwm5.pulsewidth_ms(0);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(5);
+      mypwm3.pulsewidth_ms(5);
+      break;
+    case STATE_G: // ブレーキ
+      // 左モータの制御
+      mypwm4.pulsewidth_ms(5);
+      mypwm5.pulsewidth_ms(5);
+      // 右モータの制御
+      mypwm2.pulsewidth_ms(5);
+      mypwm3.pulsewidth_ms(5);
+      break;
   }
 }
\ No newline at end of file