main.cpp

00001 #include "mbed.h"
00002 #include "locate.h"
00003 #include "math.h"
00004 PwmOut M1cw(PA_11);
00005 PwmOut M1ccw(PB_15);
00006 PwmOut M2ccw(PB_14);
00007 PwmOut M2cw(PB_13);
00008 
00009 DigitalIn start(USER_BUTTON);
00010 
00011 DigitalIn teamSW(PC_11);
00012 DigitalOut teamledblue(PC_10);
00013 DigitalOut teamledred(PC_12);
00014 
00015 DigitalIn phase1(PA_5);
00016 DigitalIn phase2(PA_6);
00017 DigitalIn phase4(PA_7);
00018 DigitalIn phase8(PB_12);
00019 
00020 const int allowlength=5;
00021 const float allowdegree=0.02;
00022 const int rightspeed=30*2-2;
00023 const int leftspeed=30*2+1;
00024 const int turnspeed=15*2;
00025 
00026 const float PIfact=2.8;
00027 const float PIblue=2.8;
00028 int phaseSW(void){
00029     
00030     
00031     phase1.mode(PullUp);
00032     phase2.mode(PullUp);
00033     phase4.mode(PullUp);
00034     phase8.mode(PullUp);    
00035     
00036     int SW=phase1+2*phase2+4*phase4+8*phase8;
00037     pc.printf("%d\n\r",SW);
00038     return SW;
00039 }
00040 void initmotor()
00041 {
00042 
00043 
00044     M1cw.period_us(256);
00045     M1ccw.period_us(256);
00046     M2cw.period_us(256);
00047     M2ccw.period_us(256);
00048 
00049 }
00050 
00051 void move(int left,int right)
00052 {
00053 
00054     float rightduty,leftduty;
00055 
00056     if(right>256) {
00057         right=256;
00058     }
00059     if(left>256) {
00060         left=256;
00061     }
00062     if(right<-256) {
00063         right=-256;
00064     }
00065     if(left<-256) {
00066         left=-256;
00067     }
00068 
00069     rightduty=right/256.0;
00070     leftduty=left/256.0;
00071     if(right>0) {
00072         M1cw.write(1-rightduty);
00073         M1ccw.write(1);
00074     } else {
00075         M1cw.write(1);
00076         M1ccw.write(1+rightduty);
00077     }
00078 
00079     if(left>0) {
00080         M2cw.write(1-leftduty);
00081         M2ccw.write(1);
00082     } else {
00083         M2cw.write(1);
00084         M2ccw.write(1+leftduty);
00085     }
00086 }
00087 
00088 
00089 void movelength(int length)
00090 {
00091     int px,py,pt;
00092     update();
00093     px=coordinateX();
00094     py=coordinateY();
00095     pt=coordinateTheta();
00096 
00097     move(rightspeed,leftspeed);
00098     while(1) {
00099 
00100         update();
00101         //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
00102         if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
00103             break;
00104         }
00105 
00106     }
00107     move(0,0);
00108 }
00109 
00110 void turncw()
00111 {
00112     float pt;
00113     move((-1)*turnspeed,turnspeed);
00114 
00115     update();
00116     pt=coordinateTheta();
00117 
00118     while(1) {
00119         update();
00120         if(pt-coordinateTheta()>PIfact/2) {
00121             move(0,0);
00122             break;
00123         }
00124     }
00125 }
00126 
00127 void turnccw()
00128 {
00129     float pt;
00130     move(turnspeed,(-1)*turnspeed);
00131 
00132     update();
00133     pt=coordinateTheta();
00134 
00135     while(1) {
00136         update();
00137         if(pt-coordinateTheta()<(-1)*PIfact/2) {
00138             move(0,0);
00139             break;
00140         }
00141     }
00142 }
00143 
00144 void turnccw80()
00145 {   float GODRAD=1.2;
00146     float pt;
00147     move(turnspeed,(-1)*turnspeed);
00148 
00149     update();
00150     pt=coordinateTheta();
00151 
00152     while(1) {
00153         update();
00154         if(pt-coordinateTheta()<(-1)*GODRAD) {
00155             move(0,0);
00156             break;
00157         }
00158     }
00159 }
00160 
00161 void turncw80()
00162 {   float GODRAD=1.2;
00163     float pt;
00164     move((-1)*turnspeed,turnspeed);
00165 
00166     update();
00167     pt=coordinateTheta();
00168 
00169     while(1) {
00170         update();
00171         if(pt-coordinateTheta()>GODRAD) {
00172             move(0,0);
00173             break;
00174         }
00175     }
00176 }
00177 
00178 void pmovex(int length){
00179     int px,py,dx,dy;
00180     int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
00181     update();
00182     px=coordinateX();
00183     py=coordinateY();
00184     move(rightspeed,leftspeed);
00185         
00186     while(1){
00187         update();
00188         dx=coordinateX()-px;
00189         dy=coordinateY()-py;
00190 
00191         if(dy>7){dy=7;}
00192         if(dy<-7){dy=-7;}
00193         move(rightspeed-k*dy,leftspeed+k*dy);
00194         
00195         if(dx>length){
00196             move(0,0);
00197             break;
00198         }
00199     }
00200 }
00201 
00202 int teamLED()
00203 {
00204     teamSW.mode(PullUp);
00205     if(teamSW) {
00206         teamledblue=1;
00207         teamledred=0;
00208         return -1;
00209     } else {
00210         teamledblue=0;
00211         teamledred=1;
00212         return 1;
00213     }
00214 }
00215 
00216 
00217 
00218 
00219 int main(){
00220     int team=1,phase=0,comand,buf;
00221     setup();
00222     initmotor();
00223     team=teamLED();
00224     phase=phaseSW();
00225     while(start);
00226     if(team>0){
00227         
00228         if(phase==0){
00229             movelength(600);
00230             turnccw();
00231             movelength(900);
00232             turnccw80();
00233             movelength(600-50);
00234             wait(30);
00235         }
00236         
00237         if(phase==1){
00238             movelength(900);
00239             turnccw();
00240             movelength(900);
00241             turnccw80();
00242             movelength(900-50);
00243             wait(30);
00244         }
00245 
00246         if(phase==2){
00247             movelength(1200);
00248             turnccw();
00249             movelength(700);
00250             turnccw80();
00251             movelength(1200-100);
00252             wait(30);
00253         }
00254         
00255         if(phase==3){
00256             movelength(1200);
00257             turnccw();
00258             movelength(1000);
00259             turnccw80();
00260             movelength(1200-100);
00261             wait(30);
00262         }
00263         
00264         if(phase==15){
00265             buf=15;
00266             while(buf>=phaseSW()){
00267                 buf=phaseSW();
00268                 }
00269                 //スイッチを上昇させると、上昇前で確定
00270             //この時点でbufに現在のスイッチの状態がはいる。
00271               pc.printf("Hello World !%d\n",buf);
00272             movelength(300+75*buf);
00273             turnccw();
00274             movelength(1200);
00275             turnccw80();
00276             movelength(300+75*buf);
00277             wait(30);
00278         }
00279     }
00280 
00281 if(team<0){
00282         
00283         if(phase==0){
00284             movelength(600);
00285             turncw80();
00286             movelength(900);
00287             turncw80();
00288             movelength(600-50);
00289             wait(30);
00290         }
00291         
00292         if(phase==1){
00293             movelength(900);
00294             turncw80();
00295             movelength(900);
00296             turncw80();
00297             movelength(900-50);
00298             wait(30);
00299         }
00300 
00301         if(phase==2){
00302             movelength(1200-50);
00303             turncw80();
00304             movelength(600);
00305             turncw80();
00306             movelength(1200-100);
00307             wait(30);
00308         }
00309         
00310         if(phase==3){
00311             movelength(1200-50);
00312             turncw80();
00313             movelength(1000);
00314             turncw80();
00315             movelength(1200-100);
00316             wait(30);
00317         }
00318         
00319         if(phase==15){
00320             buf=15;
00321             while(buf>=phaseSW()){
00322                 buf=phaseSW();
00323                 }
00324             
00325                 //スイッチを上昇させると、上昇前で確定
00326             //この時点でbufに現在のスイッチの状態がはいる。
00327             
00328             movelength(300+75*buf);
00329             turncw();
00330             movelength(1200);
00331             turncw80();
00332             movelength(300+75*buf);
00333             wait(30);
00334         }
00335     }
00336 
00337 }