a
Fork of ARAI45th by
Embed:
(wiki syntax)
Show/hide line numbers
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制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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 }
Generated on Fri Jul 15 2022 14:59:36 by
1.7.2
