圭祐 杉浦
/
ライントレーサー termB
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MotorDriver_SU USBDevice mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MotorDriver_SU.h" 00003 00004 00005 DigitalIn sw(P0_1, PullUp); 00006 DigitalOut led(P1_28); 00007 AnalogIn sw1(P0_22); 00008 AnalogIn sw2(P0_12); 00009 AnalogIn sw3(P0_14); 00010 AnalogIn sw4(P0_11); 00011 AnalogIn sw5(P0_13); 00012 00013 MotorDriver_SU motor(MOTOR_DOUBLE); 00014 MotorDriver_SU lift(MOTOR_SINGLE); 00015 00016 00017 int main( ){ 00018 float hoge1; 00019 float hoge2; 00020 float hoge3; 00021 float hoge4; 00022 float hoge5; 00023 int AI = 0; 00024 00025 00026 /********************************/ 00027 /********************************/ 00028 /********************************/ 00029 /*荷物を持ち上げる前のプログラム*/ 00030 /********************************/ 00031 /********************************/ 00032 /********************************/ 00033 00034 while(1){ 00035 00036 hoge1 = sw1.read(); 00037 hoge2 = sw2.read(); 00038 hoge3 = sw3.read(); 00039 hoge4 = sw4.read(); 00040 hoge5 = sw5.read(); 00041 00042 while( sw == 0 ){ 00043 motor.Drive(0, CW, 0); 00044 motor.Drive(1, CW, 0); 00045 00046 } 00047 /************************************/ 00048 /*ラインに沿って走るためのプログラム*/ 00049 /************************************/ 00050 ///////////まっすぐ走るためのプログラム////////// 00051 /*1.1.1 センサ１が黒*/ 00052 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00053 motor.Drive(0, CW, 250); 00054 motor.Drive(1, CW, 4095); 00055 00056 } 00057 00058 00059 /*1.1.2 センサ2が黒*/ 00060 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00061 motor.Drive(0, CW, 500); 00062 motor.Drive(1, CW, 4095); 00063 00064 } 00065 00066 00067 /*1.1.3 センサ3が黒*/ 00068 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00069 motor.Drive(0, CW, 4095); 00070 motor.Drive(1, CW, 4095); 00071 00072 } 00073 00074 00075 /*1.1.4 センサ4が黒*/ 00076 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00077 motor.Drive(0, CW, 4095); 00078 motor.Drive(1, CW, 500); 00079 00080 } 00081 00082 00083 /*1.1.5 センサ5が黒*/ 00084 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00085 motor.Drive(0, CW, 4095); 00086 motor.Drive(1, CW, 250); 00087 00088 } 00089 00090 /*1.1.6 センサ2-3が黒*/ 00091 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00092 motor.Drive(0, CW, 1500); 00093 motor.Drive(1, CW, 4095); 00094 00095 } 00096 00097 /*1.1.7 センサ3-4が黒*/ 00098 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00099 motor.Drive(0, CW, 4095); 00100 motor.Drive(1, CW, 1500); 00101 00102 } 00103 00104 ///////////大きい右カーブを曲がるためのプログラム////////// 00105 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/ 00106 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){ 00107 AI = 1; 00108 } 00109 00110 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00111 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){ 00112 AI = 1; 00113 } 00114 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00115 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){ 00116 AI = 1; 00117 } 00118 00119 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00120 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00121 AI = 1; 00122 } 00123 00124 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00125 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){ 00126 AI = 1; 00127 } 00128 00129 /*1.2.5 全部白かつAI = 1*/ 00130 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00131 motor.Drive(0, CW, 4095); 00132 motor.Drive(1, CCW, 4095); 00133 00134 } 00135 00136 ///////////大きい左カーブを曲がるためのプログラム////////// 00137 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/ 00138 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00139 AI = 2; 00140 } 00141 00142 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00143 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00144 AI = 2; 00145 } 00146 00147 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/ 00148 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ 00149 AI = 2; 00150 } 00151 00152 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/ 00153 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00154 AI = 2; 00155 } 00156 00157 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00158 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00159 AI = 2; 00160 } 00161 00162 /*1.3.6 全部白かつAI = 2*/ 00163 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00164 motor.Drive(0, CCW, 4095); 00165 motor.Drive(1, CW, 4095); 00166 00167 } 00168 00169 /**********************************/ 00170 /*停止位置で止まるためのプログラム*/ 00171 /**********************************/ 00172 00173 /*1.4.1 センサ1-3-5が黒*/ 00174 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00175 motor.Drive(0, STOP, 0); 00176 motor.Drive(1, STOP, 0); 00177 AI = 0; 00178 break; 00179 00180 } 00181 00182 /*1.4.2 センサ1-2-5が黒*/ 00183 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00184 motor.Drive(0, STOP, 0); 00185 motor.Drive(1, STOP, 0); 00186 AI = 0; 00187 break; 00188 00189 } 00190 00191 /*1.4.3 センサ1-4-5が黒*/ 00192 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00193 motor.Drive(0, STOP, 0); 00194 motor.Drive(1, STOP, 0); 00195 AI = 0; 00196 break; 00197 } 00198 00199 /*1.4.4 センサ1-2-3-5が黒*/ 00200 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00201 motor.Drive(0, STOP, 0); 00202 motor.Drive(1, STOP, 0); 00203 AI = 0; 00204 break; 00205 } 00206 00207 /*1.4.5 センサ1-3-4-5が黒*/ 00208 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00209 motor.Drive(0, STOP, 0); 00210 motor.Drive(1, STOP, 0); 00211 AI = 0; 00212 break; 00213 } 00214 } 00215 00216 ///////////停止////////// 00217 motor.Drive(0, STOP, 0); 00218 motor.Drive(1, STOP, 0); 00219 wait(0.1); 00220 ///////////リフト上げる////////// 00221 lift.Drive(0, CCW, 4095); 00222 wait(1.5); 00223 ///////////リフト停止////////// 00224 lift.Drive(0, STOP, 0); 00225 wait(0.1); 00226 ///////////停止位置を超えるために少し前進////////// 00227 motor.Drive(0, CW, 4095); 00228 motor.Drive(1, CW, 4095); 00229 wait(2.1); 00230 00231 00232 00233 00234 /********************************/ 00235 /********************************/ 00236 /********************************/ 00237 /*荷物を持ち上げた後のプログラム*/ 00238 /********************************/ 00239 /********************************/ 00240 /********************************/ 00241 00242 00243 while(1){ 00244 00245 hoge1 = sw1.read(); 00246 hoge2 = sw2.read(); 00247 hoge3 = sw3.read(); 00248 hoge4 = sw4.read(); 00249 hoge5 = sw5.read(); 00250 00251 00252 /************************************/ 00253 /*ラインに沿って走るためのプログラム*/ 00254 /************************************/ 00255 ///////////まっすぐ走るためのプログラム////////// 00256 /*1.1.1 センサ１が黒*/ 00257 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00258 motor.Drive(0, CW, 250); 00259 motor.Drive(1, CW, 4095); 00260 00261 } 00262 00263 00264 /*1.1.2 センサ2が黒*/ 00265 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00266 motor.Drive(0, CW, 500); 00267 motor.Drive(1, CW, 4095); 00268 00269 } 00270 00271 00272 /*1.1.3 センサ3が黒*/ 00273 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00274 motor.Drive(0, CW, 4095); 00275 motor.Drive(1, CW, 4095); 00276 00277 } 00278 00279 00280 /*1.1.4 センサ4が黒*/ 00281 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00282 motor.Drive(0, CW, 4095); 00283 motor.Drive(1, CW, 500); 00284 00285 } 00286 00287 00288 /*1.1.5 センサ5が黒*/ 00289 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00290 motor.Drive(0, CW, 4095); 00291 motor.Drive(1, CW, 250); 00292 00293 } 00294 00295 /*1.1.6 センサ2-3が黒*/ 00296 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00297 motor.Drive(0, CW, 1500); 00298 motor.Drive(1, CW, 4095); 00299 00300 } 00301 00302 /*1.1.7 センサ3-4が黒*/ 00303 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00304 motor.Drive(0, CW, 4095); 00305 motor.Drive(1, CW, 1500); 00306 00307 } 00308 00309 ///////////大きい右カーブを曲がるためのプログラム////////// 00310 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/ 00311 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){ 00312 AI = 1; 00313 } 00314 00315 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00316 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){ 00317 AI = 1; 00318 } 00319 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00320 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){ 00321 AI = 1; 00322 } 00323 00324 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00325 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00326 AI = 1; 00327 } 00328 00329 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00330 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){ 00331 AI = 1; 00332 } 00333 00334 /*1.2.5 全部白かつAI = 1*/ 00335 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00336 motor.Drive(0, CW, 4095); 00337 motor.Drive(1, CCW, 4095); 00338 00339 } 00340 00341 ///////////大きい左カーブを曲がるためのプログラム////////// 00342 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/ 00343 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00344 AI = 2; 00345 } 00346 00347 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00348 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00349 AI = 2; 00350 } 00351 00352 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/ 00353 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ 00354 AI = 2; 00355 } 00356 00357 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/ 00358 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00359 AI = 2; 00360 } 00361 00362 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00363 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00364 AI = 2; 00365 } 00366 00367 /*1.3.6 全部白かつAI = 2*/ 00368 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00369 motor.Drive(0, CCW, 4095); 00370 motor.Drive(1, CW, 4095); 00371 00372 } 00373 00374 /**********************************/ 00375 /*停止位置で止まるためのプログラム*/ 00376 /**********************************/ 00377 00378 /*1.4.1 センサ1-3-5が黒*/ 00379 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00380 motor.Drive(0, STOP, 0); 00381 motor.Drive(1, STOP, 0); 00382 AI = 0; 00383 break; 00384 00385 } 00386 00387 /*1.4.2 センサ1-2-5が黒*/ 00388 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00389 motor.Drive(0, STOP, 0); 00390 motor.Drive(1, STOP, 0); 00391 AI = 0; 00392 break; 00393 00394 } 00395 00396 /*1.4.3 センサ1-4-5が黒*/ 00397 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00398 motor.Drive(0, STOP, 0); 00399 motor.Drive(1, STOP, 0); 00400 AI = 0; 00401 break; 00402 } 00403 00404 /*1.4.4 センサ1-2-3-5が黒*/ 00405 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00406 motor.Drive(0, STOP, 0); 00407 motor.Drive(1, STOP, 0); 00408 AI = 0; 00409 break; 00410 } 00411 00412 /*1.4.5 センサ1-3-4-5が黒*/ 00413 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00414 motor.Drive(0, STOP, 0); 00415 motor.Drive(1, STOP, 0); 00416 AI = 0; 00417 break; 00418 } 00419 } 00420 00421 ///////////停止////////// 00422 motor.Drive(0, STOP, 0); 00423 motor.Drive(1, STOP, 0); 00424 wait(0.1); 00425 ///////////リフト下げる////////// 00426 lift.Drive(0, CW, 4095); 00427 wait(1.0); 00428 ///////////リフト停止////////// 00429 lift.Drive(0, STOP, 0); 00430 wait(0.1); 00431 ///////////バック////////// 00432 motor.Drive(0, CCW, 4095); 00433 motor.Drive(1, CCW, 4095); 00434 wait(2.0); 00435 ///////////モータ停止////////// 00436 motor.Drive(0, STOP, 0); 00437 motor.Drive(1, STOP, 0); 00438 wait(1.0); 00439 00440 00441 00442 }
Generated on Fri Jul 22 2022 00:46:51 by
1.7.2