a

Dependencies:   Locate Move Servo button mbed

Fork of 3rdcompfixstart 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=102-20;
00015 const int space=50;//壁やばそうなときのよゆう
00016 const float btime=1.9;
00017 const float allowdegree=0.2;
00018 const float allowlength=0.2;
00019 
00020 
00021 int main()
00022 {
00023     
00024     int teams=1,phase=0;
00025     teams=teamLED();
00026     int team = 1;
00027     setup(teams);
00028     initmotor(teams);
00029     open_arm();
00030     wait(2);
00031     close_arm();
00032     
00033     while(start()){}
00034    int targetx[9] = { 600,600,600,1200,900,1200,900,1200,900 };
00035    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) };
00036    int goalx=150,goaly=1200+(slidedist)*0;//yへのslidediatを無視
00037     
00038     phase=phaseSW();
00039     
00040     //
00041 
00042 //オブジェクト0,1,2回収
00043     if(phase==0){
00044        pmove(targetx[0],0); //傾きをはじめから向くように
00045        open_hand();
00046        pmove2(targetx[0],targety[0]);
00047        close_hand();
00048        wait(0.3);
00049        beltup();
00050        wait(btime);
00051        beltstop();
00052        open_hand();
00053        //回収
00054        pmove2(targetx[1],targety[1]);
00055        close_hand();
00056        wait(0.3);
00057        beltup();
00058        wait(btime);
00059        beltstop();
00060        open_hand();
00061        //回収
00062        pmove(targetx[2],targety[2]);
00063        //回収
00064        close_hand();
00065        pmove2(goalx,goaly);
00066        //リリース
00067        turnrad_ccw(PI,team);
00068        open_arm();
00069        open_hand();
00070        wait(0.5);
00071        back300();//(300,1200)なう
00072         turnrad_ccw(2*PI,team);
00073 //--------------------------------------------------------------------
00074        open_arm();
00075        pmove(targetx[3],targety[3]);
00076        close_arm();
00077        close_hand();
00078        turnrad_cw(PI,team);
00079        /*
00080        open_hanf();
00081        pmove(1800,team*(1200+slidedist));
00082        closehand();
00083        */
00084        pmove(150,team*(1200+slidedist-space));//ゴール
00085 
00086         open_arm();
00087         open_hand();
00088         wait(0.3);
00089 
00090        
00091         back300();
00092         turnrad_ccw(2*PI,team);
00093         phase=2;
00094         
00095        //また縦に回収
00096        close_arm();
00097        pmove2(900,team*(1200+slidedist+space));
00098 
00099        open_hand();
00100        pmove2(targetx[4],targety[4]);
00101        close_hand();
00102        wait(0.3);
00103        beltup();
00104        wait(btime);
00105        beltstop();
00106        open_hand();
00107        //回収
00108        pmove(targetx[6],targety[6]);
00109        close_hand();
00110        wait(0.3);
00111        beltup();
00112        wait(btime);
00113        beltstop();
00114        open_hand();
00115        //回収
00116        pmove(targetx[8],targety[8]);
00117        //回収
00118        close_hand();
00119        pmove2(150,team*(900+slidedist));//斜め移動
00120        
00121        turnrad_ccw(PI,team);
00122        open_arm();
00123        open_hand();
00124        back300();
00125     
00126        
00127        pmove2(700,(600+slidedist));//ハブ
00128 
00129        pmove(targetx[5],targety[5]);
00130        close_arm();
00131        close_hand();
00132        nxback300();
00133        
00134        pmove2(150,team*(900+slidedist));
00135        open_arm();
00136        open_hand();
00137        
00138         back300();
00139         while(1){
00140             buzzer(1);
00141             wait(0.5);
00142             buzzer(0);
00143             wait(1);   
00144         }
00145     }
00146     
00147     
00148     if(phase==1){
00149         
00150      pmove(targetx[7],targety[7]);
00151      back300();
00152      pmove2(goalx,goaly);
00153      turnrad_ccw(PI,team);
00154      open_arm();
00155      open_hand();
00156      back300();
00157      
00158      pmove2(900,600+slidedist);
00159      
00160         pmove(targetx[5],targety[5]);
00161         pmove2(goalx,goaly);
00162         turnrad_ccw(2*PI,team);
00163         open_arm();
00164         open_hand();
00165         back300();
00166         
00167     }
00168     
00169 }