圭祐 杉浦
本番(11/10)で使用したプログラム
Dependencies: MotorDriver_SU USBDevice mbed
Embed:
(wiki syntax)
Show/hide line numbers
termB-1.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 00043 /************************************/ 00044 /*ラインに沿って走るためのプログラム*/ 00045 /************************************/ 00046 ///////////まっすぐ走るためのプログラム////////// 00047 /*1.1.1 センサ１が黒*/ 00048 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00049 motor.Drive(0, CW, 500); 00050 motor.Drive(1, CW, 4095); 00051 00052 } 00053 00054 00055 /*1.1.2 センサ2が黒*/ 00056 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00057 motor.Drive(0, CW, 800); 00058 motor.Drive(1, CW, 4095); 00059 00060 } 00061 00062 00063 /*1.1.3 センサ3が黒*/ 00064 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00065 motor.Drive(0, CW, 4095); 00066 motor.Drive(1, CW, 4095); 00067 00068 } 00069 00070 00071 /*1.1.4 センサ4が黒*/ 00072 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00073 motor.Drive(0, CW, 4095); 00074 motor.Drive(1, CW, 800); 00075 00076 } 00077 00078 00079 /*1.1.5 センサ5が黒*/ 00080 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00081 motor.Drive(0, CW, 4095); 00082 motor.Drive(1, CW, 500); 00083 00084 } 00085 00086 /*1.1.6 センサ2-3が黒*/ 00087 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00088 motor.Drive(0, CW, 1500); 00089 motor.Drive(1, CW, 4095); 00090 00091 } 00092 00093 /*1.1.7 センサ3-4が黒*/ 00094 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00095 motor.Drive(0, CW, 4095); 00096 motor.Drive(1, CW, 1500); 00097 00098 } 00099 00100 ///////////大きい右カーブを曲がるためのプログラム////////// 00101 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/ 00102 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){ 00103 AI = 1; 00104 } 00105 00106 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00107 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){ 00108 AI = 1; 00109 } 00110 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00111 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){ 00112 AI = 1; 00113 } 00114 00115 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00116 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00117 AI = 1; 00118 } 00119 00120 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00121 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){ 00122 AI = 1; 00123 } 00124 00125 /*1.2.5 全部白かつAI = 1*/ 00126 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00127 motor.Drive(0, CW, 4095); 00128 motor.Drive(1, CCW, 4095); 00129 00130 } 00131 00132 ///////////大きい左カーブを曲がるためのプログラム////////// 00133 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/ 00134 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00135 AI = 2; 00136 } 00137 00138 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00139 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00140 AI = 2; 00141 } 00142 00143 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/ 00144 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ 00145 AI = 2; 00146 } 00147 00148 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/ 00149 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00150 AI = 2; 00151 } 00152 00153 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00154 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00155 AI = 2; 00156 } 00157 00158 /*1.3.6 全部白かつAI = 2*/ 00159 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00160 motor.Drive(0, CCW, 4095); 00161 motor.Drive(1, CW, 4095); 00162 00163 } 00164 00165 /**********************************/ 00166 /*停止位置で止まるためのプログラム*/ 00167 /**********************************/ 00168 00169 /*1.4.1 センサ1-3-5が黒*/ 00170 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00171 motor.Drive(0, STOP, 0); 00172 motor.Drive(1, STOP, 0); 00173 break; 00174 00175 } 00176 00177 /*1.4.2 センサ1-2-5が黒*/ 00178 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00179 motor.Drive(0, STOP, 0); 00180 motor.Drive(1, STOP, 0); 00181 break; 00182 00183 } 00184 00185 /*1.4.3 センサ1-4-5が黒*/ 00186 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00187 motor.Drive(0, STOP, 0); 00188 motor.Drive(1, STOP, 0); 00189 break; 00190 } 00191 00192 /*1.4.4 センサ1-2-3-5が黒*/ 00193 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00194 motor.Drive(0, STOP, 0); 00195 motor.Drive(1, STOP, 0); 00196 break; 00197 } 00198 00199 /*1.4.5 センサ1-3-4-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 break; 00204 } 00205 } 00206 00207 ///////////停止////////// 00208 motor.Drive(0, STOP, 0); 00209 motor.Drive(1, STOP, 0); 00210 wait(0.1); 00211 ///////////リフト上げる////////// 00212 lift.Drive(0, CCW, 4095); 00213 wait(0.8); 00214 ///////////リフト停止////////// 00215 lift.Drive(0, STOP, 0); 00216 wait(0.1); 00217 ///////////停止位置を超えるために少し前進////////// 00218 motor.Drive(0, CW, 4095); 00219 motor.Drive(1, CW, 4095); 00220 wait(1.0); 00221 00222 00223 00224 00225 /********************************/ 00226 /********************************/ 00227 /********************************/ 00228 /*荷物を持ち上げた後のプログラム*/ 00229 /********************************/ 00230 /********************************/ 00231 /********************************/ 00232 00233 00234 while(1){ 00235 00236 hoge1 = sw1.read(); 00237 hoge2 = sw2.read(); 00238 hoge3 = sw3.read(); 00239 hoge4 = sw4.read(); 00240 hoge5 = sw5.read(); 00241 00242 00243 /************************************/ 00244 /*ラインに沿って走るためのプログラム*/ 00245 /************************************/ 00246 ///////////まっすぐ走るためのプログラム////////// 00247 /*1.1.1 センサ１が黒*/ 00248 if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00249 motor.Drive(0, CW, 500); 00250 motor.Drive(1, CW, 4095); 00251 00252 } 00253 00254 00255 /*1.1.2 センサ2が黒*/ 00256 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00257 motor.Drive(0, CW, 800); 00258 motor.Drive(1, CW, 4095); 00259 00260 } 00261 00262 00263 /*1.1.3 センサ3が黒*/ 00264 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00265 motor.Drive(0, CW, 4095); 00266 motor.Drive(1, CW, 4095); 00267 00268 } 00269 00270 00271 /*1.1.4 センサ4が黒*/ 00272 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00273 motor.Drive(0, CW, 4095); 00274 motor.Drive(1, CW, 800); 00275 00276 } 00277 00278 00279 /*1.1.5 センサ5が黒*/ 00280 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00281 motor.Drive(0, CW, 4095); 00282 motor.Drive(1, CW, 500); 00283 00284 } 00285 00286 /*1.1.6 センサ2-3が黒*/ 00287 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00288 motor.Drive(0, CW, 1500); 00289 motor.Drive(1, CW, 4095); 00290 00291 } 00292 00293 /*1.1.7 センサ3-4が黒*/ 00294 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00295 motor.Drive(0, CW, 4095); 00296 motor.Drive(1, CW, 1500); 00297 00298 } 00299 00300 ///////////大きい右カーブを曲がるためのプログラム////////// 00301 /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/ 00302 else if(sw1 > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5 <= 0.3){ 00303 AI = 1; 00304 } 00305 00306 /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00307 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 <= 0.3){ 00308 AI = 1; 00309 } 00310 /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00311 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 <= 0.3){ 00312 AI = 1; 00313 } 00314 00315 /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/ 00316 else if(sw1 > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){ 00317 AI = 1; 00318 } 00319 00320 /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/ 00321 else if(sw1 > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){ 00322 AI = 1; 00323 } 00324 00325 /*1.2.5 全部白かつAI = 1*/ 00326 else if(AI == 1 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00327 motor.Drive(0, CW, 4095); 00328 motor.Drive(1, CCW, 4095); 00329 00330 } 00331 00332 ///////////大きい左カーブを曲がるためのプログラム////////// 00333 /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/ 00334 else if(sw1 <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00335 AI = 2; 00336 } 00337 00338 /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00339 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){ 00340 AI = 2; 00341 } 00342 00343 /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/ 00344 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){ 00345 AI = 2; 00346 } 00347 00348 /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/ 00349 else if(sw1 <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00350 AI = 2; 00351 } 00352 00353 /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/ 00354 else if(sw1 <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){ 00355 AI = 2; 00356 } 00357 00358 /*1.3.6 全部白かつAI = 2*/ 00359 else if(AI == 2 && sw1 > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){ 00360 motor.Drive(0, CCW, 4095); 00361 motor.Drive(1, CW, 4095); 00362 00363 } 00364 00365 /**********************************/ 00366 /*停止位置で止まるためのプログラム*/ 00367 /**********************************/ 00368 00369 /*1.4.1 センサ1-3-5が黒*/ 00370 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00371 motor.Drive(0, STOP, 0); 00372 motor.Drive(1, STOP, 0); 00373 break; 00374 00375 } 00376 00377 /*1.4.2 センサ1-2-5が黒*/ 00378 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00379 motor.Drive(0, STOP, 0); 00380 motor.Drive(1, STOP, 0); 00381 break; 00382 00383 } 00384 00385 /*1.4.3 センサ1-4-5が黒*/ 00386 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00387 motor.Drive(0, STOP, 0); 00388 motor.Drive(1, STOP, 0); 00389 break; 00390 } 00391 00392 /*1.4.4 センサ1-2-3-5が黒*/ 00393 else if(sw1 <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.5){ 00394 motor.Drive(0, STOP, 0); 00395 motor.Drive(1, STOP, 0); 00396 break; 00397 } 00398 00399 /*1.4.5 センサ1-3-4-5が黒*/ 00400 else if(sw1 <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 <= 0.5){ 00401 motor.Drive(0, STOP, 0); 00402 motor.Drive(1, STOP, 0); 00403 break; 00404 } 00405 } 00406 00407 ///////////停止////////// 00408 motor.Drive(0, STOP, 0); 00409 motor.Drive(1, STOP, 0); 00410 wait(0.1); 00411 ///////////リフト下げる////////// 00412 lift.Drive(0, CW, 4095); 00413 wait(1.0); 00414 ///////////リフト停止////////// 00415 lift.Drive(0, STOP, 0); 00416 wait(0.1); 00417 ///////////バック////////// 00418 motor.Drive(0, CCW, 4095); 00419 motor.Drive(1, CCW, 4095); 00420 wait(3.0); 00421 ///////////モータ停止////////// 00422 motor.Drive(0, STOP, 0); 00423 motor.Drive(1, STOP, 0); 00424 wait(1.0); 00425 00426 00427 00428 }
Generated on Sat Aug 20 2022 04:49:41 by
1.7.2