Collin Alexis / Mbed 2 deprecated Code_FRC_2018

Code FRC 2018

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }