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