2号機

Dependencies:   HCSR04 mbed

Fork of Nucleo_SS2 by SS2

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 
00004 AnalogIn photoref_L(A1); // フォトリフレクタ左
00005 AnalogIn photoref_M(A2); // フォトリフレクタ中央
00006 AnalogIn photoref_R(A3); // フォトリフレクタ右
00007 
00008 PwmOut mypwm4(D4); // モータ
00009 PwmOut mypwm6(D6); // モータ
00010 PwmOut mypwm2(D2); // モータ
00011 PwmOut mypwm3(D3); // モータ
00012 
00013 HCSR04  usensor1(D9,D10);  // 超音波後;Trigger(DO), Echo(PWMIN)
00014 HCSR04  usensor2(D11,D12); // 超音波横;Trigger(DO), Echo(PWMIN)
00015 
00016 Timer t;
00017 Ticker timer_flag;
00018 
00019 
00020 DigitalOut myled(LED1);
00021 
00022 
00023 unsigned int dist1,dist2;
00024 
00025 
00026 #define STATE_A (0) // 前進
00027 #define STATE_B (1)
00028 #define STATE_C (2)
00029 #define STATE_D (3)
00030 #define STATE_E (4)
00031 #define STATE_A2 (5)
00032 #define STATE_A3 (6)
00033 
00034 char mode = STATE_E;
00035 int fase;
00036 int wait_flag;
00037 int timer;
00038 void LineTrace(void);
00039 float val_L, val_R, val_M;
00040 
00041 
00042 
00043 void tflg() {
00044     timer = t.read(); 
00045 }
00046 /*void flg(){
00047         usensor.start();
00048         dist=usensor.get_dist_cm();
00049         printf("dist:%u\n", dist);
00050         }*/
00051 
00052 int main() {
00053     t.start();
00054     wait(3);
00055     wait_flag = 0;
00056     fase = 0;
00057     
00058     timer_flag.attach(&tflg, 1.0); // タイマー割り込み設定
00059     
00060     while(1) {
00061         val_M = photoref_M.read();
00062         val_L = photoref_L.read();
00063         val_R = photoref_R.read();
00064         printf("M: %f\n", val_M);
00065         printf("L: %f\n", val_L);
00066         printf("R: %f\n", val_R);
00067     
00068         if(val_L > 0.5 && val_R > 0.5) { // 左:白,右:白
00069           LineTrace(); // 直進
00070         } else if(val_L < 0.5 && val_R > 0.5) { // 左:白,右:黒
00071           mode = STATE_A2; // 右前進
00072         } else if(val_L > 0.5 && val_R < 0.5) { // 左:黒,右:白
00073           mode = STATE_A3; // 左前進
00074         } else { // 左:黒,右:黒
00075           mode = STATE_E; // ブレーキ
00076         }
00077         wait_ms(100);
00078         /*
00079         // printf("timer: %d\n", timer);
00080         
00081         // 超音波取得
00082         usensor1.start();
00083         usensor2.start();
00084         wait_ms(500); 
00085         dist1 = usensor1.get_dist_cm(); 
00086         dist2 = usensor2.get_dist_cm();
00087         printf("dist1: %ld [cm]  ",dist1);
00088         printf("dist2: %ld [cm]\n",dist2); 
00089                 
00090         switch(fase) {
00091             case 0:
00092                 mode = STATE_A; // 前進
00093                 //if(timer > 5) {
00094                     /*if(dist2 > 8){
00095                         mode = STATE_A3; //hidariに
00096                         }else if(dist2 < 10){
00097                             mode = STATE_A2;  //migiに
00098                             }else{mode = STATE_A;}
00099                     }
00100                 if(dist1 > 41 && dist1 < 250) { // 後ろの壁から45cmの地点
00101                     wait_flag = 1;
00102                     fase = 1;
00103                     t.stop();
00104                     t.reset();
00105                     t.start();
00106                 }
00107                 break;
00108             case 1:
00109                 mode = STATE_B; // 右旋回
00110                 if(timer > 2) {
00111                     wait_flag = 1;
00112                     fase = 2;
00113                     t.reset();
00114                     t.stop();
00115                     t.start();
00116                 }
00117                 break;
00118             case 2:
00119                 mode = STATE_A; // 前進
00120                 if(timer > 4.0) {
00121                     wait_flag = 1;
00122                     fase = 3;
00123                 }
00124                 break;
00125             case 3:
00126                 mode = STATE_D;
00127                 if(timer > 13.0) {
00128                     wait_flag = 1;
00129                     fase = 4;
00130                 }
00131                 break;
00132             case 4:
00133                  mode = STATE_A; // 前進
00134                  if(timer > 21.0) {
00135                     wait_flag = 1;
00136                     fase = 5;
00137                 }
00138                 break;
00139             case 5:
00140                  mode = STATE_D; // 後退
00141                  if(timer > 29.0) {
00142                     wait_flag = 1;
00143                     fase = 6;
00144                 }
00145                 break;
00146             case 6:
00147                 mode = STATE_A; // 前進
00148                 if(timer > 37.0) {
00149                     wait_flag = 1;
00150                     fase = 7;
00151                 }
00152                 break;
00153             case 7:
00154                 mode = STATE_D; // 後退
00155                 if(timer > 37) {
00156                     mode = STATE_E; // 停止
00157                 }
00158                 break;
00159             default:
00160                 break;
00161         }
00162         
00163         // 状態が切り替わるときは一時停止
00164         if(wait_flag == 1) {
00165             // フラグの初期化
00166             wait_flag = 0;
00167             myled = 0; // LED消灯
00168             // 左モータの制御
00169             mypwm4.pulsewidth_ms(50);
00170             mypwm6.pulsewidth_ms(50);
00171             // 右モータの制御
00172             mypwm2.pulsewidth_ms(50);
00173             mypwm3.pulsewidth_ms(50);
00174             // 500ms待機する
00175             mode = STATE_E;
00176             wait(0.5);
00177         }
00178         */
00179         switch(mode) {
00180             // STATE_A : 前進(左:正転 右:正転)
00181             case STATE_A:
00182                 printf("STATE_A\n");
00183                 myled = 1; // LED点灯
00184                 // 左モータの制御
00185                 mypwm4.pulsewidth_ms(15);
00186                 mypwm6.pulsewidth_ms(0);
00187                 // 右モータの制御
00188                 mypwm2.pulsewidth_ms(15);
00189                 mypwm3.pulsewidth_ms(0);
00190                 break;
00191             // STATE_A2 : 右に曲がる(左:正転 右:正転)
00192             case STATE_A2:
00193                 printf("STATE_A2\n");
00194                 myled = 1; // LED点灯
00195                 // 左モータの制御
00196                 mypwm4.pulsewidth_ms(10);
00197                 mypwm6.pulsewidth_ms(0);
00198                 // 右モータの制御
00199                 mypwm2.pulsewidth_ms(6);
00200                 mypwm3.pulsewidth_ms(0);
00201                 break;
00202             // STATE_A3 : hidariに曲がる(左:正転 右:正転)
00203             case STATE_A3:
00204                 printf("STATE_A3\n");
00205                 myled = 1; // LED点灯
00206                 // 左モータの制御
00207                 mypwm4.pulsewidth_ms(6);
00208                 mypwm6.pulsewidth_ms(0);
00209                 // 右モータの制御
00210                 mypwm2.pulsewidth_ms(10);
00211                 mypwm3.pulsewidth_ms(0);
00212                 break;
00213             // STATE_B : 右旋回(左:正転 右:逆転)
00214             case STATE_B:
00215                 printf("STATE_B\n");
00216                 myled = 1; // LED点灯
00217                 // 左モータの制御
00218                 mypwm4.pulsewidth_ms(15);
00219                 mypwm6.pulsewidth_ms(0);
00220                 // 右モータの制御
00221                 mypwm2.pulsewidth_ms(0);
00222                 mypwm3.pulsewidth_ms(15);
00223                 break;
00224             // STATE_C : 左旋回(左:逆転 右:正転)
00225             case STATE_C:
00226                 printf("STATE_C\n");
00227                 myled = 1; // LED点灯
00228                 // 左モータの制御
00229                 mypwm4.pulsewidth_ms(0);
00230                 mypwm6.pulsewidth_ms(15);
00231                 // 右モータの制御
00232                 mypwm2.pulsewidth_ms(15);
00233                 mypwm3.pulsewidth_ms(0);
00234                 break;
00235             // STATE_D : 後退(左:逆転 右:逆転)
00236             case STATE_D:
00237                 printf("STATE_D\n");
00238                 myled = 1; // LED点灯
00239                 // 左モータの制御
00240                 mypwm4.pulsewidth_ms(0);
00241                 mypwm6.pulsewidth_ms(15);
00242                 // 右モータの制御
00243                 mypwm2.pulsewidth_ms(0);
00244                 mypwm3.pulsewidth_ms(15);
00245                 break;
00246             // STATE_E : ブレーキ(左:ブレーキ 右:ブレーキ)
00247             case STATE_E:
00248                 printf("STATE_E\n");
00249                 myled = 0; // LED消灯
00250                 // 左モータの制御
00251                 mypwm4.pulsewidth_ms(30);
00252                 mypwm6.pulsewidth_ms(30);
00253                 // 右モータの制御
00254                 mypwm2.pulsewidth_ms(30);
00255                 mypwm3.pulsewidth_ms(30);
00256                 break;
00257             default:
00258                 break;
00259         }
00260     }
00261 }
00262 
00263 
00264 void LineTrace(void) {   // ラインの右側でライントレース
00265   if(val_M > 0.5) { // 中央:白
00266     mode = STATE_A2;      // 左前進
00267   } else {               // 中央:黒
00268     mode = STATE_A3;      // 右前進
00269   }
00270   wait_ms(50);
00271 }