Collin Alexis
/
Code_FRC_2018
Code FRC 2018
robot_general.cpp@0:aebeb20b201a, 2019-03-21 (annotated)
- Committer:
- AlexisCollin
- Date:
- Thu Mar 21 14:59:13 2019 +0000
- Revision:
- 0:aebeb20b201a
code de l'an dernier
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AlexisCollin | 0:aebeb20b201a | 1 | #include "robot_general.h" |
AlexisCollin | 0:aebeb20b201a | 2 | |
AlexisCollin | 0:aebeb20b201a | 3 | DigitalIn Daniels_in(PA_15); |
AlexisCollin | 0:aebeb20b201a | 4 | |
AlexisCollin | 0:aebeb20b201a | 5 | //DigitalIn UBp(USER_BUTTON); |
AlexisCollin | 0:aebeb20b201a | 6 | InterruptIn UBp_it(USER_BUTTON); |
AlexisCollin | 0:aebeb20b201a | 7 | Timeout ubp_to; |
AlexisCollin | 0:aebeb20b201a | 8 | |
AlexisCollin | 0:aebeb20b201a | 9 | //Serial dbug(USBTX, USBRX, 115200); |
AlexisCollin | 0:aebeb20b201a | 10 | Serial dbug(PA_0, PA_1, 38400); |
AlexisCollin | 0:aebeb20b201a | 11 | |
AlexisCollin | 0:aebeb20b201a | 12 | Timer timer_generale; |
AlexisCollin | 0:aebeb20b201a | 13 | |
AlexisCollin | 0:aebeb20b201a | 14 | int flagUBp = 0, flagDaniels = 0, match_en_cours = 0; |
AlexisCollin | 0:aebeb20b201a | 15 | int flagEv = 0, flagAv = 1, flagDpl = 0, flag_auto_suiv = 0; |
AlexisCollin | 0:aebeb20b201a | 16 | int sensrotation=-1; |
AlexisCollin | 0:aebeb20b201a | 17 | |
AlexisCollin | 0:aebeb20b201a | 18 | short pos_balle[10][4];int nb_balle = 0, balle_retrouve = 0; |
AlexisCollin | 0:aebeb20b201a | 19 | |
AlexisCollin | 0:aebeb20b201a | 20 | |
AlexisCollin | 0:aebeb20b201a | 21 | |
AlexisCollin | 0:aebeb20b201a | 22 | unsigned int temps_ecoule = 0; |
AlexisCollin | 0:aebeb20b201a | 23 | |
AlexisCollin | 0:aebeb20b201a | 24 | void isr_UBp();void user_bp_ar(); |
AlexisCollin | 0:aebeb20b201a | 25 | |
AlexisCollin | 0:aebeb20b201a | 26 | 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}; |
AlexisCollin | 0:aebeb20b201a | 27 | enum etat_visee_balle_T etat_visee_balle = init_visee_balle , etat_visee_balle_prec = Perdu; |
AlexisCollin | 0:aebeb20b201a | 28 | int timer_visee_balle=0; |
AlexisCollin | 0:aebeb20b201a | 29 | void automate_visee_balle(); |
AlexisCollin | 0:aebeb20b201a | 30 | |
AlexisCollin | 0:aebeb20b201a | 31 | 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}; |
AlexisCollin | 0:aebeb20b201a | 32 | enum etat_pied_ballon_T etat_pied_ballon = init_pied_ballon_dierct , etat_pied_ballon_prec = fin_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 33 | int timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 34 | void automate_pied_ballon(); |
AlexisCollin | 0:aebeb20b201a | 35 | |
AlexisCollin | 0:aebeb20b201a | 36 | enum etat_macro_auto_T {init_macro_auto}; |
AlexisCollin | 0:aebeb20b201a | 37 | enum etat_macro_auto_T etat_macro_auto = init_macro_auto , etat_macro_auto_prec = init_macro_auto; |
AlexisCollin | 0:aebeb20b201a | 38 | int timer_macro_auto = 0; |
AlexisCollin | 0:aebeb20b201a | 39 | void automate_macro_auto(); |
AlexisCollin | 0:aebeb20b201a | 40 | |
AlexisCollin | 0:aebeb20b201a | 41 | enum etat_tirer_balle_complet_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; |
AlexisCollin | 0:aebeb20b201a | 42 | enum etat_tirer_balle_complet_T etat_tirer_balle_complet = init_tirer_balle_complet , etat_tirer_balle_complet_prec = init_tirer_balle_complet; |
AlexisCollin | 0:aebeb20b201a | 43 | int timer_tirer_balle_complet = 0; |
AlexisCollin | 0:aebeb20b201a | 44 | void automate_tirer_balle_complet(); |
AlexisCollin | 0:aebeb20b201a | 45 | |
AlexisCollin | 0:aebeb20b201a | 46 | enum etat_3balles_odo_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; |
AlexisCollin | 0:aebeb20b201a | 47 | enum etat_3balles_odo_T etat_3balles_odo = init_tirer_balle_complet , etat_3balles_odo = init_tirer_balle_complet; |
AlexisCollin | 0:aebeb20b201a | 48 | int timer_3balles_odo = 0; |
AlexisCollin | 0:aebeb20b201a | 49 | void automate_3balles_odo(); |
AlexisCollin | 0:aebeb20b201a | 50 | |
AlexisCollin | 0:aebeb20b201a | 51 | |
AlexisCollin | 0:aebeb20b201a | 52 | void automate_base(); |
AlexisCollin | 0:aebeb20b201a | 53 | int etat_auto = 0, etat_auto_prec = 0; |
AlexisCollin | 0:aebeb20b201a | 54 | int etat_auto_base = 0, etat_auto_base_prec = 0; |
AlexisCollin | 0:aebeb20b201a | 55 | |
AlexisCollin | 0:aebeb20b201a | 56 | void if_UBp(); |
AlexisCollin | 0:aebeb20b201a | 57 | void wait_debut_de_partie(); |
AlexisCollin | 0:aebeb20b201a | 58 | |
AlexisCollin | 0:aebeb20b201a | 59 | |
AlexisCollin | 0:aebeb20b201a | 60 | |
AlexisCollin | 0:aebeb20b201a | 61 | int main() |
AlexisCollin | 0:aebeb20b201a | 62 | { |
AlexisCollin | 0:aebeb20b201a | 63 | dbug.printf("\nSETUP "); |
AlexisCollin | 0:aebeb20b201a | 64 | can_init(); |
AlexisCollin | 0:aebeb20b201a | 65 | UBp_it.rise(&isr_UBp); |
AlexisCollin | 0:aebeb20b201a | 66 | //SetOdometrie(26,500,55,0); |
AlexisCollin | 0:aebeb20b201a | 67 | bonus_findepartie(0); |
AlexisCollin | 0:aebeb20b201a | 68 | |
AlexisCollin | 0:aebeb20b201a | 69 | cmd_servo = 0; |
AlexisCollin | 0:aebeb20b201a | 70 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 71 | ejection_active = 1; |
AlexisCollin | 0:aebeb20b201a | 72 | temps_ecoule = 0; |
AlexisCollin | 0:aebeb20b201a | 73 | match_en_cours = 0; |
AlexisCollin | 0:aebeb20b201a | 74 | |
AlexisCollin | 0:aebeb20b201a | 75 | //BendRadius(139, 900, -1,0); |
AlexisCollin | 0:aebeb20b201a | 76 | //GoStraight(3800,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 77 | dbug.printf("END SETUP\n"); |
AlexisCollin | 0:aebeb20b201a | 78 | while(!match_en_cours){ |
AlexisCollin | 0:aebeb20b201a | 79 | trait_can(); |
AlexisCollin | 0:aebeb20b201a | 80 | wait_debut_de_partie();} |
AlexisCollin | 0:aebeb20b201a | 81 | |
AlexisCollin | 0:aebeb20b201a | 82 | timer_generale.start(); |
AlexisCollin | 0:aebeb20b201a | 83 | //Rotate(3600); |
AlexisCollin | 0:aebeb20b201a | 84 | //BendRadius(139, -900, -1,0); |
AlexisCollin | 0:aebeb20b201a | 85 | //while(1); |
AlexisCollin | 0:aebeb20b201a | 86 | //dbug.printf("%d\n", flagDaniels); |
AlexisCollin | 0:aebeb20b201a | 87 | |
AlexisCollin | 0:aebeb20b201a | 88 | while(1) |
AlexisCollin | 0:aebeb20b201a | 89 | { |
AlexisCollin | 0:aebeb20b201a | 90 | if(temps_ecoule > DUREE_MATCH_TIC) |
AlexisCollin | 0:aebeb20b201a | 91 | { |
AlexisCollin | 0:aebeb20b201a | 92 | match_en_cours = 0; |
AlexisCollin | 0:aebeb20b201a | 93 | SendRawId(ASSERVISSEMENT_STOP); |
AlexisCollin | 0:aebeb20b201a | 94 | ejection_active = 0; |
AlexisCollin | 0:aebeb20b201a | 95 | automate_ejecteur(); |
AlexisCollin | 0:aebeb20b201a | 96 | wait(6); |
AlexisCollin | 0:aebeb20b201a | 97 | bonus_findepartie(1); |
AlexisCollin | 0:aebeb20b201a | 98 | while(1); |
AlexisCollin | 0:aebeb20b201a | 99 | } |
AlexisCollin | 0:aebeb20b201a | 100 | else if(timer_generale.read_us() > TIKER_GENERALE_US) |
AlexisCollin | 0:aebeb20b201a | 101 | {timer_generale.reset();temps_ecoule++; |
AlexisCollin | 0:aebeb20b201a | 102 | trait_can(); |
AlexisCollin | 0:aebeb20b201a | 103 | f_mesure(); |
AlexisCollin | 0:aebeb20b201a | 104 | pos_disquette(); |
AlexisCollin | 0:aebeb20b201a | 105 | |
AlexisCollin | 0:aebeb20b201a | 106 | //if_UBp(); |
AlexisCollin | 0:aebeb20b201a | 107 | //automate_base(); |
AlexisCollin | 0:aebeb20b201a | 108 | automate_pied_ballon(); |
AlexisCollin | 0:aebeb20b201a | 109 | automate_visee_balle(); |
AlexisCollin | 0:aebeb20b201a | 110 | |
AlexisCollin | 0:aebeb20b201a | 111 | automate_ejecteur(); |
AlexisCollin | 0:aebeb20b201a | 112 | if(((temps_ecoule%500) == 0))// execute chaques secondes |
AlexisCollin | 0:aebeb20b201a | 113 | { |
AlexisCollin | 0:aebeb20b201a | 114 | dbug.printf("Pos x:%d, y:%d, te:%d, Tecoule:%d\n", pos[0], pos[1], pos[2],temps_ecoule*TIKER_GENERALE_US/1000000); |
AlexisCollin | 0:aebeb20b201a | 115 | //dbug.printf("Dis D:%f, G:%f, min:%f\n", DTD_avg_Ex, DTG_avg_Ex, DT_avg_min_Ex); |
AlexisCollin | 0:aebeb20b201a | 116 | } |
AlexisCollin | 0:aebeb20b201a | 117 | } |
AlexisCollin | 0:aebeb20b201a | 118 | |
AlexisCollin | 0:aebeb20b201a | 119 | } |
AlexisCollin | 0:aebeb20b201a | 120 | } |
AlexisCollin | 0:aebeb20b201a | 121 | |
AlexisCollin | 0:aebeb20b201a | 122 | void automate_macro_auto() |
AlexisCollin | 0:aebeb20b201a | 123 | { |
AlexisCollin | 0:aebeb20b201a | 124 | timer_macro_auto++; |
AlexisCollin | 0:aebeb20b201a | 125 | switch(etat_macro_auto) |
AlexisCollin | 0:aebeb20b201a | 126 | { |
AlexisCollin | 0:aebeb20b201a | 127 | case init_macro_auto: |
AlexisCollin | 0:aebeb20b201a | 128 | etat_macro_auto = init_macro_auto; timer_macro_auto = 0; |
AlexisCollin | 0:aebeb20b201a | 129 | break; |
AlexisCollin | 0:aebeb20b201a | 130 | } |
AlexisCollin | 0:aebeb20b201a | 131 | } |
AlexisCollin | 0:aebeb20b201a | 132 | |
AlexisCollin | 0:aebeb20b201a | 133 | void automate_3balles_odo() |
AlexisCollin | 0:aebeb20b201a | 134 | { |
AlexisCollin | 0:aebeb20b201a | 135 | timer_3balles_odot++; |
AlexisCollin | 0:aebeb20b201a | 136 | switch(etat_3balles_odo) |
AlexisCollin | 0:aebeb20b201a | 137 | { |
AlexisCollin | 0:aebeb20b201a | 138 | case 1: |
AlexisCollin | 0:aebeb20b201a | 139 | ejection_active = 1; |
AlexisCollin | 0:aebeb20b201a | 140 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 141 | GoStraight(1805,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 142 | etat_3balles_odo = 2; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 143 | break; |
AlexisCollin | 0:aebeb20b201a | 144 | |
AlexisCollin | 0:aebeb20b201a | 145 | case 2 : |
AlexisCollin | 0:aebeb20b201a | 146 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 147 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 148 | flagDaniels = 1; |
AlexisCollin | 0:aebeb20b201a | 149 | Rotate(-930);//rotation acoté de la balle |
AlexisCollin | 0:aebeb20b201a | 150 | etat_3balles_odo = pied_ballon_tbc; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 151 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 152 | break; |
AlexisCollin | 0:aebeb20b201a | 153 | |
AlexisCollin | 0:aebeb20b201a | 154 | case 3 : |
AlexisCollin | 0:aebeb20b201a | 155 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 156 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 157 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 158 | GoStraight(1500,0,0,0);//on va vers la deuxieme balle |
AlexisCollin | 0:aebeb20b201a | 159 | etat_3balles_odo = 4; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 160 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 161 | break; |
AlexisCollin | 0:aebeb20b201a | 162 | |
AlexisCollin | 0:aebeb20b201a | 163 | case 4 : |
AlexisCollin | 0:aebeb20b201a | 164 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 165 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 166 | Rotate(940);//deuxieme balle gobé ,on tourne |
AlexisCollin | 0:aebeb20b201a | 167 | etat_3balles_odo = 5; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 168 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 169 | break; |
AlexisCollin | 0:aebeb20b201a | 170 | |
AlexisCollin | 0:aebeb20b201a | 171 | case 5 : |
AlexisCollin | 0:aebeb20b201a | 172 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 173 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 174 | envoyer_balle = 1;ejection_continue = 1;//tir la deuxieme balle |
AlexisCollin | 0:aebeb20b201a | 175 | flagDaniels = 1; |
AlexisCollin | 0:aebeb20b201a | 176 | Rotate(-940);//ici implementer le deuxieme ciblage laser |
AlexisCollin | 0:aebeb20b201a | 177 | etat_3balles_odo = 6; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 178 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 179 | break; |
AlexisCollin | 0:aebeb20b201a | 180 | |
AlexisCollin | 0:aebeb20b201a | 181 | case 6: |
AlexisCollin | 0:aebeb20b201a | 182 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 183 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 184 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 185 | GoStraight(1500,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 186 | etat_3balles_odo = 7; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 187 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 188 | break; |
AlexisCollin | 0:aebeb20b201a | 189 | |
AlexisCollin | 0:aebeb20b201a | 190 | case 7: |
AlexisCollin | 0:aebeb20b201a | 191 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 192 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 193 | Rotate(930); |
AlexisCollin | 0:aebeb20b201a | 194 | etat_3balles_odo = 8; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 195 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 196 | break; |
AlexisCollin | 0:aebeb20b201a | 197 | |
AlexisCollin | 0:aebeb20b201a | 198 | case 8: |
AlexisCollin | 0:aebeb20b201a | 199 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 200 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 201 | envoyer_balle = 1;ejection_continue = 1;//envoie de la troisième balle |
AlexisCollin | 0:aebeb20b201a | 202 | etat_3balles_odo = 9; timer_3balles_odot = 0; |
AlexisCollin | 0:aebeb20b201a | 203 | sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite |
AlexisCollin | 0:aebeb20b201a | 204 | }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; |
AlexisCollin | 0:aebeb20b201a | 205 | break; |
AlexisCollin | 0:aebeb20b201a | 206 | |
AlexisCollin | 0:aebeb20b201a | 207 | case 9: |
AlexisCollin | 0:aebeb20b201a | 208 | break; |
AlexisCollin | 0:aebeb20b201a | 209 | |
AlexisCollin | 0:aebeb20b201a | 210 | case 10: |
AlexisCollin | 0:aebeb20b201a | 211 | break; |
AlexisCollin | 0:aebeb20b201a | 212 | } |
AlexisCollin | 0:aebeb20b201a | 213 | } |
AlexisCollin | 0:aebeb20b201a | 214 | |
AlexisCollin | 0:aebeb20b201a | 215 | |
AlexisCollin | 0:aebeb20b201a | 216 | |
AlexisCollin | 0:aebeb20b201a | 217 | void automate_tirer_balle_complet() |
AlexisCollin | 0:aebeb20b201a | 218 | { |
AlexisCollin | 0:aebeb20b201a | 219 | timer_tirer_balle_complet++; |
AlexisCollin | 0:aebeb20b201a | 220 | switch(etat_tirer_balle_complet) |
AlexisCollin | 0:aebeb20b201a | 221 | { |
AlexisCollin | 0:aebeb20b201a | 222 | case init_tirer_balle_complet: |
AlexisCollin | 0:aebeb20b201a | 223 | sensrotation = -sensrotation; |
AlexisCollin | 0:aebeb20b201a | 224 | etat_pied_ballon = init_pied_ballon_dierct; |
AlexisCollin | 0:aebeb20b201a | 225 | etat_tirer_balle_complet = pied_ballon_tbc; timer_tirer_balle_complet = 0; |
AlexisCollin | 0:aebeb20b201a | 226 | break; |
AlexisCollin | 0:aebeb20b201a | 227 | |
AlexisCollin | 0:aebeb20b201a | 228 | case pied_ballon_tbc: |
AlexisCollin | 0:aebeb20b201a | 229 | automate_pied_ballon(); |
AlexisCollin | 0:aebeb20b201a | 230 | if(etat_pied_ballon == fin_pied_ballon) |
AlexisCollin | 0:aebeb20b201a | 231 | { |
AlexisCollin | 0:aebeb20b201a | 232 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 233 | etat_tirer_balle_complet = visee_balle_tbc; timer_tirer_balle_complet = 0; |
AlexisCollin | 0:aebeb20b201a | 234 | } |
AlexisCollin | 0:aebeb20b201a | 235 | break; |
AlexisCollin | 0:aebeb20b201a | 236 | |
AlexisCollin | 0:aebeb20b201a | 237 | case visee_balle_tbc: |
AlexisCollin | 0:aebeb20b201a | 238 | automate_visee_balle(); |
AlexisCollin | 0:aebeb20b201a | 239 | if(etat_visee_balle == fin_visee_balle) |
AlexisCollin | 0:aebeb20b201a | 240 | { |
AlexisCollin | 0:aebeb20b201a | 241 | //etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 242 | etat_tirer_balle_complet = fin_tirer_balle_complet; timer_tirer_balle_complet = 0; |
AlexisCollin | 0:aebeb20b201a | 243 | } |
AlexisCollin | 0:aebeb20b201a | 244 | else if(etat_visee_balle == error_visee_balle) |
AlexisCollin | 0:aebeb20b201a | 245 | { |
AlexisCollin | 0:aebeb20b201a | 246 | //etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 247 | etat_tirer_balle_complet = error_tirer_balle_complet; timer_tirer_balle_complet = 0; |
AlexisCollin | 0:aebeb20b201a | 248 | } |
AlexisCollin | 0:aebeb20b201a | 249 | break; |
AlexisCollin | 0:aebeb20b201a | 250 | |
AlexisCollin | 0:aebeb20b201a | 251 | case fin_tirer_balle_complet: |
AlexisCollin | 0:aebeb20b201a | 252 | break; |
AlexisCollin | 0:aebeb20b201a | 253 | |
AlexisCollin | 0:aebeb20b201a | 254 | case error_tirer_balle_complet: |
AlexisCollin | 0:aebeb20b201a | 255 | break; |
AlexisCollin | 0:aebeb20b201a | 256 | |
AlexisCollin | 0:aebeb20b201a | 257 | |
AlexisCollin | 0:aebeb20b201a | 258 | } |
AlexisCollin | 0:aebeb20b201a | 259 | } |
AlexisCollin | 0:aebeb20b201a | 260 | |
AlexisCollin | 0:aebeb20b201a | 261 | void automate_pied_ballon() |
AlexisCollin | 0:aebeb20b201a | 262 | { |
AlexisCollin | 0:aebeb20b201a | 263 | timer_pied_ballon++; |
AlexisCollin | 0:aebeb20b201a | 264 | switch(etat_pied_ballon) |
AlexisCollin | 0:aebeb20b201a | 265 | { |
AlexisCollin | 0:aebeb20b201a | 266 | case init_pied_ballon_dierct: |
AlexisCollin | 0:aebeb20b201a | 267 | short y_temp; |
AlexisCollin | 0:aebeb20b201a | 268 | if(sensrotation==1)y_temp = 300; |
AlexisCollin | 0:aebeb20b201a | 269 | else y_temp = 3700; |
AlexisCollin | 0:aebeb20b201a | 270 | GoToPosition(3500, y_temp, 0, 0); |
AlexisCollin | 0:aebeb20b201a | 271 | etat_pied_ballon = avance_x_DT_pb; timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 272 | break; |
AlexisCollin | 0:aebeb20b201a | 273 | |
AlexisCollin | 0:aebeb20b201a | 274 | case avance_x_DT_pb: |
AlexisCollin | 0:aebeb20b201a | 275 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 276 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 277 | GoStraight(3050-DTA_avg_Ex,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 278 | //SetOdometrie(26,pos[0],y_temp,sensrotation*900); |
AlexisCollin | 0:aebeb20b201a | 279 | etat_pied_ballon = rotation_ballon_x_pb; timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 280 | }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 281 | break; |
AlexisCollin | 0:aebeb20b201a | 282 | |
AlexisCollin | 0:aebeb20b201a | 283 | case rotation_ballon_x_pb: |
AlexisCollin | 0:aebeb20b201a | 284 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 285 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 286 | if(DTA_avg_Ex<3070 && DTA_avg_Ex>3020){ |
AlexisCollin | 0:aebeb20b201a | 287 | Rotate(sensrotation*900); |
AlexisCollin | 0:aebeb20b201a | 288 | etat_pied_ballon = reculer_ballon_x_pb; timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 289 | } |
AlexisCollin | 0:aebeb20b201a | 290 | }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 291 | break; |
AlexisCollin | 0:aebeb20b201a | 292 | |
AlexisCollin | 0:aebeb20b201a | 293 | case reculer_ballon_x_pb: |
AlexisCollin | 0:aebeb20b201a | 294 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 295 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 296 | GoStraight(-250,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 297 | etat_pied_ballon = recalage_pied_pb; timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 298 | }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 299 | break; |
AlexisCollin | 0:aebeb20b201a | 300 | |
AlexisCollin | 0:aebeb20b201a | 301 | case recalage_pied_pb: |
AlexisCollin | 0:aebeb20b201a | 302 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 303 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 304 | short y_temp; |
AlexisCollin | 0:aebeb20b201a | 305 | if(sensrotation==1)y_temp = 55; |
AlexisCollin | 0:aebeb20b201a | 306 | else y_temp = 3945; |
AlexisCollin | 0:aebeb20b201a | 307 | SetOdometrie(26,3050,y_temp,sensrotation*900); |
AlexisCollin | 0:aebeb20b201a | 308 | etat_pied_ballon = fin_pied_ballon; timer_pied_ballon = 0; |
AlexisCollin | 0:aebeb20b201a | 309 | }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 310 | break; |
AlexisCollin | 0:aebeb20b201a | 311 | |
AlexisCollin | 0:aebeb20b201a | 312 | case fin_pied_ballon: |
AlexisCollin | 0:aebeb20b201a | 313 | break; |
AlexisCollin | 0:aebeb20b201a | 314 | |
AlexisCollin | 0:aebeb20b201a | 315 | case error_pied_ballon: |
AlexisCollin | 0:aebeb20b201a | 316 | break; |
AlexisCollin | 0:aebeb20b201a | 317 | } |
AlexisCollin | 0:aebeb20b201a | 318 | if(etat_pied_ballon != etat_pied_ballon_prec)dbug.printf("Etat visee_balle : %d\n",etat_pied_ballon); |
AlexisCollin | 0:aebeb20b201a | 319 | etat_pied_ballon_prec = etat_pied_ballon; |
AlexisCollin | 0:aebeb20b201a | 320 | } |
AlexisCollin | 0:aebeb20b201a | 321 | |
AlexisCollin | 0:aebeb20b201a | 322 | |
AlexisCollin | 0:aebeb20b201a | 323 | |
AlexisCollin | 0:aebeb20b201a | 324 | void automate_visee_balle() |
AlexisCollin | 0:aebeb20b201a | 325 | { |
AlexisCollin | 0:aebeb20b201a | 326 | timer_visee_balle++; |
AlexisCollin | 0:aebeb20b201a | 327 | |
AlexisCollin | 0:aebeb20b201a | 328 | switch(etat_visee_balle) |
AlexisCollin | 0:aebeb20b201a | 329 | { |
AlexisCollin | 0:aebeb20b201a | 330 | case init_visee_balle ://--------------------------------------0 |
AlexisCollin | 0:aebeb20b201a | 331 | SendSpeed(80, 600); |
AlexisCollin | 0:aebeb20b201a | 332 | Rotate(sensrotation*900); |
AlexisCollin | 0:aebeb20b201a | 333 | etat_visee_balle = fin_vir_rapide; |
AlexisCollin | 0:aebeb20b201a | 334 | timer_visee_balle=0; |
AlexisCollin | 0:aebeb20b201a | 335 | break; |
AlexisCollin | 0:aebeb20b201a | 336 | |
AlexisCollin | 0:aebeb20b201a | 337 | case fin_vir_rapide : //--------------------------------------1 |
AlexisCollin | 0:aebeb20b201a | 338 | if(balleg || balled ) |
AlexisCollin | 0:aebeb20b201a | 339 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 340 | dbug.printf("balle 1-----1\n"); |
AlexisCollin | 0:aebeb20b201a | 341 | pos_balle[0][2] = pos[2]; |
AlexisCollin | 0:aebeb20b201a | 342 | SendRawId(ASSERVISSEMENT_STOP); |
AlexisCollin | 0:aebeb20b201a | 343 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 344 | etat_visee_balle = debut_vir_lent; |
AlexisCollin | 0:aebeb20b201a | 345 | } else if(flagFinDpl || timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 346 | break; |
AlexisCollin | 0:aebeb20b201a | 347 | |
AlexisCollin | 0:aebeb20b201a | 348 | case debut_vir_lent://--------------------------------------2 |
AlexisCollin | 0:aebeb20b201a | 349 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 350 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 351 | dbug.printf("balle 2-----2\n"); |
AlexisCollin | 0:aebeb20b201a | 352 | SendSpeed(18, 300); |
AlexisCollin | 0:aebeb20b201a | 353 | Rotate((pos_balle[0][2]-pos[2])*2); |
AlexisCollin | 0:aebeb20b201a | 354 | etat_visee_balle = fin_vir_lent; |
AlexisCollin | 0:aebeb20b201a | 355 | }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 356 | break; |
AlexisCollin | 0:aebeb20b201a | 357 | |
AlexisCollin | 0:aebeb20b201a | 358 | case balle_disparue: |
AlexisCollin | 0:aebeb20b201a | 359 | break; |
AlexisCollin | 0:aebeb20b201a | 360 | |
AlexisCollin | 0:aebeb20b201a | 361 | case fin_vir_lent://--------------------------------------3 |
AlexisCollin | 0:aebeb20b201a | 362 | if(balleg && balled) |
AlexisCollin | 0:aebeb20b201a | 363 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 364 | pos_balle[0][2] = pos[2]; |
AlexisCollin | 0:aebeb20b201a | 365 | SendRawId(ASSERVISSEMENT_STOP); |
AlexisCollin | 0:aebeb20b201a | 366 | SendSpeed(100, 600); |
AlexisCollin | 0:aebeb20b201a | 367 | if(DT_avg_min_Ex<2000)etat_visee_balle = avancer; |
AlexisCollin | 0:aebeb20b201a | 368 | else etat_visee_balle = avancer_20; |
AlexisCollin | 0:aebeb20b201a | 369 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 370 | }else{ |
AlexisCollin | 0:aebeb20b201a | 371 | etat_visee_balle = debut_vir_lent; |
AlexisCollin | 0:aebeb20b201a | 372 | flagFinDpl = 1; |
AlexisCollin | 0:aebeb20b201a | 373 | } |
AlexisCollin | 0:aebeb20b201a | 374 | break; |
AlexisCollin | 0:aebeb20b201a | 375 | |
AlexisCollin | 0:aebeb20b201a | 376 | |
AlexisCollin | 0:aebeb20b201a | 377 | case avancer_20://--------------------------------------4 |
AlexisCollin | 0:aebeb20b201a | 378 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 379 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 380 | GoStraight(DT_avg_min_Ex-D_ROULEAU_AXE-500,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 381 | etat_visee_balle = debut_vir_lent2; |
AlexisCollin | 0:aebeb20b201a | 382 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 383 | }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 384 | break; |
AlexisCollin | 0:aebeb20b201a | 385 | |
AlexisCollin | 0:aebeb20b201a | 386 | case debut_vir_lent2://--------------------------------------5 |
AlexisCollin | 0:aebeb20b201a | 387 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 388 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 389 | |
AlexisCollin | 0:aebeb20b201a | 390 | SendSpeed(18, 300); |
AlexisCollin | 0:aebeb20b201a | 391 | |
AlexisCollin | 0:aebeb20b201a | 392 | if(!balleg && !balled) |
AlexisCollin | 0:aebeb20b201a | 393 | { |
AlexisCollin | 0:aebeb20b201a | 394 | etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 395 | } |
AlexisCollin | 0:aebeb20b201a | 396 | else{ |
AlexisCollin | 0:aebeb20b201a | 397 | if(!balleg && balled) |
AlexisCollin | 0:aebeb20b201a | 398 | { |
AlexisCollin | 0:aebeb20b201a | 399 | BendRadius(139, sensrotation*30, sensrotation,0); |
AlexisCollin | 0:aebeb20b201a | 400 | etat_visee_balle = fin_vir_lent2; |
AlexisCollin | 0:aebeb20b201a | 401 | } |
AlexisCollin | 0:aebeb20b201a | 402 | else if(balleg && !balled) |
AlexisCollin | 0:aebeb20b201a | 403 | { |
AlexisCollin | 0:aebeb20b201a | 404 | BendRadius(139, 30, sensrotation,0); |
AlexisCollin | 0:aebeb20b201a | 405 | etat_visee_balle = fin_vir_lent2; |
AlexisCollin | 0:aebeb20b201a | 406 | } |
AlexisCollin | 0:aebeb20b201a | 407 | else if(balleg && balled) |
AlexisCollin | 0:aebeb20b201a | 408 | { |
AlexisCollin | 0:aebeb20b201a | 409 | flagFinDpl = 1; |
AlexisCollin | 0:aebeb20b201a | 410 | SendSpeed(100, 600); |
AlexisCollin | 0:aebeb20b201a | 411 | etat_visee_balle = avancer; |
AlexisCollin | 0:aebeb20b201a | 412 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 413 | } |
AlexisCollin | 0:aebeb20b201a | 414 | } |
AlexisCollin | 0:aebeb20b201a | 415 | }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 416 | break; |
AlexisCollin | 0:aebeb20b201a | 417 | |
AlexisCollin | 0:aebeb20b201a | 418 | case fin_vir_lent2://--------------------------------------6 |
AlexisCollin | 0:aebeb20b201a | 419 | if(balleg && balled) |
AlexisCollin | 0:aebeb20b201a | 420 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 421 | SendRawId(ASSERVISSEMENT_STOP); |
AlexisCollin | 0:aebeb20b201a | 422 | SendSpeed(100, 600); |
AlexisCollin | 0:aebeb20b201a | 423 | etat_visee_balle = avancer; |
AlexisCollin | 0:aebeb20b201a | 424 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 425 | }else{flagFinDpl = 1; |
AlexisCollin | 0:aebeb20b201a | 426 | etat_visee_balle = debut_vir_lent2; |
AlexisCollin | 0:aebeb20b201a | 427 | } |
AlexisCollin | 0:aebeb20b201a | 428 | break; |
AlexisCollin | 0:aebeb20b201a | 429 | |
AlexisCollin | 0:aebeb20b201a | 430 | |
AlexisCollin | 0:aebeb20b201a | 431 | case avancer://--------------------------------------7 |
AlexisCollin | 0:aebeb20b201a | 432 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 433 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 434 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 435 | GoStraight(DT_avg_min_Ex-(D_ROULEAU_AXE),0,0,0); |
AlexisCollin | 0:aebeb20b201a | 436 | etat_visee_balle = rotation; |
AlexisCollin | 0:aebeb20b201a | 437 | } |
AlexisCollin | 0:aebeb20b201a | 438 | if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 439 | break; |
AlexisCollin | 0:aebeb20b201a | 440 | |
AlexisCollin | 0:aebeb20b201a | 441 | case rotation://--------------------------------------8 |
AlexisCollin | 0:aebeb20b201a | 442 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 443 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 444 | if(balle_gobbee) |
AlexisCollin | 0:aebeb20b201a | 445 | { |
AlexisCollin | 0:aebeb20b201a | 446 | short dTeta = -pos[2]; |
AlexisCollin | 0:aebeb20b201a | 447 | Rotate(dTeta); |
AlexisCollin | 0:aebeb20b201a | 448 | etat_visee_balle = lancer; |
AlexisCollin | 0:aebeb20b201a | 449 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 450 | }else etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 451 | } |
AlexisCollin | 0:aebeb20b201a | 452 | break; |
AlexisCollin | 0:aebeb20b201a | 453 | |
AlexisCollin | 0:aebeb20b201a | 454 | case lancer: |
AlexisCollin | 0:aebeb20b201a | 455 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 456 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 457 | envoyer_balle = 1; |
AlexisCollin | 0:aebeb20b201a | 458 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 459 | etat_visee_balle = fin_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 460 | |
AlexisCollin | 0:aebeb20b201a | 461 | } if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 462 | break; |
AlexisCollin | 0:aebeb20b201a | 463 | |
AlexisCollin | 0:aebeb20b201a | 464 | case fin_visee_balle: |
AlexisCollin | 0:aebeb20b201a | 465 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 466 | break; |
AlexisCollin | 0:aebeb20b201a | 467 | case error_visee_balle: |
AlexisCollin | 0:aebeb20b201a | 468 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 469 | break; |
AlexisCollin | 0:aebeb20b201a | 470 | |
AlexisCollin | 0:aebeb20b201a | 471 | case retour : |
AlexisCollin | 0:aebeb20b201a | 472 | GoToPosition(300, 0, 0, 0); |
AlexisCollin | 0:aebeb20b201a | 473 | etat_visee_balle = recalage; |
AlexisCollin | 0:aebeb20b201a | 474 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 475 | break; |
AlexisCollin | 0:aebeb20b201a | 476 | |
AlexisCollin | 0:aebeb20b201a | 477 | case recalage : |
AlexisCollin | 0:aebeb20b201a | 478 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 479 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 480 | GoStraight(-295,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 481 | etat_visee_balle = recalage2; |
AlexisCollin | 0:aebeb20b201a | 482 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 483 | }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 484 | break; |
AlexisCollin | 0:aebeb20b201a | 485 | case recalage2 : |
AlexisCollin | 0:aebeb20b201a | 486 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 487 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 488 | GoStraight(30,2,50,0); |
AlexisCollin | 0:aebeb20b201a | 489 | etat_visee_balle = recalage2; |
AlexisCollin | 0:aebeb20b201a | 490 | timer_visee_balle = 0; |
AlexisCollin | 0:aebeb20b201a | 491 | }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 492 | break; |
AlexisCollin | 0:aebeb20b201a | 493 | case recalage3 : |
AlexisCollin | 0:aebeb20b201a | 494 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 495 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 496 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 497 | } |
AlexisCollin | 0:aebeb20b201a | 498 | break; |
AlexisCollin | 0:aebeb20b201a | 499 | } |
AlexisCollin | 0:aebeb20b201a | 500 | if(etat_visee_balle != etat_visee_balle_prec)dbug.printf("Etat visee_balle : %d\n",etat_visee_balle); |
AlexisCollin | 0:aebeb20b201a | 501 | etat_visee_balle_prec = etat_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 502 | } |
AlexisCollin | 0:aebeb20b201a | 503 | |
AlexisCollin | 0:aebeb20b201a | 504 | void automate_base() |
AlexisCollin | 0:aebeb20b201a | 505 | { |
AlexisCollin | 0:aebeb20b201a | 506 | switch(etat_auto_base) |
AlexisCollin | 0:aebeb20b201a | 507 | { |
AlexisCollin | 0:aebeb20b201a | 508 | case 0 : |
AlexisCollin | 0:aebeb20b201a | 509 | temps_ecoule = 0; |
AlexisCollin | 0:aebeb20b201a | 510 | etat_auto_base = 1; |
AlexisCollin | 0:aebeb20b201a | 511 | break; |
AlexisCollin | 0:aebeb20b201a | 512 | |
AlexisCollin | 0:aebeb20b201a | 513 | case 1 : |
AlexisCollin | 0:aebeb20b201a | 514 | ejection_active = 1; |
AlexisCollin | 0:aebeb20b201a | 515 | ejection_continue = 1; |
AlexisCollin | 0:aebeb20b201a | 516 | GoStraight(1805,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 517 | etat_auto_base=2; |
AlexisCollin | 0:aebeb20b201a | 518 | break; |
AlexisCollin | 0:aebeb20b201a | 519 | |
AlexisCollin | 0:aebeb20b201a | 520 | case 2 : |
AlexisCollin | 0:aebeb20b201a | 521 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 522 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 523 | flagDaniels = 1; |
AlexisCollin | 0:aebeb20b201a | 524 | Rotate(-850);//rotation acoté de la balle |
AlexisCollin | 0:aebeb20b201a | 525 | etat_auto_base=201; |
AlexisCollin | 0:aebeb20b201a | 526 | |
AlexisCollin | 0:aebeb20b201a | 527 | } |
AlexisCollin | 0:aebeb20b201a | 528 | break; |
AlexisCollin | 0:aebeb20b201a | 529 | |
AlexisCollin | 0:aebeb20b201a | 530 | case 201 : |
AlexisCollin | 0:aebeb20b201a | 531 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 532 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 533 | sensrotation=-1; |
AlexisCollin | 0:aebeb20b201a | 534 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 535 | etat_auto_base=202; |
AlexisCollin | 0:aebeb20b201a | 536 | } |
AlexisCollin | 0:aebeb20b201a | 537 | break; |
AlexisCollin | 0:aebeb20b201a | 538 | |
AlexisCollin | 0:aebeb20b201a | 539 | case 202 : |
AlexisCollin | 0:aebeb20b201a | 540 | automate_visee_balle(); //ciblage vers la gauche |
AlexisCollin | 0:aebeb20b201a | 541 | if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle |
AlexisCollin | 0:aebeb20b201a | 542 | { |
AlexisCollin | 0:aebeb20b201a | 543 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 544 | etat_auto_base = 3; |
AlexisCollin | 0:aebeb20b201a | 545 | } |
AlexisCollin | 0:aebeb20b201a | 546 | break; |
AlexisCollin | 0:aebeb20b201a | 547 | //---------------------FIN--------------------------- |
AlexisCollin | 0:aebeb20b201a | 548 | |
AlexisCollin | 0:aebeb20b201a | 549 | case 3 : |
AlexisCollin | 0:aebeb20b201a | 550 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 551 | GoStraight(1500,0,0,0);//on va vers la deuxieme balle |
AlexisCollin | 0:aebeb20b201a | 552 | etat_auto_base=4; |
AlexisCollin | 0:aebeb20b201a | 553 | break; |
AlexisCollin | 0:aebeb20b201a | 554 | |
AlexisCollin | 0:aebeb20b201a | 555 | case 4 : |
AlexisCollin | 0:aebeb20b201a | 556 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 557 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 558 | Rotate(900);//deuxieme balle gobé ,on tourne |
AlexisCollin | 0:aebeb20b201a | 559 | etat_auto_base=5; |
AlexisCollin | 0:aebeb20b201a | 560 | } |
AlexisCollin | 0:aebeb20b201a | 561 | break; |
AlexisCollin | 0:aebeb20b201a | 562 | |
AlexisCollin | 0:aebeb20b201a | 563 | case 5 : |
AlexisCollin | 0:aebeb20b201a | 564 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 565 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 566 | envoyer_balle = 1;//tir la deuxieme balle |
AlexisCollin | 0:aebeb20b201a | 567 | flagDaniels = 1; |
AlexisCollin | 0:aebeb20b201a | 568 | Rotate(-850);//ici implementer le deuxieme ciblage laser |
AlexisCollin | 0:aebeb20b201a | 569 | etat_auto_base=601; |
AlexisCollin | 0:aebeb20b201a | 570 | } |
AlexisCollin | 0:aebeb20b201a | 571 | break; |
AlexisCollin | 0:aebeb20b201a | 572 | |
AlexisCollin | 0:aebeb20b201a | 573 | case 601 : |
AlexisCollin | 0:aebeb20b201a | 574 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 575 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 576 | sensrotation=-1; |
AlexisCollin | 0:aebeb20b201a | 577 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 578 | etat_auto_base=602; |
AlexisCollin | 0:aebeb20b201a | 579 | } |
AlexisCollin | 0:aebeb20b201a | 580 | break; |
AlexisCollin | 0:aebeb20b201a | 581 | |
AlexisCollin | 0:aebeb20b201a | 582 | case 602 : |
AlexisCollin | 0:aebeb20b201a | 583 | automate_visee_balle(); //ciblage vers la gauche |
AlexisCollin | 0:aebeb20b201a | 584 | if(etat_visee_balle == 4)//on attend d'etre sur la balle |
AlexisCollin | 0:aebeb20b201a | 585 | { |
AlexisCollin | 0:aebeb20b201a | 586 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 587 | etat_auto_base = 7; |
AlexisCollin | 0:aebeb20b201a | 588 | } |
AlexisCollin | 0:aebeb20b201a | 589 | break; |
AlexisCollin | 0:aebeb20b201a | 590 | |
AlexisCollin | 0:aebeb20b201a | 591 | case 7: |
AlexisCollin | 0:aebeb20b201a | 592 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 593 | GoStraight(1500,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 594 | etat_auto_base=8; |
AlexisCollin | 0:aebeb20b201a | 595 | break; |
AlexisCollin | 0:aebeb20b201a | 596 | |
AlexisCollin | 0:aebeb20b201a | 597 | case 8 : |
AlexisCollin | 0:aebeb20b201a | 598 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 599 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 600 | Rotate(900); |
AlexisCollin | 0:aebeb20b201a | 601 | etat_auto_base=9; |
AlexisCollin | 0:aebeb20b201a | 602 | } |
AlexisCollin | 0:aebeb20b201a | 603 | break; |
AlexisCollin | 0:aebeb20b201a | 604 | |
AlexisCollin | 0:aebeb20b201a | 605 | case 9 : |
AlexisCollin | 0:aebeb20b201a | 606 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 607 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 608 | envoyer_balle = 1;//envoie de la troisième balle |
AlexisCollin | 0:aebeb20b201a | 609 | etat_auto_base=10; |
AlexisCollin | 0:aebeb20b201a | 610 | sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite |
AlexisCollin | 0:aebeb20b201a | 611 | } |
AlexisCollin | 0:aebeb20b201a | 612 | break; |
AlexisCollin | 0:aebeb20b201a | 613 | |
AlexisCollin | 0:aebeb20b201a | 614 | case 10: |
AlexisCollin | 0:aebeb20b201a | 615 | GoStraight(1500,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 616 | etat_auto_base=11; |
AlexisCollin | 0:aebeb20b201a | 617 | break; |
AlexisCollin | 0:aebeb20b201a | 618 | |
AlexisCollin | 0:aebeb20b201a | 619 | case 11 : |
AlexisCollin | 0:aebeb20b201a | 620 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 621 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 622 | Rotate(900); |
AlexisCollin | 0:aebeb20b201a | 623 | //cmd_servo = 2; |
AlexisCollin | 0:aebeb20b201a | 624 | etat_auto_base=12; |
AlexisCollin | 0:aebeb20b201a | 625 | } |
AlexisCollin | 0:aebeb20b201a | 626 | break; |
AlexisCollin | 0:aebeb20b201a | 627 | |
AlexisCollin | 0:aebeb20b201a | 628 | case 12 : |
AlexisCollin | 0:aebeb20b201a | 629 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 630 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 631 | GoStraight(100,2,0,0); |
AlexisCollin | 0:aebeb20b201a | 632 | cmd_servo = 2; |
AlexisCollin | 0:aebeb20b201a | 633 | etat_auto_base=13; |
AlexisCollin | 0:aebeb20b201a | 634 | } |
AlexisCollin | 0:aebeb20b201a | 635 | break; |
AlexisCollin | 0:aebeb20b201a | 636 | case 13 : |
AlexisCollin | 0:aebeb20b201a | 637 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 638 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 639 | GoStraight(3800,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 640 | cmd_servo = 1; |
AlexisCollin | 0:aebeb20b201a | 641 | etat_auto_base=14; |
AlexisCollin | 0:aebeb20b201a | 642 | } |
AlexisCollin | 0:aebeb20b201a | 643 | break; |
AlexisCollin | 0:aebeb20b201a | 644 | |
AlexisCollin | 0:aebeb20b201a | 645 | case 14 : |
AlexisCollin | 0:aebeb20b201a | 646 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 647 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 648 | GoStraight(-500,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 649 | etat_auto_base=15; |
AlexisCollin | 0:aebeb20b201a | 650 | } |
AlexisCollin | 0:aebeb20b201a | 651 | break; |
AlexisCollin | 0:aebeb20b201a | 652 | |
AlexisCollin | 0:aebeb20b201a | 653 | case 15 : |
AlexisCollin | 0:aebeb20b201a | 654 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 655 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 656 | Rotate(1800); |
AlexisCollin | 0:aebeb20b201a | 657 | cmd_servo = 0; |
AlexisCollin | 0:aebeb20b201a | 658 | etat_auto_base=16; |
AlexisCollin | 0:aebeb20b201a | 659 | sensrotation=1; |
AlexisCollin | 0:aebeb20b201a | 660 | } |
AlexisCollin | 0:aebeb20b201a | 661 | break; |
AlexisCollin | 0:aebeb20b201a | 662 | case 16 : |
AlexisCollin | 0:aebeb20b201a | 663 | |
AlexisCollin | 0:aebeb20b201a | 664 | break; |
AlexisCollin | 0:aebeb20b201a | 665 | |
AlexisCollin | 0:aebeb20b201a | 666 | |
AlexisCollin | 0:aebeb20b201a | 667 | |
AlexisCollin | 0:aebeb20b201a | 668 | case 100 : |
AlexisCollin | 0:aebeb20b201a | 669 | etat_visee_balle = init_visee_balle;//On passe dans l'automate de ciblage |
AlexisCollin | 0:aebeb20b201a | 670 | etat_auto_base=101; |
AlexisCollin | 0:aebeb20b201a | 671 | break; |
AlexisCollin | 0:aebeb20b201a | 672 | |
AlexisCollin | 0:aebeb20b201a | 673 | case 101 : |
AlexisCollin | 0:aebeb20b201a | 674 | automate_visee_balle(); //ciblage vers la gauche |
AlexisCollin | 0:aebeb20b201a | 675 | if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle |
AlexisCollin | 0:aebeb20b201a | 676 | { |
AlexisCollin | 0:aebeb20b201a | 677 | etat_visee_balle = init_visee_balle; |
AlexisCollin | 0:aebeb20b201a | 678 | etat_auto_base = 102; |
AlexisCollin | 0:aebeb20b201a | 679 | } |
AlexisCollin | 0:aebeb20b201a | 680 | break; |
AlexisCollin | 0:aebeb20b201a | 681 | |
AlexisCollin | 0:aebeb20b201a | 682 | case 102 : |
AlexisCollin | 0:aebeb20b201a | 683 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 684 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 685 | ejection_continue = 0; |
AlexisCollin | 0:aebeb20b201a | 686 | GoStraight(DTD_avg_Ex,0,0,0); |
AlexisCollin | 0:aebeb20b201a | 687 | etat_auto_base=103; |
AlexisCollin | 0:aebeb20b201a | 688 | } |
AlexisCollin | 0:aebeb20b201a | 689 | break; |
AlexisCollin | 0:aebeb20b201a | 690 | |
AlexisCollin | 0:aebeb20b201a | 691 | case 103 : |
AlexisCollin | 0:aebeb20b201a | 692 | if(flagFinDpl) |
AlexisCollin | 0:aebeb20b201a | 693 | {flagFinDpl = 0; |
AlexisCollin | 0:aebeb20b201a | 694 | Rotate(-pos[2]); |
AlexisCollin | 0:aebeb20b201a | 695 | etat_auto_base=100; |
AlexisCollin | 0:aebeb20b201a | 696 | } |
AlexisCollin | 0:aebeb20b201a | 697 | break; |
AlexisCollin | 0:aebeb20b201a | 698 | default : |
AlexisCollin | 0:aebeb20b201a | 699 | |
AlexisCollin | 0:aebeb20b201a | 700 | break; |
AlexisCollin | 0:aebeb20b201a | 701 | |
AlexisCollin | 0:aebeb20b201a | 702 | } |
AlexisCollin | 0:aebeb20b201a | 703 | if(etat_auto_base != etat_auto_base_prec)dbug.printf("Etat auto_base: %d\n",etat_auto_base); |
AlexisCollin | 0:aebeb20b201a | 704 | etat_auto_base_prec = etat_auto_base; |
AlexisCollin | 0:aebeb20b201a | 705 | } |
AlexisCollin | 0:aebeb20b201a | 706 | |
AlexisCollin | 0:aebeb20b201a | 707 | void wait_debut_de_partie() |
AlexisCollin | 0:aebeb20b201a | 708 | { |
AlexisCollin | 0:aebeb20b201a | 709 | if(!Daniels_in) |
AlexisCollin | 0:aebeb20b201a | 710 | { |
AlexisCollin | 0:aebeb20b201a | 711 | dbug.printf("-----DEBUT DE PARTIE------\n"); |
AlexisCollin | 0:aebeb20b201a | 712 | match_en_cours = 1; |
AlexisCollin | 0:aebeb20b201a | 713 | } |
AlexisCollin | 0:aebeb20b201a | 714 | } |
AlexisCollin | 0:aebeb20b201a | 715 | |
AlexisCollin | 0:aebeb20b201a | 716 | void user_bp_ar(){UBp_it.enable_irq();} |
AlexisCollin | 0:aebeb20b201a | 717 | void isr_UBp() |
AlexisCollin | 0:aebeb20b201a | 718 | { |
AlexisCollin | 0:aebeb20b201a | 719 | UBp_it.disable_irq();dbug.printf("ubp\n"); |
AlexisCollin | 0:aebeb20b201a | 720 | ubp_to.attach(&user_bp_ar, 0.3); |
AlexisCollin | 0:aebeb20b201a | 721 | flagUBp = 1; |
AlexisCollin | 0:aebeb20b201a | 722 | } |