Collin Alexis
Code FRC 2018
Embed:
(wiki syntax)
Show/hide line numbers
robot_general.cpp
00001 #include "robot_general.h" 00002 00003 DigitalIn Daniels_in(PA_15); 00004 00005 //DigitalIn UBp(USER_BUTTON); 00006 InterruptIn UBp_it(USER_BUTTON); 00007 Timeout ubp_to; 00008 00009 //Serial dbug(USBTX, USBRX, 115200); 00010 Serial dbug(PA_0, PA_1, 38400); 00011 00012 Timer timer_generale; 00013 00014 int flagUBp = 0, flagDaniels = 0, match_en_cours = 0; 00015 int flagEv = 0, flagAv = 1, flagDpl = 0, flag_auto_suiv = 0; 00016 int sensrotation=-1; 00017 00018 short pos_balle[10][4];int nb_balle = 0, balle_retrouve = 0; 00019 00020 00021 00022 unsigned int temps_ecoule = 0; 00023 00024 void isr_UBp();void user_bp_ar(); 00025 00026 enum etat_visee_balle_T {init_visee_balle, fin_vir_rapide, debut_vir_lent, fin_vir_lent, avancer_20,debut_vir_lent2, fin_vir_lent2,avancer,rotation, lancer, fin_visee_balle, error_visee_balle, D, ED, Perdu, debut_vir_retour, fin_vir_rapide_odo,M, balle_disparue, recalage, recalage3, recalage2, retour}; 00027 enum etat_visee_balle_T etat_visee_balle = init_visee_balle , etat_visee_balle_prec = Perdu; 00028 int timer_visee_balle=0; 00029 void automate_visee_balle(); 00030 00031 enum etat_pied_ballon_T {init_pied_ballon_dierct, avance_x_DT_pb, rotation_ballon_x_pb, reculer_ballon_x_pb, recalage_pied_pb,fin_pied_ballon, error_pied_ballon}; 00032 enum etat_pied_ballon_T etat_pied_ballon = init_pied_ballon_dierct , etat_pied_ballon_prec = fin_pied_ballon; 00033 int timer_pied_ballon = 0; 00034 void automate_pied_ballon(); 00035 00036 enum etat_macro_auto_T {init_macro_auto}; 00037 enum etat_macro_auto_T etat_macro_auto = init_macro_auto , etat_macro_auto_prec = init_macro_auto; 00038 int timer_macro_auto = 0; 00039 void automate_macro_auto(); 00040 00041 enum etat_tirer_balle_complet_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; 00042 enum etat_tirer_balle_complet_T etat_tirer_balle_complet = init_tirer_balle_complet , etat_tirer_balle_complet_prec = init_tirer_balle_complet; 00043 int timer_tirer_balle_complet = 0; 00044 void automate_tirer_balle_complet(); 00045 00046 enum etat_3balles_odo_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; 00047 enum etat_3balles_odo_T etat_3balles_odo = init_tirer_balle_complet , etat_3balles_odo = init_tirer_balle_complet; 00048 int timer_3balles_odo = 0; 00049 void automate_3balles_odo(); 00050 00051 00052 void automate_base(); 00053 int etat_auto = 0, etat_auto_prec = 0; 00054 int etat_auto_base = 0, etat_auto_base_prec = 0; 00055 00056 void if_UBp(); 00057 void wait_debut_de_partie(); 00058 00059 00060 00061 int main() 00062 { 00063 dbug.printf("\nSETUP "); 00064 can_init(); 00065 UBp_it.rise(&isr_UBp); 00066 //SetOdometrie(26,500,55,0); 00067 bonus_findepartie(0); 00068 00069 cmd_servo = 0; 00070 ejection_continue = 1; 00071 ejection_active = 1; 00072 temps_ecoule = 0; 00073 match_en_cours = 0; 00074 00075 //BendRadius(139, 900, -1,0); 00076 //GoStraight(3800,0,0,0); 00077 dbug.printf("END SETUP\n"); 00078 while(!match_en_cours){ 00079 trait_can(); 00080 wait_debut_de_partie();} 00081 00082 timer_generale.start(); 00083 //Rotate(3600); 00084 //BendRadius(139, -900, -1,0); 00085 //while(1); 00086 //dbug.printf("%d\n", flagDaniels); 00087 00088 while(1) 00089 { 00090 if(temps_ecoule > DUREE_MATCH_TIC) 00091 { 00092 match_en_cours = 0; 00093 SendRawId(ASSERVISSEMENT_STOP); 00094 ejection_active = 0; 00095 automate_ejecteur(); 00096 wait(6); 00097 bonus_findepartie(1); 00098 while(1); 00099 } 00100 else if(timer_generale.read_us() > TIKER_GENERALE_US) 00101 {timer_generale.reset();temps_ecoule++; 00102 trait_can(); 00103 f_mesure(); 00104 pos_disquette(); 00105 00106 //if_UBp(); 00107 //automate_base(); 00108 automate_pied_ballon(); 00109 automate_visee_balle(); 00110 00111 automate_ejecteur(); 00112 if(((temps_ecoule%500) == 0))// execute chaques secondes 00113 { 00114 dbug.printf("Pos x:%d, y:%d, te:%d, Tecoule:%d\n", pos[0], pos[1], pos[2],temps_ecoule*TIKER_GENERALE_US/1000000); 00115 //dbug.printf("Dis D:%f, G:%f, min:%f\n", DTD_avg_Ex, DTG_avg_Ex, DT_avg_min_Ex); 00116 } 00117 } 00118 00119 } 00120 } 00121 00122 void automate_macro_auto() 00123 { 00124 timer_macro_auto++; 00125 switch(etat_macro_auto) 00126 { 00127 case init_macro_auto: 00128 etat_macro_auto = init_macro_auto; timer_macro_auto = 0; 00129 break; 00130 } 00131 } 00132 00133 void automate_3balles_odo() 00134 { 00135 timer_3balles_odot++; 00136 switch(etat_3balles_odo) 00137 { 00138 case 1: 00139 ejection_active = 1; 00140 ejection_continue = 1; 00141 GoStraight(1805,0,0,0); 00142 etat_3balles_odo = 2; timer_3balles_odot = 0; 00143 break; 00144 00145 case 2 : 00146 if(flagFinDpl) 00147 {flagFinDpl = 0; 00148 flagDaniels = 1; 00149 Rotate(-930);//rotation acoté de la balle 00150 etat_3balles_odo = pied_ballon_tbc; timer_3balles_odot = 0; 00151 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00152 break; 00153 00154 case 3 : 00155 if(flagFinDpl) 00156 {flagFinDpl = 0; 00157 ejection_continue = 0; 00158 GoStraight(1500,0,0,0);//on va vers la deuxieme balle 00159 etat_3balles_odo = 4; timer_3balles_odot = 0; 00160 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00161 break; 00162 00163 case 4 : 00164 if(flagFinDpl) 00165 {flagFinDpl = 0; 00166 Rotate(940);//deuxieme balle gobé ,on tourne 00167 etat_3balles_odo = 5; timer_3balles_odot = 0; 00168 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00169 break; 00170 00171 case 5 : 00172 if(flagFinDpl) 00173 {flagFinDpl = 0; 00174 envoyer_balle = 1;ejection_continue = 1;//tir la deuxieme balle 00175 flagDaniels = 1; 00176 Rotate(-940);//ici implementer le deuxieme ciblage laser 00177 etat_3balles_odo = 6; timer_3balles_odot = 0; 00178 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00179 break; 00180 00181 case 6: 00182 if(flagFinDpl) 00183 {flagFinDpl = 0; 00184 ejection_continue = 0; 00185 GoStraight(1500,0,0,0); 00186 etat_3balles_odo = 7; timer_3balles_odot = 0; 00187 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00188 break; 00189 00190 case 7: 00191 if(flagFinDpl) 00192 {flagFinDpl = 0; 00193 Rotate(930); 00194 etat_3balles_odo = 8; timer_3balles_odot = 0; 00195 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00196 break; 00197 00198 case 8: 00199 if(flagFinDpl) 00200 {flagFinDpl = 0; 00201 envoyer_balle = 1;ejection_continue = 1;//envoie de la troisième balle 00202 etat_3balles_odo = 9; timer_3balles_odot = 0; 00203 sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite 00204 }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; 00205 break; 00206 00207 case 9: 00208 break; 00209 00210 case 10: 00211 break; 00212 } 00213 } 00214 00215 00216 00217 void automate_tirer_balle_complet() 00218 { 00219 timer_tirer_balle_complet++; 00220 switch(etat_tirer_balle_complet) 00221 { 00222 case init_tirer_balle_complet: 00223 sensrotation = -sensrotation; 00224 etat_pied_ballon = init_pied_ballon_dierct; 00225 etat_tirer_balle_complet = pied_ballon_tbc; timer_tirer_balle_complet = 0; 00226 break; 00227 00228 case pied_ballon_tbc: 00229 automate_pied_ballon(); 00230 if(etat_pied_ballon == fin_pied_ballon) 00231 { 00232 etat_visee_balle = init_visee_balle; 00233 etat_tirer_balle_complet = visee_balle_tbc; timer_tirer_balle_complet = 0; 00234 } 00235 break; 00236 00237 case visee_balle_tbc: 00238 automate_visee_balle(); 00239 if(etat_visee_balle == fin_visee_balle) 00240 { 00241 //etat_visee_balle = init_visee_balle; 00242 etat_tirer_balle_complet = fin_tirer_balle_complet; timer_tirer_balle_complet = 0; 00243 } 00244 else if(etat_visee_balle == error_visee_balle) 00245 { 00246 //etat_visee_balle = init_visee_balle; 00247 etat_tirer_balle_complet = error_tirer_balle_complet; timer_tirer_balle_complet = 0; 00248 } 00249 break; 00250 00251 case fin_tirer_balle_complet: 00252 break; 00253 00254 case error_tirer_balle_complet: 00255 break; 00256 00257 00258 } 00259 } 00260 00261 void automate_pied_ballon() 00262 { 00263 timer_pied_ballon++; 00264 switch(etat_pied_ballon) 00265 { 00266 case init_pied_ballon_dierct: 00267 short y_temp; 00268 if(sensrotation==1)y_temp = 300; 00269 else y_temp = 3700; 00270 GoToPosition(3500, y_temp, 0, 0); 00271 etat_pied_ballon = avance_x_DT_pb; timer_pied_ballon = 0; 00272 break; 00273 00274 case avance_x_DT_pb: 00275 if(flagFinDpl) 00276 {flagFinDpl = 0; 00277 GoStraight(3050-DTA_avg_Ex,0,0,0); 00278 //SetOdometrie(26,pos[0],y_temp,sensrotation*900); 00279 etat_pied_ballon = rotation_ballon_x_pb; timer_pied_ballon = 0; 00280 }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; 00281 break; 00282 00283 case rotation_ballon_x_pb: 00284 if(flagFinDpl) 00285 {flagFinDpl = 0; 00286 if(DTA_avg_Ex<3070 && DTA_avg_Ex>3020){ 00287 Rotate(sensrotation*900); 00288 etat_pied_ballon = reculer_ballon_x_pb; timer_pied_ballon = 0; 00289 } 00290 }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; 00291 break; 00292 00293 case reculer_ballon_x_pb: 00294 if(flagFinDpl) 00295 {flagFinDpl = 0; 00296 GoStraight(-250,0,0,0); 00297 etat_pied_ballon = recalage_pied_pb; timer_pied_ballon = 0; 00298 }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; 00299 break; 00300 00301 case recalage_pied_pb: 00302 if(flagFinDpl) 00303 {flagFinDpl = 0; 00304 short y_temp; 00305 if(sensrotation==1)y_temp = 55; 00306 else y_temp = 3945; 00307 SetOdometrie(26,3050,y_temp,sensrotation*900); 00308 etat_pied_ballon = fin_pied_ballon; timer_pied_ballon = 0; 00309 }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; 00310 break; 00311 00312 case fin_pied_ballon: 00313 break; 00314 00315 case error_pied_ballon: 00316 break; 00317 } 00318 if(etat_pied_ballon != etat_pied_ballon_prec)dbug.printf("Etat visee_balle : %d\n",etat_pied_ballon); 00319 etat_pied_ballon_prec = etat_pied_ballon; 00320 } 00321 00322 00323 00324 void automate_visee_balle() 00325 { 00326 timer_visee_balle++; 00327 00328 switch(etat_visee_balle) 00329 { 00330 case init_visee_balle ://--------------------------------------0 00331 SendSpeed(80, 600); 00332 Rotate(sensrotation*900); 00333 etat_visee_balle = fin_vir_rapide; 00334 timer_visee_balle=0; 00335 break; 00336 00337 case fin_vir_rapide : //--------------------------------------1 00338 if(balleg || balled ) 00339 {flagFinDpl = 0; 00340 dbug.printf("balle 1-----1\n"); 00341 pos_balle[0][2] = pos[2]; 00342 SendRawId(ASSERVISSEMENT_STOP); 00343 timer_visee_balle = 0; 00344 etat_visee_balle = debut_vir_lent; 00345 } else if(flagFinDpl || timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00346 break; 00347 00348 case debut_vir_lent://--------------------------------------2 00349 if(flagFinDpl) 00350 {flagFinDpl = 0; 00351 dbug.printf("balle 2-----2\n"); 00352 SendSpeed(18, 300); 00353 Rotate((pos_balle[0][2]-pos[2])*2); 00354 etat_visee_balle = fin_vir_lent; 00355 }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00356 break; 00357 00358 case balle_disparue: 00359 break; 00360 00361 case fin_vir_lent://--------------------------------------3 00362 if(balleg && balled) 00363 {flagFinDpl = 0; 00364 pos_balle[0][2] = pos[2]; 00365 SendRawId(ASSERVISSEMENT_STOP); 00366 SendSpeed(100, 600); 00367 if(DT_avg_min_Ex<2000)etat_visee_balle = avancer; 00368 else etat_visee_balle = avancer_20; 00369 timer_visee_balle = 0; 00370 }else{ 00371 etat_visee_balle = debut_vir_lent; 00372 flagFinDpl = 1; 00373 } 00374 break; 00375 00376 00377 case avancer_20://--------------------------------------4 00378 if(flagFinDpl) 00379 {flagFinDpl = 0; 00380 GoStraight(DT_avg_min_Ex-D_ROULEAU_AXE-500,0,0,0); 00381 etat_visee_balle = debut_vir_lent2; 00382 timer_visee_balle = 0; 00383 }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00384 break; 00385 00386 case debut_vir_lent2://--------------------------------------5 00387 if(flagFinDpl) 00388 {flagFinDpl = 0; 00389 00390 SendSpeed(18, 300); 00391 00392 if(!balleg && !balled) 00393 { 00394 etat_visee_balle = error_visee_balle; 00395 } 00396 else{ 00397 if(!balleg && balled) 00398 { 00399 BendRadius(139, sensrotation*30, sensrotation,0); 00400 etat_visee_balle = fin_vir_lent2; 00401 } 00402 else if(balleg && !balled) 00403 { 00404 BendRadius(139, 30, sensrotation,0); 00405 etat_visee_balle = fin_vir_lent2; 00406 } 00407 else if(balleg && balled) 00408 { 00409 flagFinDpl = 1; 00410 SendSpeed(100, 600); 00411 etat_visee_balle = avancer; 00412 timer_visee_balle = 0; 00413 } 00414 } 00415 }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00416 break; 00417 00418 case fin_vir_lent2://--------------------------------------6 00419 if(balleg && balled) 00420 {flagFinDpl = 0; 00421 SendRawId(ASSERVISSEMENT_STOP); 00422 SendSpeed(100, 600); 00423 etat_visee_balle = avancer; 00424 timer_visee_balle = 0; 00425 }else{flagFinDpl = 1; 00426 etat_visee_balle = debut_vir_lent2; 00427 } 00428 break; 00429 00430 00431 case avancer://--------------------------------------7 00432 if(flagFinDpl) 00433 {flagFinDpl = 0; 00434 ejection_continue = 0; 00435 GoStraight(DT_avg_min_Ex-(D_ROULEAU_AXE),0,0,0); 00436 etat_visee_balle = rotation; 00437 } 00438 if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00439 break; 00440 00441 case rotation://--------------------------------------8 00442 if(flagFinDpl) 00443 {flagFinDpl = 0; 00444 if(balle_gobbee) 00445 { 00446 short dTeta = -pos[2]; 00447 Rotate(dTeta); 00448 etat_visee_balle = lancer; 00449 timer_visee_balle = 0; 00450 }else etat_visee_balle = error_visee_balle; 00451 } 00452 break; 00453 00454 case lancer: 00455 if(flagFinDpl) 00456 {flagFinDpl = 0; 00457 envoyer_balle = 1; 00458 ejection_continue = 1; 00459 etat_visee_balle = fin_visee_balle; 00460 00461 } if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00462 break; 00463 00464 case fin_visee_balle: 00465 ejection_continue = 1; 00466 break; 00467 case error_visee_balle: 00468 ejection_continue = 1; 00469 break; 00470 00471 case retour : 00472 GoToPosition(300, 0, 0, 0); 00473 etat_visee_balle = recalage; 00474 timer_visee_balle = 0; 00475 break; 00476 00477 case recalage : 00478 if(flagFinDpl) 00479 {flagFinDpl = 0; 00480 GoStraight(-295,0,0,0); 00481 etat_visee_balle = recalage2; 00482 timer_visee_balle = 0; 00483 }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00484 break; 00485 case recalage2 : 00486 if(flagFinDpl) 00487 {flagFinDpl = 0; 00488 GoStraight(30,2,50,0); 00489 etat_visee_balle = recalage2; 00490 timer_visee_balle = 0; 00491 }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; 00492 break; 00493 case recalage3 : 00494 if(flagFinDpl) 00495 {flagFinDpl = 0; 00496 etat_visee_balle = init_visee_balle; 00497 } 00498 break; 00499 } 00500 if(etat_visee_balle != etat_visee_balle_prec)dbug.printf("Etat visee_balle : %d\n",etat_visee_balle); 00501 etat_visee_balle_prec = etat_visee_balle; 00502 } 00503 00504 void automate_base() 00505 { 00506 switch(etat_auto_base) 00507 { 00508 case 0 : 00509 temps_ecoule = 0; 00510 etat_auto_base = 1; 00511 break; 00512 00513 case 1 : 00514 ejection_active = 1; 00515 ejection_continue = 1; 00516 GoStraight(1805,0,0,0); 00517 etat_auto_base=2; 00518 break; 00519 00520 case 2 : 00521 if(flagFinDpl) 00522 {flagFinDpl = 0; 00523 flagDaniels = 1; 00524 Rotate(-850);//rotation acoté de la balle 00525 etat_auto_base=201; 00526 00527 } 00528 break; 00529 00530 case 201 : 00531 if(flagFinDpl) 00532 {flagFinDpl = 0; 00533 sensrotation=-1; 00534 etat_visee_balle = init_visee_balle; 00535 etat_auto_base=202; 00536 } 00537 break; 00538 00539 case 202 : 00540 automate_visee_balle(); //ciblage vers la gauche 00541 if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle 00542 { 00543 etat_visee_balle = init_visee_balle; 00544 etat_auto_base = 3; 00545 } 00546 break; 00547 //---------------------FIN--------------------------- 00548 00549 case 3 : 00550 ejection_continue = 0; 00551 GoStraight(1500,0,0,0);//on va vers la deuxieme balle 00552 etat_auto_base=4; 00553 break; 00554 00555 case 4 : 00556 if(flagFinDpl) 00557 {flagFinDpl = 0; 00558 Rotate(900);//deuxieme balle gobé ,on tourne 00559 etat_auto_base=5; 00560 } 00561 break; 00562 00563 case 5 : 00564 if(flagFinDpl) 00565 {flagFinDpl = 0; 00566 envoyer_balle = 1;//tir la deuxieme balle 00567 flagDaniels = 1; 00568 Rotate(-850);//ici implementer le deuxieme ciblage laser 00569 etat_auto_base=601; 00570 } 00571 break; 00572 00573 case 601 : 00574 if(flagFinDpl) 00575 {flagFinDpl = 0; 00576 sensrotation=-1; 00577 etat_visee_balle = init_visee_balle; 00578 etat_auto_base=602; 00579 } 00580 break; 00581 00582 case 602 : 00583 automate_visee_balle(); //ciblage vers la gauche 00584 if(etat_visee_balle == 4)//on attend d'etre sur la balle 00585 { 00586 etat_visee_balle = init_visee_balle; 00587 etat_auto_base = 7; 00588 } 00589 break; 00590 00591 case 7: 00592 ejection_continue = 0; 00593 GoStraight(1500,0,0,0); 00594 etat_auto_base=8; 00595 break; 00596 00597 case 8 : 00598 if(flagFinDpl) 00599 {flagFinDpl = 0; 00600 Rotate(900); 00601 etat_auto_base=9; 00602 } 00603 break; 00604 00605 case 9 : 00606 if(flagFinDpl) 00607 {flagFinDpl = 0; 00608 envoyer_balle = 1;//envoie de la troisième balle 00609 etat_auto_base=10; 00610 sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite 00611 } 00612 break; 00613 00614 case 10: 00615 GoStraight(1500,0,0,0); 00616 etat_auto_base=11; 00617 break; 00618 00619 case 11 : 00620 if(flagFinDpl) 00621 {flagFinDpl = 0; 00622 Rotate(900); 00623 //cmd_servo = 2; 00624 etat_auto_base=12; 00625 } 00626 break; 00627 00628 case 12 : 00629 if(flagFinDpl) 00630 {flagFinDpl = 0; 00631 GoStraight(100,2,0,0); 00632 cmd_servo = 2; 00633 etat_auto_base=13; 00634 } 00635 break; 00636 case 13 : 00637 if(flagFinDpl) 00638 {flagFinDpl = 0; 00639 GoStraight(3800,0,0,0); 00640 cmd_servo = 1; 00641 etat_auto_base=14; 00642 } 00643 break; 00644 00645 case 14 : 00646 if(flagFinDpl) 00647 {flagFinDpl = 0; 00648 GoStraight(-500,0,0,0); 00649 etat_auto_base=15; 00650 } 00651 break; 00652 00653 case 15 : 00654 if(flagFinDpl) 00655 {flagFinDpl = 0; 00656 Rotate(1800); 00657 cmd_servo = 0; 00658 etat_auto_base=16; 00659 sensrotation=1; 00660 } 00661 break; 00662 case 16 : 00663 00664 break; 00665 00666 00667 00668 case 100 : 00669 etat_visee_balle = init_visee_balle;//On passe dans l'automate de ciblage 00670 etat_auto_base=101; 00671 break; 00672 00673 case 101 : 00674 automate_visee_balle(); //ciblage vers la gauche 00675 if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle 00676 { 00677 etat_visee_balle = init_visee_balle; 00678 etat_auto_base = 102; 00679 } 00680 break; 00681 00682 case 102 : 00683 if(flagFinDpl) 00684 {flagFinDpl = 0; 00685 ejection_continue = 0; 00686 GoStraight(DTD_avg_Ex,0,0,0); 00687 etat_auto_base=103; 00688 } 00689 break; 00690 00691 case 103 : 00692 if(flagFinDpl) 00693 {flagFinDpl = 0; 00694 Rotate(-pos[2]); 00695 etat_auto_base=100; 00696 } 00697 break; 00698 default : 00699 00700 break; 00701 00702 } 00703 if(etat_auto_base != etat_auto_base_prec)dbug.printf("Etat auto_base: %d\n",etat_auto_base); 00704 etat_auto_base_prec = etat_auto_base; 00705 } 00706 00707 void wait_debut_de_partie() 00708 { 00709 if(!Daniels_in) 00710 { 00711 dbug.printf("-----DEBUT DE PARTIE------\n"); 00712 match_en_cours = 1; 00713 } 00714 } 00715 00716 void user_bp_ar(){UBp_it.enable_irq();} 00717 void isr_UBp() 00718 { 00719 UBp_it.disable_irq();dbug.printf("ubp\n"); 00720 ubp_to.attach(&user_bp_ar, 0.3); 00721 flagUBp = 1; 00722 }
Generated on Wed Aug 17 2022 22:52:28 by
1.7.2