Rui Kato
/
Nucleo_SS2_No2
2号機
Fork of Nucleo_SS2 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 21 2022 04:27:16 by 1.7.2