2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

Files at this revision

API Documentation at this revision

Comitter:
kato8
Date:
Tue Dec 19 01:23:44 2017 +0000
Parent:
5:ab74616c24b4
Commit message:
2??

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r ab74616c24b4 -r d264f7f2b781 main.cpp
--- a/main.cpp	Tue Dec 12 00:49:19 2017 +0000
+++ b/main.cpp	Tue Dec 19 01:23:44 2017 +0000
@@ -1,23 +1,29 @@
 #include "mbed.h"
 #include "hcsr04.h"
 
-PwmOut mypwm4(D4);
-PwmOut mypwm6(D6);
-PwmOut mypwm2(D2);
-PwmOut mypwm3(D3);
+AnalogIn photoref_L(A1); // フォトリフレクタ左
+AnalogIn photoref_M(A2); // フォトリフレクタ中央
+AnalogIn photoref_R(A3); // フォトリフレクタ右
 
-Ticker flag;
-Ticker timerflag;
+PwmOut mypwm4(D4); // モータ
+PwmOut mypwm6(D6); // モータ
+PwmOut mypwm2(D2); // モータ
+PwmOut mypwm3(D3); // モータ
+
+HCSR04  usensor1(D9,D10);  // 超音波後;Trigger(DO), Echo(PWMIN)
+HCSR04  usensor2(D11,D12); // 超音波横;Trigger(DO), Echo(PWMIN)
+
 Timer t;
+Ticker timer_flag;
+
 
 DigitalOut myled(LED1);
 
-HCSR04  usensor1(D9,D10); // Trigger(DO), Echo(PWMIN)
-HCSR04  usensor2(D11,D12); // Trigger(DO), Echo(PWMIN)
+
 unsigned int dist1,dist2;
 
 
-#define STATE_A (0)
+#define STATE_A (0) // 前進
 #define STATE_B (1)
 #define STATE_C (2)
 #define STATE_D (3)
@@ -25,15 +31,18 @@
 #define STATE_A2 (5)
 #define STATE_A3 (6)
 
-
-char mode,fase;
+char mode = STATE_E;
+int fase;
 int wait_flag;
 int timer;
+void LineTrace(void);
+float val_L, val_R, val_M;
 
 
-void tflg(){
-        timer = t.read(); 
-        }
+
+void tflg() {
+    timer = t.read(); 
+}
 /*void flg(){
         usensor.start();
         dist=usensor.get_dist_cm();
@@ -41,205 +50,222 @@
         }*/
 
 int main() {
+    t.start();
+    wait(3);
+    wait_flag = 0;
+    fase = 0;
     
-t.start();
-mode = STATE_E;
-wait_flag = 0;
-
-fase = 0;
-
-mypwm4.period_ms(100);
-mypwm6.period_ms(100);
-mypwm2.period_ms(100);
-mypwm3.period_ms(100);
+    timer_flag.attach(&tflg, 1.0); // タイマー割り込み設定
+    
+    while(1) {
+        val_M = photoref_M.read();
+        val_L = photoref_L.read();
+        val_R = photoref_R.read();
+        printf("M: %f\n", val_M);
+        printf("L: %f\n", val_L);
+        printf("R: %f\n", val_R);
     
-timerflag.attach(&tflg, 1.0);
-/*flag.attach(&flg, 1.0);*/
-
-      
-while(1) {
-usensor1.start();
-usensor2.start();
-wait(0.2); 
-dist1=usensor1.get_dist_cm(); 
-dist2=usensor2.get_dist_cm();
-/*printf("dist1: %ld\n",dist1);  */     
-printf("dist2: %ld\n",dist2); 
-
-
-  /*  switch(timer)
-{
-    case 8.0:
-        wait_flag = 1;
-        mode = STATE_A;
-        break;
-    case 12.0:
-        wait_flag = 1;
-        mode = STATE_B;
-        break;
-    case 15.5:
-        wait_flag = 1;
-        mode = STATE_A2;
-        break;
-    case 22.0:
-        wait_flag = 1;
-        mode = STATE_D;
-        break;
-    case 30.0:
-        wait_flag = 1;
-        mode = STATE_A;
-        break;
-    case 37.0:
-        wait_flag = 1;
-        mode = STATE_C;
-        break;
-    case 42.0:
-        wait_flag = 1;
-        mode = STATE_A;
-        break; 
+        if(val_L > 0.5 && val_R > 0.5) { // 左:白,右:白
+          LineTrace(); // 直進
+        } else if(val_L < 0.5 && val_R > 0.5) { // 左:白,右:黒
+          mode = STATE_A2; // 右前進
+        } else if(val_L > 0.5 && val_R < 0.5) { // 左:黒,右:白
+          mode = STATE_A3; // 左前進
+        } else { // 左:黒,右:黒
+          mode = STATE_E; // ブレーキ
+        }
+        wait_ms(100);
+        /*
+        // printf("timer: %d\n", timer);
         
-}*/
-
-switch (fase)
-        {
+        // 超音波取得
+        usensor1.start();
+        usensor2.start();
+        wait_ms(500); 
+        dist1 = usensor1.get_dist_cm(); 
+        dist2 = usensor2.get_dist_cm();
+        printf("dist1: %ld [cm]  ",dist1);
+        printf("dist2: %ld [cm]\n",dist2); 
+                
+        switch(fase) {
             case 0:
-            /*printf("timer: %d\n", timer);*/
-                if(timer > 5){ 
+                mode = STATE_A; // 前進
+                //if(timer > 5) {
                     /*if(dist2 > 8){
                         mode = STATE_A3; //hidariに
                         }else if(dist2 < 10){
                             mode = STATE_A2;  //migiに
                             }else{mode = STATE_A;}
-                }*/if(dist1 > 45 && dist1<250){
+                    }
+                if(dist1 > 41 && dist1 < 250) { // 後ろの壁から45cmの地点
+                    wait_flag = 1;
                     fase = 1;
                     t.stop();
                     t.reset();
                     t.start();
                 }
-                break;   
+                break;
             case 1:
-                if(timer == 0){
-                    wait_flag = 1;
-                    mode = STATE_B;
-                    }else if(timer > 2 && dist1 > 60){
+                mode = STATE_B; // 右旋回
+                if(timer > 2) {
                     wait_flag = 1;
-                    mode = STATE_A;
+                    fase = 2;
                     t.reset();
-                     t.stop();
-                    fase = 2;
+                    t.stop();
                     t.start();
-                    }
+                }
                 break;
             case 2:
-                if(timer == 6.0){
+                mode = STATE_A; // 前進
+                if(timer > 4.0) {
+                    wait_flag = 1;
+                    fase = 3;
+                }
+                break;
+            case 3:
+                mode = STATE_D;
+                if(timer > 13.0) {
+                    wait_flag = 1;
+                    fase = 4;
+                }
+                break;
+            case 4:
+                 mode = STATE_A; // 前進
+                 if(timer > 21.0) {
                     wait_flag = 1;
-                    mode = STATE_D;
-                    }else if(timer == 16.0){
-                        wait_flag = 1;
-                        mode = STATE_A;
-                        }else if(timer == 26.0){
-                            wait_flag = 1;
-                            mode = STATE_D;
-                            }else if(timer == 36.0){
-                                wait_flag = 1;
-                                mode = STATE_A;
-                                }else if(timer == 46.0){
-                                    wait_flag = 1;
-                                    mode = STATE_D;}
+                    fase = 5;
+                }
+                break;
+            case 5:
+                 mode = STATE_D; // 後退
+                 if(timer > 29.0) {
+                    wait_flag = 1;
+                    fase = 6;
+                }
+                break;
+            case 6:
+                mode = STATE_A; // 前進
+                if(timer > 37.0) {
+                    wait_flag = 1;
+                    fase = 7;
+                }
+                break;
+            case 7:
+                mode = STATE_D; // 後退
+                if(timer > 37) {
+                    mode = STATE_E; // 停止
+                }
+                break;
+            default:
+                break;
         }
-
-// 状態が切り替わるときは一時停止
-        if(wait_flag==1)
-        {
+        
+        // 状態が切り替わるときは一時停止
+        if(wait_flag == 1) {
             // フラグの初期化
             wait_flag = 0;
             myled = 0; // LED消灯
             // 左モータの制御
-            mypwm4.pulsewidth_ms(80);
-            mypwm6.pulsewidth_ms(80);
+            mypwm4.pulsewidth_ms(50);
+            mypwm6.pulsewidth_ms(50);
             // 右モータの制御
-            mypwm2.pulsewidth_ms(80);
-            mypwm3.pulsewidth_ms(80);
+            mypwm2.pulsewidth_ms(50);
+            mypwm3.pulsewidth_ms(50);
             // 500ms待機する
+            mode = STATE_E;
             wait(0.5);
         }
-        
-switch (mode)
-        {
+        */
+        switch(mode) {
             // STATE_A : 前進(左:正転 右:正転)
             case STATE_A:
+                printf("STATE_A\n");
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(70);
+                mypwm4.pulsewidth_ms(15);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(70);
+                mypwm2.pulsewidth_ms(15);
                 mypwm3.pulsewidth_ms(0);
                 break;
-                // STATE_A2 : 右に曲がる(左:正転 右:正転)
+            // STATE_A2 : 右に曲がる(左:正転 右:正転)
             case STATE_A2:
+                printf("STATE_A2\n");
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(50);
+                mypwm4.pulsewidth_ms(10);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(80);
+                mypwm2.pulsewidth_ms(6);
                 mypwm3.pulsewidth_ms(0);
                 break;
-                // STATE_A3 : hidariに曲がる(左:正転 右:正転)
+            // STATE_A3 : hidariに曲がる(左:正転 右:正転)
             case STATE_A3:
+                printf("STATE_A3\n");
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(80);
+                mypwm4.pulsewidth_ms(6);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
                 mypwm2.pulsewidth_ms(10);
                 mypwm3.pulsewidth_ms(0);
                 break;
-                
             // STATE_B : 右旋回(左:正転 右:逆転)
             case STATE_B:
+                printf("STATE_B\n");
                 myled = 1; // LED点灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(100);
+                mypwm4.pulsewidth_ms(15);
                 mypwm6.pulsewidth_ms(0);
                 // 右モータの制御
                 mypwm2.pulsewidth_ms(0);
-                mypwm3.pulsewidth_ms(100);
+                mypwm3.pulsewidth_ms(15);
                 break;
-                
             // STATE_C : 左旋回(左:逆転 右:正転)
             case STATE_C:
+                printf("STATE_C\n");
+                myled = 1; // LED点灯
+                // 左モータの制御
+                mypwm4.pulsewidth_ms(0);
+                mypwm6.pulsewidth_ms(15);
+                // 右モータの制御
+                mypwm2.pulsewidth_ms(15);
+                mypwm3.pulsewidth_ms(0);
+                break;
+            // STATE_D : 後退(左:逆転 右:逆転)
+            case STATE_D:
+                printf("STATE_D\n");
                 myled = 1; // LED点灯
                 // 左モータの制御
                 mypwm4.pulsewidth_ms(0);
-                mypwm6.pulsewidth_ms(100);
-                // 右モータの制御
-                mypwm2.pulsewidth_ms(100);
-                mypwm3.pulsewidth_ms(0);
-                break;
-                
-            // STATE_D : 後退(左:逆転 右:逆転)
-            case STATE_D:
-                myled = 1; // LED点灯
-                // 左モータの制御
-                mypwm4.pulsewidth_ms(0);
-                mypwm6.pulsewidth_ms(80);
+                mypwm6.pulsewidth_ms(15);
                 // 右モータの制御
                 mypwm2.pulsewidth_ms(0);
-                mypwm3.pulsewidth_ms(83);
+                mypwm3.pulsewidth_ms(15);
                 break;
-                
             // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ)
             case STATE_E:
+                printf("STATE_E\n");
                 myled = 0; // LED消灯
                 // 左モータの制御
-                mypwm4.pulsewidth_ms(60);
-                mypwm6.pulsewidth_ms(60);
+                mypwm4.pulsewidth_ms(30);
+                mypwm6.pulsewidth_ms(30);
                 // 右モータの制御
-                mypwm2.pulsewidth_ms(60);
-                mypwm3.pulsewidth_ms(60);
+                mypwm2.pulsewidth_ms(30);
+                mypwm3.pulsewidth_ms(30);
+                break;
+            default:
                 break;
         }
     }
-}    }
+}
+
+
+void LineTrace(void) {   // ラインの右側でライントレース
+  if(val_M > 0.5) { // 中央:白
+    mode = STATE_A2;      // 左前進
+  } else {               // 中央:黒
+    mode = STATE_A3;      // 右前進
+  }
+  wait_ms(50);
+}
\ No newline at end of file