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