a
Dependencies: Locate Move Servo button mbed
Fork of 4thcomp by
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 #include "locate.h" 00004 #include "move.h" 00005 #include "servo.h" 00006 #include "button.h" 00007 00008 DigitalOut ledg(PC_2); //green 00009 DigitalOut ledy(PC_3); //yellow 00010 DigitalOut ledr(PC_0); //red 00011 00012 00013 const int dist=150;//タイヤ、アーム間の距離 00014 const int slidedist=0; //102-20; 00015 const int space=50;//壁やばそうなときのよゆう 00016 //btimeはservoに移動 00017 const float allowdegree=0.2; 00018 const float allowlength=0.2; 00019 00020 const int unit=583; 00021 00022 00023 int main() 00024 { 00025 Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信 00026 int teams=1,phase=0; 00027 teams=teamLED(); 00028 int team = 1; 00029 setup(teams); 00030 initmotor(teams); 00031 00032 open_arm(); 00033 wait(2); 00034 close_arm(); 00035 buzzer(1); 00036 wait(1); 00037 buzzer(0); 00038 00039 int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 }; 00040 int targety[9] = { team*(300+slidedist),team*(600+slidedist),team*(900+slidedist),team*(1200+slidedist),team*(900+slidedist),team*(600+slidedist),team*(600+slidedist),team*(0+slidedist),team*(300+slidedist) }; 00041 int goalx=220,goaly=1200-30+(slidedist)*0;//yへのslidediatを無視 00042 00043 phase=phaseSW(); 00044 00045 if(phase==0) { 00046 buzzer(1); 00047 wait(0.4); 00048 buzzer(0); 00049 wait(0.2); 00050 buzzer(1); 00051 wait(0.4); 00052 buzzer(0); 00053 wait(0.2); 00054 buzzer(1); 00055 wait(0.4); 00056 buzzer(0); 00057 } 00058 00059 while(start()) {} 00060 if(phase==15) { 00061 movelength(900); 00062 buzzer(1); 00063 00064 while(1) { 00065 update(); 00066 } 00067 } 00068 00069 if(phase==14) { 00070 int i = 1; 00071 while(1) 00072 { 00073 00074 turnrad(-2*PI*3*(i++)); 00075 wait(5); 00076 } 00077 00078 while(1) { 00079 update(); 00080 } 00081 } 00082 if(phase==13) { 00083 move(-70, -70); 00084 } 00085 00086 if(phase==12) { 00087 while(1) { 00088 open_arm(); 00089 wait(3); 00090 close_arm(); 00091 wait(3); 00092 } 00093 } 00094 00095 if(phase==11) { 00096 while(1) { 00097 close_hand(); 00098 wait(0.3); 00099 lift(); 00100 wait(0.2); 00101 open_hand(); 00102 wait(2); 00103 } 00104 } 00105 00106 if(phase==10) { 00107 while(1) { 00108 move(70,70); 00109 update(); 00110 } 00111 } 00112 if(phase==9) { 00113 while(1) { 00114 move(32,30); 00115 while(1) { 00116 update_np(); 00117 if(coordinateX()>900) { 00118 break; 00119 } 00120 } 00121 pc.printf("start"); 00122 update(); 00123 } 00124 } 00125 00126 //オブジェクト0,1,2回収 00127 if(phase==0) { 00128 00129 pmove2(targetx[0],-60); 00130 open_hand(); 00131 pmove2(targetx[0],targety[0]); 00132 close_hand(); 00133 wait(0.3); 00134 lift(); 00135 open_hand(); 00136 //回収 00137 pmove2(targetx[1],targety[1]); 00138 close_hand(); 00139 wait(0.3); 00140 lift(); 00141 open_hand(); 00142 //回収 00143 pmove2(targetx[2],targety[2]); 00144 //回収 00145 close_hand(); 00146 pmove2(goalx,goaly-40); 00147 //リリース 00148 turnrad_ccw(PI,team); 00149 open_arm(); 00150 open_hand(); 00151 wait(0.5); 00152 00153 back300();//(300,1200)なう 00154 turnrad_ccw(2*PI,team); 00155 //-------------------------------------------------------------------- 00156 open_hand(); 00157 \ 00158 pmove2(targetx[3]-50,targety[3]); 00159 00160 close_arm(); 00161 close_hand(); 00162 turnrad_cw(PI,team); 00163 00164 pmove2(300,team*(goaly));//ゴール 00165 00166 open_arm(); 00167 open_hand(); 00168 wait(0.3); 00169 00170 00171 back300(); 00172 turnrad_ccw(2*PI,team); 00173 phase=2; 00174 00175 //また縦に回収 00176 close_arm(); 00177 pmove2(900,team*(1200+30)); 00178 00179 open_hand(); 00180 pmove2(targetx[4],targety[4]); 00181 close_hand(); 00182 wait(0.3); 00183 lift(); 00184 open_hand(); 00185 //回収 00186 pmove2(targetx[6],targety[6]); 00187 close_hand(); 00188 wait(0.3); 00189 lift(); 00190 open_hand(); 00191 //回収 00192 pmove2(targetx[8],targety[8]); 00193 //回収 00194 close_hand(); 00195 pmove2(150,team*(600+slidedist));//斜め移動 00196 00197 turnrad_ccw(PI,team); 00198 open_arm(); 00199 open_hand(); 00200 back300(); 00201 00202 00203 pmove2(600,600); 00204 pmove2(targetx[5]-50,targety[5]); 00205 close_arm(); 00206 close_hand(); 00207 nxback300(); 00208 pmove2(150,600+slidedist); 00209 open_arm(); 00210 open_hand(); 00211 back300(); 00212 pmove2(900-50,50); 00213 pmove2(1200-90,0); 00214 close_arm(); 00215 close_hand(); 00216 nxback300(); 00217 pmove2(150,900+slidedist); 00218 turnrad_ccw(PI,team); 00219 open_arm(); 00220 open_hand(); 00221 back300(); 00222 00223 while(1) { 00224 buzzer(1); 00225 wait(0.5); 00226 buzzer(0); 00227 wait(1); 00228 } 00229 } 00230 00231 00232 if(phase==2) { 00233 open_arm(); 00234 00235 open_hand(); 00236 pmove2(900,0); 00237 pmove2(1200-80,0);//オブジェクト 00238 close_arm(); 00239 close_hand(); 00240 nxback300(); 00241 pmove2(150,900+slidedist); 00242 turnrad_ccw(PI,team); 00243 open_arm(); 00244 open_hand(); 00245 back300(); 00246 00247 pmove2(600,600); 00248 pmove2(targetx[5]-80,targety[5]); 00249 close_arm(); 00250 close_hand(); 00251 nxback300(); 00252 pmove2(150,600+slidedist); 00253 back300(); 00254 00255 } 00256 00257 if(phase==1) { 00258 00259 pmove(900,0); 00260 open_hand(); 00261 pmove2(targetx[8],targety[8]); 00262 close_hand(); 00263 wait(0.3); 00264 lift(); 00265 open_hand(); 00266 //回収 00267 pmove2(targetx[6],targety[6]); 00268 close_hand(); 00269 wait(0.3); 00270 lift(); 00271 open_hand(); 00272 //回収 00273 pmove(targetx[4],targety[4]); 00274 //回収 00275 close_hand(); 00276 pmove2(goalx,goaly-40); 00277 //リリース 00278 turnrad_ccw(PI,team); 00279 open_arm(); 00280 open_hand(); 00281 wait(0.5); 00282 back300();//(300,1200)なう 00283 00284 } 00285 00286 00287 if(phase==3) { 00288 00289 00290 pmove2(targetx[0],-60); 00291 open_hand(); 00292 pmove2(targetx[0],targety[0]); 00293 close_hand(); 00294 wait(0.3); 00295 lift(); 00296 open_hand(); 00297 //回収 00298 pmove2(targetx[1],targety[1]); 00299 close_hand(); 00300 wait(0.3); 00301 lift(); 00302 open_hand(); 00303 //回収 00304 pmove2(targetx[2],targety[2]); 00305 //回収 00306 close_hand(); 00307 pmove2(goalx,goaly-40); 00308 //リリース 00309 turnrad_ccw(PI,team); 00310 open_arm(); 00311 open_hand(); 00312 wait(0.5); 00313 back300();//(300,1200)なう 00314 turnrad_ccw(2*PI,team); 00315 00316 00317 close_arm(); 00318 pmove2(900,team*(1200+30)); 00319 00320 open_hand(); 00321 pmove2(targetx[4],targety[4]); 00322 close_hand(); 00323 wait(0.3); 00324 lift(); 00325 open_hand(); 00326 //回収 00327 pmove2(targetx[6],targety[6]); 00328 close_hand(); 00329 wait(0.3); 00330 lift(); 00331 open_hand(); 00332 //回収 00333 pmove2(targetx[8],targety[8]); 00334 //回収 00335 close_hand(); 00336 pmove2(150,team*(600+slidedist));//斜め移動 00337 00338 turnrad_ccw((-1)*PI,team); 00339 open_arm(); 00340 open_hand(); 00341 back300(); 00342 } 00343 00344 if(phase == 10) { 00345 buzzer(1); 00346 turnrad(6*PI); 00347 while(1) update(); 00348 } 00349 00350 00351 00352 if(phase == 8) { 00353 open_arm(); 00354 open_hand(); 00355 00356 pmove2(0,1000); 00357 pmove2(600,1200); 00358 pmove2(1200-50,1200); 00359 close_hand(); 00360 close_arm(); 00361 turnrad_cw((-1)*PI,1); 00362 pmove2(150,1200); 00363 open_hand(); 00364 open_arm(); 00365 back300(); 00366 00367 turnrad_ccw(0,1); 00368 pmove2(1800,1200); 00369 close_hand(); 00370 close_arm(); 00371 turnrad_cw((-1)*PI,1); 00372 pmove2(150,1200); 00373 open_hand(); 00374 open_arm(); 00375 back300(); 00376 } 00377 00378 if(phase == 7) { 00379 open_arm(); 00380 open_hand(); 00381 00382 pmove2(0,1000); 00383 pmove2(600,1200); 00384 pmove2(1800,1200); 00385 close_hand(); 00386 close_arm(); 00387 turnrad_cw((-1)*PI,1); 00388 pmove2(150,1200); 00389 open_hand(); 00390 open_arm(); 00391 back300(); 00392 00393 00394 } 00395 00396 if(phase == 6) { 00397 buzzer(1); 00398 initbutton(); 00399 int command[3]; 00400 commandIn(command); 00401 00402 commandMove(command[0], command[1], command[2]); 00403 } 00404 00405 if(phase == 5) { 00406 buzzer(1); 00407 initbutton(); 00408 int command[3]; 00409 commandIn(command); 00410 00411 commandMoveEnemy(command[0], command[1], command[2]); 00412 } 00413 00414 }
Generated on Wed Jul 13 2022 20:27:46 by
1.7.2
