a

Dependencies:   Locate Move Servo button mbed

Fork of 4thcomp by 涼太郎 中村

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }