CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@87:349194e73b77, 2019-06-01 (annotated)

Committer:
kyxstark
Date:
Sat Jun 01 00:26:35 2019 +0000
Revision:
87:349194e73b77
Parent:
86:05a0c066bdbe
Child:
88:37714217b4c4
test reprise asserv stop (non teste);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 44:badcbe8766e9 1 #include "global.h"
Sitkah 31:833fc481b002 2 #include <string.h>
Artiom 44:badcbe8766e9 3 #include <sstream>
Artiom 52:a47350923b5e 4 #include <math.h>
Artiom 67:96f914f92d2d 5 #include <vector>
Sitkah 29:41e02746041d 6 //#include "StrategieManager.h"
Sitkah 29:41e02746041d 7
kyxstark 55:1e3dab1f90f4 8
Sitkah 29:41e02746041d 9
Sitkah 29:41e02746041d 10 #define M_PI 3.14159265358979323846
Sitkah 35:742dc6b200b0 11 #define VERT 0xFF00FF00
Sitkah 29:41e02746041d 12 #define ROUGE 0xFFFF0000
Sitkah 35:742dc6b200b0 13 #define BLEU 0xFF0000FF
kyxstark 58:faef8d1b8ed8 14 #define JAUNE 0xFFFDD835//FEFE00
Sitkah 29:41e02746041d 15 #define BLANC 0xFF000000
Sitkah 31:833fc481b002 16 #define ORANGE 0xFFFFA500
Sitkah 31:833fc481b002 17 #define NOIR 0xFF000000
Artiom 44:badcbe8766e9 18 #define DIY_GREY 0xFFDFDFDF
kyxstark 58:faef8d1b8ed8 19 #define VIOLET 0xFF4527A0
antbig 0:ad97421fb1fb 20
Sitkah 29:41e02746041d 21 char tableau_aff[10][50];
Artiom 44:badcbe8766e9 22 char tableau_etat[22][50]= {
Sitkah 29:41e02746041d 23 "Check_carte_screen",
Sitkah 29:41e02746041d 24 "Check_carte_screen_wait_ack",
Sitkah 29:41e02746041d 25 "Check_cartes",
Sitkah 29:41e02746041d 26 "Check_cartes_wait_ack",
Sitkah 29:41e02746041d 27 "Wait_force",
Sitkah 29:41e02746041d 28 "Config",
Sitkah 29:41e02746041d 29 "Game_init",
Sitkah 29:41e02746041d 30 "Game_wait_for_jack",
Sitkah 29:41e02746041d 31 "Game_start",
Sitkah 29:41e02746041d 32 "Game_next_instruction",
Sitkah 29:41e02746041d 33 "Game_instruction",
Sitkah 29:41e02746041d 34 "Game_wait_ack",
Sitkah 29:41e02746041d 35 "Game_jump_time",
Sitkah 29:41e02746041d 36 "Game_jump_config",
Sitkah 29:41e02746041d 37 "Game_jump_position",
Sitkah 29:41e02746041d 38 "Game_wait_end_instruction",
Sitkah 29:41e02746041d 39 "Warning_timeout",
Sitkah 29:41e02746041d 40 "Waring_end_balise_wait",
Sitkah 29:41e02746041d 41 "Warning_end_last_instruction",
Sitkah 29:41e02746041d 42 "Warning_switch_strategie",
Sitkah 29:41e02746041d 43 "End",
Sitkah 29:41e02746041d 44 "End_loop",
Sitkah 29:41e02746041d 45 };
Sitkah 29:41e02746041d 46
Sitkah 34:6aa4b46b102e 47 int waitingAckID_FIN;
Sitkah 34:6aa4b46b102e 48 int waitingAckFrom_FIN;
Sitkah 29:41e02746041d 49
Sitkah 29:41e02746041d 50 Ticker ticker;
Sitkah 29:41e02746041d 51 TS_DISCO_F469NI ts;
Sitkah 29:41e02746041d 52 LCD_DISCO_F469NI lcd;
Sitkah 29:41e02746041d 53
Sitkah 29:41e02746041d 54 TS_StateTypeDef TS_State;
Sitkah 29:41e02746041d 55
Sitkah 29:41e02746041d 56 Ticker chrono;
Sitkah 29:41e02746041d 57 Timeout AffTime;
Sitkah 29:41e02746041d 58 Timer timer;
antbig 0:ad97421fb1fb 59 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
antbig 0:ad97421fb1fb 60 Timer gameTimer;
antbig 0:ad97421fb1fb 61 Timer debugetatTimer;
antbig 5:dcd817534b57 62 Timer timeoutWarning;
antbig 5:dcd817534b57 63 Timer timeoutWarningWaitEnd;
Artiom 44:badcbe8766e9 64 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
antbig 0:ad97421fb1fb 65
Sitkah 29:41e02746041d 66 unsigned char screenChecktry = 0;
Sitkah 29:41e02746041d 67 unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};
Sitkah 29:41e02746041d 68
Sitkah 29:41e02746041d 69 char counter = 0;
Sitkah 29:41e02746041d 70 char check;
Sitkah 29:41e02746041d 71 char Jack = 1;
Sitkah 40:21bb685b553b 72 short SCORE_GLOBAL=0;
Artiom 78:c0533a36da8f 73 short SCORE_GR=0;
Sitkah 38:76f886a1c8e6 74 short SCORE_PR=0;
Sitkah 29:41e02746041d 75
Sitkah 29:41e02746041d 76 int flag = 0, flag_strat = 0, flag_timer;
kyxstark 87:349194e73b77 77 int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
Sitkah 29:41e02746041d 78 char Ack_strat = 0;
Sitkah 29:41e02746041d 79 signed char Strat = 0;
antbig 0:ad97421fb1fb 80 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
antbig 0:ad97421fb1fb 81
antbig 0:ad97421fb1fb 82 signed short x_robot,y_robot,theta_robot;//La position du robot
ClementBreteau 25:f140c93a8666 83 signed short target_x_robot, target_y_robot, target_theta_robot;
ClementBreteau 25:f140c93a8666 84 E_InstructionType actionPrecedente;
antbig 12:14729d584500 85 signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
Sitkah 29:41e02746041d 86 //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Sitkah 29:41e02746041d 87 int flagSendCan=1;
Artiom 83:23e04b85ae06 88 unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
Sitkah 35:742dc6b200b0 89 unsigned short angleRecalage = 0;
antbig 0:ad97421fb1fb 90 unsigned char checkCurrent = 0;
Artiom 44:badcbe8766e9 91 unsigned char countAliveCard = 0;
Sitkah 35:742dc6b200b0 92 unsigned char ligne=0;
antbig 0:ad97421fb1fb 93
Artiom 44:badcbe8766e9 94
Artiom 44:badcbe8766e9 95 int Fevitement=0;
Artiom 44:badcbe8766e9 96 int EvitEtat= 0;
Artiom 44:badcbe8766e9 97 int stop_evitement=0;
Artiom 44:badcbe8766e9 98
Artiom 44:badcbe8766e9 99
Sitkah 37:fca332b64b42 100 float angle_moyen_balise_IR = 0.0;
Sitkah 37:fca332b64b42 101
Sitkah 37:fca332b64b42 102
Sitkah 29:41e02746041d 103 signed char Strategie = 0; //N° de la strategie (1-10)
antbig 1:116040d14164 104
ClementBreteau 14:c8fc06c4887f 105 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
ClementBreteau 14:c8fc06c4887f 106
antbig 5:dcd817534b57 107 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
antbig 5:dcd817534b57 108
Artiom 44:badcbe8766e9 109 unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
Artiom 44:badcbe8766e9 110 unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
Artiom 76:b5eb99354389 111 short direction;
antbig 12:14729d584500 112
antbig 28:acd18776ed2d 113 unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
antbig 28:acd18776ed2d 114
antbig 28:acd18776ed2d 115 struct S_Instruction instruction;
antbig 28:acd18776ed2d 116
Sitkah 30:a1e37af4bbde 117 char couleur1, couleur2, couleur3;
Artiom 44:badcbe8766e9 118 float cptf;
Artiom 44:badcbe8766e9 119 int cpt,cpt1;
Sitkah 29:41e02746041d 120
Sitkah 32:1c9ab15c740e 121 typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_CAPTEURS, TEST_SERVO, TEST_TIR, DEMO_IMMEUBLE,DEMO_TRIEUR, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
Sitkah 29:41e02746041d 122 T_etat etat = INIT;
Sitkah 29:41e02746041d 123 E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
Artiom 83:23e04b85ae06 124 E_stratGameEtat memGameEtat= gameEtat;
Sitkah 29:41e02746041d 125 E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
Sitkah 38:76f886a1c8e6 126 E_Stratposdebut etat_pos=RECALAGE_1;
Sitkah 29:41e02746041d 127
Sitkah 29:41e02746041d 128 /////////////////DEFINITION DES BOUTONS////////////////////
Villanut 45:4f93e99bac6e 129 Button COTE_VERT(0, 25, 400, 300, "JAUNE");
Villanut 45:4f93e99bac6e 130 Button COTE_ORANGE(0, 350, 400, 300, "VIOLET");
kyxstark 58:faef8d1b8ed8 131 Button COTE_JAUNE(0, 25, 400, 300, "JAUNE");
kyxstark 58:faef8d1b8ed8 132 Button COTE_VIOLET(0, 350, 400, 300, "VIOLET");
Artiom 44:badcbe8766e9 133 Button RETOUR (0, 680, 400, 110, "--Precedent--");
Artiom 44:badcbe8766e9 134 Button LANCER (0, 200, 400, 200, "--LANCER--");
Artiom 44:badcbe8766e9 135 Button CHECK (0, 420, 400, 200, "Valider");
Artiom 44:badcbe8766e9 136 Button MATCH (0, 50, 400, 320, "Match");
Artiom 44:badcbe8766e9 137 Button DEMONSTRATION (0, 400, 400, 320, "Demo");
Artiom 44:badcbe8766e9 138 Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
Artiom 44:badcbe8766e9 139 Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
Artiom 44:badcbe8766e9 140 Button TEST_COULEURS(0,245,400,100,"Test capteurs");
Artiom 44:badcbe8766e9 141 Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
Artiom 44:badcbe8766e9 142 Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
Artiom 44:badcbe8766e9 143 Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
Artiom 44:badcbe8766e9 144 Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
Artiom 44:badcbe8766e9 145 Button EPURATION(0, 150, 400, 100, "epuration");
Artiom 44:badcbe8766e9 146 Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
Artiom 44:badcbe8766e9 147 Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
Artiom 44:badcbe8766e9 148 Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
Artiom 44:badcbe8766e9 149 Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
Artiom 44:badcbe8766e9 150 Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
Artiom 44:badcbe8766e9 151 Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
Artiom 44:badcbe8766e9 152 Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
Artiom 44:badcbe8766e9 153 Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
Artiom 44:badcbe8766e9 154 Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
Artiom 44:badcbe8766e9 155 Button TRI(0, 25, 400, 100, "Test tri");
Artiom 44:badcbe8766e9 156 Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
Artiom 44:badcbe8766e9 157 Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
Artiom 44:badcbe8766e9 158 Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
Artiom 44:badcbe8766e9 159 Button SUIVANT(0,380,200,100,"Suivant");
Artiom 44:badcbe8766e9 160 Button COLOR_ORANGE (0, 230, 190, 110,"");
Artiom 44:badcbe8766e9 161 Button COLOR_JAUNE (210, 230, 190, 110,"");
Artiom 44:badcbe8766e9 162 Button COLOR_VERT (0, 350, 190, 110,"");
Artiom 44:badcbe8766e9 163 Button COLOR_BLEU (210, 350, 190, 110,"");
Artiom 44:badcbe8766e9 164 Button COLOR_NOIR (105, 470, 190, 110,"");
Artiom 44:badcbe8766e9 165 ////////////////////////////////////////////////////////////
Sitkah 29:41e02746041d 166
Sitkah 29:41e02746041d 167 void SendRawId (unsigned short id);
Sitkah 29:41e02746041d 168 void SelectionStrat (unsigned char numeroStrat);
Sitkah 29:41e02746041d 169 void Setflag(void);
Sitkah 29:41e02746041d 170 void can2Rx_ISR(void);
Sitkah 29:41e02746041d 171 signed char Bouton_Strat (void);
Sitkah 41:b029ddc4d60e 172 signed char blocage_balise;
Sitkah 29:41e02746041d 173 void print_segment(int nombre, int decalage);
Sitkah 29:41e02746041d 174 void affichage_compteur (int nombre);
Sitkah 29:41e02746041d 175 void effacer_segment(long couleur);
Sitkah 29:41e02746041d 176
Sitkah 30:a1e37af4bbde 177 unsigned short telemetreDistance=0;
Sitkah 34:6aa4b46b102e 178 unsigned short telemetreDistance_avant_gauche=0;
Sitkah 34:6aa4b46b102e 179 unsigned short telemetreDistance_avant_droite=0;
Sitkah 34:6aa4b46b102e 180 unsigned short telemetreDistance_arriere_gauche=0;
Sitkah 34:6aa4b46b102e 181 unsigned short telemetreDistance_arriere_droite=0;
Sitkah 29:41e02746041d 182
Artiom 62:c4863b4b2543 183 unsigned char DT_AVD_interrupt=0;
Artiom 62:c4863b4b2543 184 unsigned char DT_AVG_interrupt=0;
Artiom 62:c4863b4b2543 185 unsigned char DT_ARD_interrupt=0;
Artiom 62:c4863b4b2543 186 unsigned char DT_ARG_interrupt=0;
Artiom 62:c4863b4b2543 187
Artiom 62:c4863b4b2543 188
Sitkah 29:41e02746041d 189 #ifdef ROBOT_BIG
Sitkah 29:41e02746041d 190
Sitkah 29:41e02746041d 191
Sitkah 38:76f886a1c8e6 192 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
Sitkah 38:76f886a1c8e6 193 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
Sitkah 29:41e02746041d 194
Artiom 50:a5361ffeefc8 195 InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
Sitkah 29:41e02746041d 196 #else
Sitkah 29:41e02746041d 197
Sitkah 29:41e02746041d 198
Sitkah 38:76f886a1c8e6 199 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
Sitkah 38:76f886a1c8e6 200 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
Artiom 50:a5361ffeefc8 201 InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
Sitkah 29:41e02746041d 202
Sitkah 29:41e02746041d 203
Sitkah 29:41e02746041d 204 #endif
Sitkah 29:41e02746041d 205
Sitkah 29:41e02746041d 206
Artiom 44:badcbe8766e9 207
Artiom 44:badcbe8766e9 208
Sitkah 29:41e02746041d 209
ClementBreteau 14:c8fc06c4887f 210
antbig 4:88431b537477 211 /****************************************************************************************/
antbig 4:88431b537477 212 /* FUNCTION NAME: chronometre_ISR */
antbig 4:88431b537477 213 /* DESCRIPTION : Interruption à la fin des 90s du match */
antbig 4:88431b537477 214 /****************************************************************************************/
antbig 0:ad97421fb1fb 215 void chronometre_ISR (void)
antbig 0:ad97421fb1fb 216 {
antbig 0:ad97421fb1fb 217 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
antbig 0:ad97421fb1fb 218 SendRawId(GLOBAL_GAME_END);//Indication fin de match
Sitkah 29:41e02746041d 219 etat=FIN;
antbig 0:ad97421fb1fb 220 gameTimer.stop();//Arret du timer
Artiom 44:badcbe8766e9 221
antbig 1:116040d14164 222 while(1);//On bloque la programme dans l'interruption
antbig 0:ad97421fb1fb 223 }
antbig 0:ad97421fb1fb 224
Sitkah 29:41e02746041d 225
Sitkah 29:41e02746041d 226
antbig 4:88431b537477 227 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 228 /* FUNCTION NAME: jack_ISR */
antbig 8:0edc7dfb7f7e 229 /* DESCRIPTION : Interruption en changement d'état sur le Jack */
antbig 8:0edc7dfb7f7e 230 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 231 void jack_ISR (void)
antbig 8:0edc7dfb7f7e 232 {
antbig 8:0edc7dfb7f7e 233 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 8:0edc7dfb7f7e 234 gameEtat = ETAT_GAME_START;//On débute le match
Sitkah 29:41e02746041d 235 etat=COMPTEUR;
Sitkah 41:b029ddc4d60e 236 blocage_balise=1;
antbig 8:0edc7dfb7f7e 237 }
antbig 8:0edc7dfb7f7e 238 }
antbig 8:0edc7dfb7f7e 239
antbig 8:0edc7dfb7f7e 240 /****************************************************************************************/
Sitkah 29:41e02746041d 241 /* FUNCTION NAME: SelectionStrat */
Sitkah 29:41e02746041d 242 /* DESCRIPTION : Affiche la Stratégie sélectionnée sur l'ihm */
Sitkah 29:41e02746041d 243 /****************************************************************************************/
Sitkah 29:41e02746041d 244
Sitkah 29:41e02746041d 245
Sitkah 29:41e02746041d 246 void SelectionStrat (unsigned char Strategie)
Sitkah 29:41e02746041d 247 {
Sitkah 29:41e02746041d 248 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 249 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 250
Artiom 44:badcbe8766e9 251 switch (Strategie+1) {
Sitkah 29:41e02746041d 252 case 0x1 :
Sitkah 29:41e02746041d 253 //description de Strategie n°1
Sitkah 29:41e02746041d 254 lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 255 break;
Artiom 44:badcbe8766e9 256
Sitkah 29:41e02746041d 257 case 0x2 :
Sitkah 29:41e02746041d 258 //description de Strategie n°2
Sitkah 29:41e02746041d 259 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 260 break;
Artiom 44:badcbe8766e9 261
Sitkah 29:41e02746041d 262 case 0x3 :
Sitkah 29:41e02746041d 263 //description de Strategie n°3
Sitkah 29:41e02746041d 264 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 265 break;
Artiom 44:badcbe8766e9 266
Sitkah 29:41e02746041d 267 case 0x4 :
Sitkah 29:41e02746041d 268 //description de Strategie n°4
Sitkah 29:41e02746041d 269 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 270 break;
Artiom 44:badcbe8766e9 271
Sitkah 29:41e02746041d 272 case 0x5 :
Sitkah 29:41e02746041d 273 //description de Strategie n°5
Sitkah 29:41e02746041d 274 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 275 break;
Artiom 44:badcbe8766e9 276
Sitkah 29:41e02746041d 277 case 0x6 :
Sitkah 29:41e02746041d 278 //description de Strategie n°5
Sitkah 29:41e02746041d 279 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 280 break;
Artiom 44:badcbe8766e9 281
Sitkah 29:41e02746041d 282 case 0x7 :
Sitkah 29:41e02746041d 283 //description de Strategie n°5
Sitkah 29:41e02746041d 284 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 285 break;
Artiom 44:badcbe8766e9 286
Sitkah 29:41e02746041d 287 case 0x8 :
Sitkah 29:41e02746041d 288 //description de Strategie n°5
Sitkah 29:41e02746041d 289 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 290 break;
Artiom 44:badcbe8766e9 291
Sitkah 29:41e02746041d 292 case 0x9 :
Sitkah 29:41e02746041d 293 //description de Strategie n°5
Sitkah 29:41e02746041d 294 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 295 break;
Artiom 44:badcbe8766e9 296
Sitkah 29:41e02746041d 297 case 0xA :
Sitkah 29:41e02746041d 298 //description de Strategie n°5
Sitkah 29:41e02746041d 299 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 300 break;
Artiom 44:badcbe8766e9 301 }
Sitkah 29:41e02746041d 302 }
Sitkah 29:41e02746041d 303
Sitkah 29:41e02746041d 304 void Setflag(void)
Sitkah 29:41e02746041d 305 {
Sitkah 29:41e02746041d 306 flagSendCan = 1;
Sitkah 29:41e02746041d 307 }
Sitkah 29:41e02746041d 308
Sitkah 42:657b6a573e11 309
Artiom 44:badcbe8766e9 310 //Affiche une variable sur l'écran tactile//
Artiom 44:badcbe8766e9 311 void affichage_var(double Var)
Artiom 44:badcbe8766e9 312 {
Sitkah 35:742dc6b200b0 313 if(ligne==7)
Sitkah 35:742dc6b200b0 314 ligne=0;
Sitkah 35:742dc6b200b0 315 char aff[10]="toto";
Sitkah 35:742dc6b200b0 316 sprintf(aff,"%lf ",Var);
Sitkah 40:21bb685b553b 317 lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE);
Sitkah 38:76f886a1c8e6 318 //ligne++;
Artiom 44:badcbe8766e9 319
Sitkah 35:742dc6b200b0 320 }
Sitkah 42:657b6a573e11 321
Sitkah 42:657b6a573e11 322
Sitkah 42:657b6a573e11 323 /****************************************************************************************/
Sitkah 42:657b6a573e11 324 /* FUNCTION NAME: affichage_debug */
Sitkah 42:657b6a573e11 325 /* DESCRIPTION : Affiche l'état de gameEtat sur l'écran lcd */
Artiom 44:badcbe8766e9 326 /****************************************************************************************/
Artiom 44:badcbe8766e9 327 void affichage_debug(int Var)
Artiom 44:badcbe8766e9 328 {
Sitkah 29:41e02746041d 329 int i;
Sitkah 29:41e02746041d 330 int conv=(int)Var;
Sitkah 29:41e02746041d 331 SUIVANT.Draw(ROUGE, 0);
Artiom 44:badcbe8766e9 332 for(i=0; i<9; i++) {
Sitkah 29:41e02746041d 333 strcpy(tableau_aff[i],"");
Sitkah 29:41e02746041d 334 strcpy(tableau_aff[i],tableau_aff[i+1]);
Sitkah 29:41e02746041d 335 }
Sitkah 29:41e02746041d 336 strcpy(tableau_aff[9],tableau_etat[conv]);
Artiom 44:badcbe8766e9 337 for(i=0; i<10; i++) {
Sitkah 34:6aa4b46b102e 338 lcd.SetBackColor(VERT);
Sitkah 29:41e02746041d 339 lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
Artiom 44:badcbe8766e9 340 }
Sitkah 42:657b6a573e11 341 /*while(!ack_bluetooth){ // mode pas à pas en bluetooth ou via écran
Sitkah 30:a1e37af4bbde 342 //liaison_bluetooth();
Sitkah 29:41e02746041d 343 }
Sitkah 30:a1e37af4bbde 344 ack_bluetooth=0;*/
Sitkah 30:a1e37af4bbde 345 /*while(SUIVANT.Touched()==0);
Sitkah 30:a1e37af4bbde 346 while(SUIVANT.Touched());*/
Artiom 44:badcbe8766e9 347 }
Sitkah 29:41e02746041d 348
Sitkah 42:657b6a573e11 349 /****************************************************************************************/
Sitkah 42:657b6a573e11 350 /* FUNCTION NAME: automate_etat_ihm */
Sitkah 42:657b6a573e11 351 /* DESCRIPTION : Automate de gestion de l'affichage */
Artiom 44:badcbe8766e9 352 /****************************************************************************************/
Sitkah 29:41e02746041d 353 void automate_etat_ihm(void)
Sitkah 29:41e02746041d 354 {
Sitkah 29:41e02746041d 355 int j;
Artiom 44:badcbe8766e9 356 if (j==0) {
Sitkah 29:41e02746041d 357 ts.Init(lcd.GetXSize(), lcd.GetYSize());
Sitkah 29:41e02746041d 358 j++;
Sitkah 29:41e02746041d 359 }
Artiom 44:badcbe8766e9 360 ts.GetState(&TS_State);
Artiom 44:badcbe8766e9 361 switch (etat) {
Sitkah 42:657b6a573e11 362 case INIT : //intialise l'écran et passe à l'attente d'initialisation des cartes
Artiom 44:badcbe8766e9 363 ts.GetState(&TS_State);
Sitkah 29:41e02746041d 364 canProcessRx();
Artiom 44:badcbe8766e9 365
Artiom 44:badcbe8766e9 366
Artiom 44:badcbe8766e9 367
Artiom 44:badcbe8766e9 368
Sitkah 29:41e02746041d 369 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 370 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 371 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 372 wait(0.15);
Sitkah 29:41e02746041d 373 lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE);
Sitkah 29:41e02746041d 374 //cartes non verifiées////////////////
Sitkah 29:41e02746041d 375 lcd.SetTextColor(DIY_GREY);
Sitkah 29:41e02746041d 376 lcd.FillRect(0,400,400,150); //carte moteur
Sitkah 29:41e02746041d 377 lcd.FillRect(0,600,400,150); //Balise
Artiom 44:badcbe8766e9 378
Sitkah 29:41e02746041d 379 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 380 lcd.SetBackColor(DIY_GREY);
Sitkah 29:41e02746041d 381 lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
Artiom 44:badcbe8766e9 382 lcd.DisplayStringAt(110,650, (uint8_t *)"Balise", LEFT_MODE);
Sitkah 29:41e02746041d 383 ////////////////////////////////////////
Artiom 44:badcbe8766e9 384
Artiom 44:badcbe8766e9 385 FORCE_LAUNCH.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 386
Sitkah 29:41e02746041d 387 etat=ATT;
Sitkah 29:41e02746041d 388 break;
Artiom 44:badcbe8766e9 389
Sitkah 42:657b6a573e11 390 case ATT : //Si les cartes sont présentes passe directement à choix sinon attente de force Launch (cette partie est encore buggée mais les cartes affichent bien leur présence donc faut juste force launch tout le temps...)
Artiom 44:badcbe8766e9 391 if (flag==1) {
Sitkah 29:41e02746041d 392 etat = CHOIX;
Sitkah 29:41e02746041d 393 gameEtat = ETAT_CONFIG;
Artiom 44:badcbe8766e9 394 } else if (FORCE_LAUNCH.Touched()) {
Sitkah 29:41e02746041d 395 etat = CHOIX;
Sitkah 29:41e02746041d 396 gameEtat = ETAT_CONFIG;
Sitkah 30:a1e37af4bbde 397 while(FORCE_LAUNCH.Touched());
Sitkah 29:41e02746041d 398 }
Artiom 44:badcbe8766e9 399
Sitkah 29:41e02746041d 400 break;
Artiom 44:badcbe8766e9 401
Artiom 44:badcbe8766e9 402
Sitkah 42:657b6a573e11 403 case CHOIX : //Match ou DEMO
Sitkah 29:41e02746041d 404 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 405 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 406 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 407 lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE);
Sitkah 29:41e02746041d 408 DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
Sitkah 29:41e02746041d 409 MATCH.Draw(0xFFF01010, 0);
Artiom 44:badcbe8766e9 410 while(etat == CHOIX) {
Sitkah 29:41e02746041d 411 canProcessRx();
Artiom 44:badcbe8766e9 412 if(DEMONSTRATION.Touched()) {
Sitkah 29:41e02746041d 413 etat = DEMO;
Sitkah 29:41e02746041d 414 while(DEMONSTRATION.Touched());
Sitkah 29:41e02746041d 415 }
Artiom 44:badcbe8766e9 416
Artiom 44:badcbe8766e9 417 if(MATCH.Touched()) {
Sitkah 29:41e02746041d 418 etat = SELECT_SIDE;
Sitkah 29:41e02746041d 419 while(MATCH.Touched());
Sitkah 29:41e02746041d 420 }
Sitkah 29:41e02746041d 421
Sitkah 29:41e02746041d 422 }
Sitkah 29:41e02746041d 423 break;
Artiom 44:badcbe8766e9 424
Artiom 44:badcbe8766e9 425 case DEMO :
Sitkah 29:41e02746041d 426 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 427 RETOUR.Draw(0xFFFF0000, 0);
Sitkah 34:6aa4b46b102e 428 TEST_HERKULEX.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 429 TEST_LASER.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 430 TEST_COULEURS.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 431 TEST_TIR_BALLE.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 432 TEST_IMMEUBLE.Draw(VERT,0);
Sitkah 34:6aa4b46b102e 433 TEST_TRIEUR.Draw(VERT,0);
Sitkah 42:657b6a573e11 434 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config//
Artiom 44:badcbe8766e9 435 InversStrat = 0;//Pas d'inversion de la couleur
Sitkah 29:41e02746041d 436 }
Artiom 44:badcbe8766e9 437 while (etat == DEMO) { ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
Sitkah 29:41e02746041d 438 canProcessRx();
Artiom 44:badcbe8766e9 439 if(TEST_HERKULEX.Touched()) {
Sitkah 30:a1e37af4bbde 440 //Strat = 0x10;
Sitkah 31:833fc481b002 441 while(TEST_HERKULEX.Touched());
Sitkah 32:1c9ab15c740e 442 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 443 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 444 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 445 trame_Tx.type = CANData;
Sitkah 31:833fc481b002 446 trame_Tx.id=CHOICE_COLOR;
Sitkah 32:1c9ab15c740e 447 trame_Tx.data[0]=0x2;
Sitkah 31:833fc481b002 448 can2.write(trame_Tx);
Sitkah 31:833fc481b002 449 TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
Sitkah 31:833fc481b002 450 etat = TEST_SERVO;
Artiom 44:badcbe8766e9 451 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 30:a1e37af4bbde 452 ModeDemo=1;
Artiom 44:badcbe8766e9 453 } else if(TEST_LASER.Touched()) {
Artiom 44:badcbe8766e9 454 //Strat = 0x11;
Artiom 44:badcbe8766e9 455 while(TEST_LASER.Touched());
Sitkah 31:833fc481b002 456 TEST_LASER.Draw(0xFFF0F0F0, 0);
Sitkah 31:833fc481b002 457 etat = TEST_TELEMETRE;
Artiom 44:badcbe8766e9 458 } else if (TEST_COULEURS.Touched()) {
Sitkah 31:833fc481b002 459 while(TEST_COULEURS.Touched());
Artiom 44:badcbe8766e9 460 TEST_LASER.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 461 etat =TEST_CAPTEURS ;
Sitkah 31:833fc481b002 462 }
Artiom 44:badcbe8766e9 463
Artiom 44:badcbe8766e9 464 else if (TEST_TIR_BALLE.Touched()) {
Sitkah 31:833fc481b002 465 while(TEST_TIR_BALLE.Touched());
Artiom 62:c4863b4b2543 466 while(1) {
Artiom 62:c4863b4b2543 467 TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
Artiom 79:d9c64cd4588c 468 // rn42_Tx.printf("A");//experience
Artiom 79:d9c64cd4588c 469 #ifdef ROBOT_SMALL
Artiom 79:d9c64cd4588c 470 liaison_Tx.envoyer_short(0x30,666);
Artiom 79:d9c64cd4588c 471 pc.printf("data\r");
Artiom 79:d9c64cd4588c 472 #else
Artiom 80:545a9bf561e1 473
Artiom 80:545a9bf561e1 474 // lire();
Artiom 80:545a9bf561e1 475 /* if (bluetooth.readable())
Artiom 80:545a9bf561e1 476 pc.putc(bluetooth.getc());
Artiom 80:545a9bf561e1 477
Artiom 80:545a9bf561e1 478 if (pc.readable()) {
Artiom 80:545a9bf561e1 479 char c = pc.getc();
Artiom 80:545a9bf561e1 480 if (c == 'A') {
Artiom 80:545a9bf561e1 481 liaison.envoyer_short(PAQUET_IDENTIFIANT_RAFRAICHIRSCORE, 20);
Artiom 80:545a9bf561e1 482 pc.printf("rafraichir\n");
Artiom 80:545a9bf561e1 483 }
Artiom 80:545a9bf561e1 484 }*/
Artiom 79:d9c64cd4588c 485 #endif
Artiom 61:4046a91e1b0e 486 }
Artiom 61:4046a91e1b0e 487 //ModeDemo=1;
Artiom 44:badcbe8766e9 488 } else if(TEST_IMMEUBLE.Touched()) {
Artiom 44:badcbe8766e9 489 while(TEST_IMMEUBLE.Touched());
Artiom 44:badcbe8766e9 490 TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 491 etat =DEMO_IMMEUBLE;
Artiom 44:badcbe8766e9 492 lcd.Clear(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 493 } else if(TEST_TRIEUR.Touched()) {
Sitkah 32:1c9ab15c740e 494 while(TEST_TRIEUR.Touched());
Sitkah 32:1c9ab15c740e 495 etat=DEMO_TRIEUR;
Sitkah 32:1c9ab15c740e 496 lcd.Clear(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 497 }
Artiom 44:badcbe8766e9 498 if(RETOUR.Touched()) {
Sitkah 29:41e02746041d 499 etat = CHOIX;
Sitkah 29:41e02746041d 500 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 501
Sitkah 29:41e02746041d 502 }
Sitkah 29:41e02746041d 503 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
Sitkah 29:41e02746041d 504 Ack_strat = 1;
Sitkah 29:41e02746041d 505 wait_ms(10);
Sitkah 29:41e02746041d 506 }
Sitkah 29:41e02746041d 507 }
Sitkah 29:41e02746041d 508 break;
Villanut 75:1db1b929f13d 509
Artiom 44:badcbe8766e9 510
Sitkah 42:657b6a573e11 511 case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
Sitkah 31:833fc481b002 512 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 513 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 514 ABAISSE_BLOC.Draw(VERT, 0);
Sitkah 31:833fc481b002 515 RELEVE_BLOC.Draw(VERT, 0);
Sitkah 31:833fc481b002 516 BRAS_ABEILLE_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 517 BRAS_ABEILLE_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 518 INTERRUPTEUR_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 519 INTERRUPTEUR_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 520 RETOUR.Draw(0xFFFF0000,0);
Artiom 44:badcbe8766e9 521 while(etat==TEST_SERVO) {
Artiom 44:badcbe8766e9 522 if(RETOUR.Touched()) {
Sitkah 31:833fc481b002 523 while (RETOUR.Touched());
Sitkah 31:833fc481b002 524 etat=DEMO;
Artiom 44:badcbe8766e9 525 } else if(ABAISSE_BLOC.Touched()) {
Artiom 44:badcbe8766e9 526 while (ABAISSE_BLOC.Touched());
Villanut 45:4f93e99bac6e 527 SendRawId(GABARIT_PETIT_ROBOT);
Sitkah 31:833fc481b002 528 break;
Artiom 44:badcbe8766e9 529 } else if(RELEVE_BLOC.Touched()) {
Artiom 44:badcbe8766e9 530 while (RELEVE_BLOC.Touched());
Villanut 45:4f93e99bac6e 531 SendRawId(PRESENTOIR_AVANT);
Sitkah 31:833fc481b002 532 break;
Artiom 44:badcbe8766e9 533 } else if(BRAS_ABEILLE_ON.Touched()) {
Sitkah 31:833fc481b002 534 while (BRAS_ABEILLE_ON.Touched());
kyxstark 55:1e3dab1f90f4 535 //SendRawId(BRAS_ABEILLE_UP);
Sitkah 31:833fc481b002 536 break;
Artiom 44:badcbe8766e9 537
Artiom 44:badcbe8766e9 538 } else if(BRAS_ABEILLE_OFF.Touched()) {
Artiom 44:badcbe8766e9 539 while (BRAS_ABEILLE_OFF.Touched());
kyxstark 55:1e3dab1f90f4 540 //SendRawId(BRAS_ABEILLE_DOWN);
Artiom 44:badcbe8766e9 541 break;
Artiom 44:badcbe8766e9 542 } else if(INTERRUPTEUR_ON.Touched()) {
Artiom 44:badcbe8766e9 543 while (INTERRUPTEUR_ON.Touched());
kyxstark 55:1e3dab1f90f4 544 //SendRawId(ALLUMER_PANNEAU_UP);
Artiom 44:badcbe8766e9 545 break;
Artiom 44:badcbe8766e9 546 } else if(INTERRUPTEUR_OFF.Touched()) {
Artiom 44:badcbe8766e9 547 while (INTERRUPTEUR_OFF.Touched());
kyxstark 55:1e3dab1f90f4 548 //SendRawId(ALLUMER_PANNEAU_DOWN);
Sitkah 31:833fc481b002 549 break;
Sitkah 31:833fc481b002 550 }
Artiom 44:badcbe8766e9 551 }
Sitkah 31:833fc481b002 552 break;
Artiom 44:badcbe8766e9 553
Sitkah 42:657b6a573e11 554 case TEST_TIR: // TEST DES FONCTIONS LIEES AUX TIRS
Sitkah 31:833fc481b002 555 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 556 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 557 TIR_CHATEAU.Draw(VERT, 0);
Sitkah 31:833fc481b002 558 EPURATION.Draw(VERT, 0);
Sitkah 31:833fc481b002 559 LANCEUR_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 560 LANCEUR_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 561 RETOUR.Draw(ROUGE, 0);
Artiom 62:c4863b4b2543 562
Artiom 67:96f914f92d2d 563 etat=DEMO;
Villanut 75:1db1b929f13d 564
Artiom 44:badcbe8766e9 565 break;
Artiom 44:badcbe8766e9 566
Artiom 44:badcbe8766e9 567
Artiom 44:badcbe8766e9 568
Sitkah 42:657b6a573e11 569 case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
Sitkah 31:833fc481b002 570 ModeDemo=1;
Sitkah 31:833fc481b002 571 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 572 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 573 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 574 RETOUR.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 575 while(etat==TEST_TELEMETRE) {
Sitkah 31:833fc481b002 576 SendRawId(DATA_RECALAGE);
Artiom 62:c4863b4b2543 577 SendRawId(DATA_TELEMETRE_LOGIQUE);
Artiom 50:a5361ffeefc8 578 wait_ms(100);
Sitkah 31:833fc481b002 579 canProcessRx();
Artiom 44:badcbe8766e9 580 if(RETOUR.Touched()) {
Artiom 44:badcbe8766e9 581 while( RETOUR.Touched());
Artiom 44:badcbe8766e9 582 etat=DEMO;
Artiom 44:badcbe8766e9 583 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 584 }
Sitkah 29:41e02746041d 585 }
Sitkah 42:657b6a573e11 586 break; ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////
Artiom 44:badcbe8766e9 587
Artiom 44:badcbe8766e9 588
Sitkah 42:657b6a573e11 589 case SELECT_SIDE : // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS
Sitkah 29:41e02746041d 590 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 591 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 592 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 593
Sitkah 29:41e02746041d 594 lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
Artiom 62:c4863b4b2543 595 COTE_JAUNE.Draw(JAUNE, 0);
Artiom 62:c4863b4b2543 596 COTE_VIOLET.Draw(VIOLET, 0);
Sitkah 29:41e02746041d 597 RETOUR.Draw(LCD_COLOR_RED, 0);
Artiom 44:badcbe8766e9 598
Artiom 44:badcbe8766e9 599
Artiom 44:badcbe8766e9 600 while (etat == SELECT_SIDE) {
Sitkah 29:41e02746041d 601 canProcessRx();
kyxstark 58:faef8d1b8ed8 602 if(COTE_JAUNE.Touched()) {
Sitkah 29:41e02746041d 603 Cote = 0x0;
Sitkah 29:41e02746041d 604 InversStrat = Cote;
Sitkah 29:41e02746041d 605 etat = TACTIQUE;
Sitkah 32:1c9ab15c740e 606 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 607 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 608 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 609 trame_Tx.type = CANData;
Sitkah 30:a1e37af4bbde 610 trame_Tx.id=CHOICE_COLOR;
Sitkah 30:a1e37af4bbde 611 trame_Tx.data[0]=Cote;
Sitkah 30:a1e37af4bbde 612 can2.write(trame_Tx);
kyxstark 58:faef8d1b8ed8 613 while(COTE_JAUNE.Touched());
Artiom 44:badcbe8766e9 614
Sitkah 29:41e02746041d 615 }
Artiom 44:badcbe8766e9 616
kyxstark 58:faef8d1b8ed8 617 if(COTE_VIOLET.Touched()) {
Sitkah 29:41e02746041d 618 Cote = 0x1;
Sitkah 29:41e02746041d 619 InversStrat= Cote;
Sitkah 29:41e02746041d 620 etat = TACTIQUE;
Sitkah 32:1c9ab15c740e 621 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 622 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 623 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 624 trame_Tx.type = CANData;
Sitkah 30:a1e37af4bbde 625 trame_Tx.id=CHOICE_COLOR;
Sitkah 30:a1e37af4bbde 626 trame_Tx.data[0]=Cote;
Sitkah 30:a1e37af4bbde 627 can2.write(trame_Tx);
kyxstark 58:faef8d1b8ed8 628 while(COTE_VIOLET.Touched());
Sitkah 29:41e02746041d 629 }
Artiom 44:badcbe8766e9 630
Artiom 44:badcbe8766e9 631 if(RETOUR.Touched()) {
Sitkah 29:41e02746041d 632 etat = CHOIX;
Sitkah 29:41e02746041d 633 while(RETOUR.Touched());
Sitkah 29:41e02746041d 634 }
Sitkah 29:41e02746041d 635 }
Artiom 44:badcbe8766e9 636
Sitkah 29:41e02746041d 637 break;
Artiom 44:badcbe8766e9 638
Sitkah 42:657b6a573e11 639 case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE
Artiom 44:badcbe8766e9 640 if (Cote == 0) {
kyxstark 58:faef8d1b8ed8 641 lcd.Clear(JAUNE);
kyxstark 58:faef8d1b8ed8 642 lcd.SetBackColor(JAUNE);
Artiom 44:badcbe8766e9 643 } else if (Cote == 1) {
kyxstark 58:faef8d1b8ed8 644 lcd.Clear(VIOLET);
kyxstark 58:faef8d1b8ed8 645 lcd.SetBackColor(VIOLET);
Artiom 44:badcbe8766e9 646 } else {
Sitkah 38:76f886a1c8e6 647 lcd.Clear(BLEU);
Sitkah 38:76f886a1c8e6 648 lcd.SetBackColor(BLEU);
Artiom 44:badcbe8766e9 649 }
Artiom 44:badcbe8766e9 650
Artiom 44:badcbe8766e9 651 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 652
Sitkah 29:41e02746041d 653 lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE);
Artiom 44:badcbe8766e9 654
Sitkah 29:41e02746041d 655 Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
Artiom 44:badcbe8766e9 656 if (Strategie == -1) {
Sitkah 29:41e02746041d 657 etat = SELECT_SIDE;
Artiom 44:badcbe8766e9 658 } else {
Artiom 44:badcbe8766e9 659 etat = DETAILS;
Sitkah 29:41e02746041d 660 }
Sitkah 29:41e02746041d 661 wait(0.1);
Sitkah 29:41e02746041d 662 break;
Artiom 44:badcbe8766e9 663
Sitkah 42:657b6a573e11 664 case DETAILS : //SECONDE VALIDATION DE LA STRAT
Sitkah 29:41e02746041d 665 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 666 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 667 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 668 CHECK.Draw(VERT);
Sitkah 29:41e02746041d 669 RETOUR.Draw(LCD_COLOR_RED);
Artiom 44:badcbe8766e9 670
Sitkah 42:657b6a573e11 671 SelectionStrat(Strategie); //affiche la stratégie selectionnée
Artiom 44:badcbe8766e9 672
Artiom 44:badcbe8766e9 673 while (etat == DETAILS) {
Sitkah 29:41e02746041d 674 canProcessRx();
Artiom 44:badcbe8766e9 675 if (CHECK.Touched()) {
Artiom 44:badcbe8766e9 676 if(gameEtat == ETAT_CONFIG) {
Artiom 44:badcbe8766e9 677 gameEtat = ETAT_GAME_INIT;
Artiom 44:badcbe8766e9 678 etat=LECTURE;
Artiom 44:badcbe8766e9 679
Sitkah 29:41e02746041d 680 }
Artiom 44:badcbe8766e9 681 while(CHECK.Touched());
Artiom 44:badcbe8766e9 682 }
Artiom 44:badcbe8766e9 683
Artiom 44:badcbe8766e9 684 if(RETOUR.Touched()) {
Artiom 44:badcbe8766e9 685 etat = TACTIQUE;
Artiom 44:badcbe8766e9 686 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 687 }
Artiom 44:badcbe8766e9 688 }
Sitkah 29:41e02746041d 689 break;
Artiom 44:badcbe8766e9 690
Artiom 44:badcbe8766e9 691
Sitkah 29:41e02746041d 692 case LECTURE :
Artiom 44:badcbe8766e9 693 break;
Sitkah 42:657b6a573e11 694 case AFF_WAIT_JACK : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK
Sitkah 29:41e02746041d 695 lcd.Clear(BLANC);
Sitkah 29:41e02746041d 696 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 697 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 698
Artiom 44:badcbe8766e9 699 if (Cote == 0) {
Sitkah 34:6aa4b46b102e 700 lcd.Clear(VERT);
Sitkah 34:6aa4b46b102e 701 lcd.SetBackColor(VERT);
Artiom 44:badcbe8766e9 702 } else if (Cote == 1) {
Sitkah 38:76f886a1c8e6 703 lcd.Clear(ORANGE);
Sitkah 38:76f886a1c8e6 704 lcd.SetBackColor(ORANGE);
Artiom 44:badcbe8766e9 705 } else {
Sitkah 34:6aa4b46b102e 706 lcd.Clear(VERT);
Sitkah 34:6aa4b46b102e 707 lcd.SetBackColor(VERT);
Sitkah 29:41e02746041d 708 }
Sitkah 29:41e02746041d 709 canProcessRx();
Artiom 44:badcbe8766e9 710 lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
Sitkah 29:41e02746041d 711 etat=WAIT_JACK;
Sitkah 29:41e02746041d 712 break;
Artiom 44:badcbe8766e9 713
Sitkah 42:657b6a573e11 714 case WAIT_JACK: //VERITABLE ATTENTE DU JACK
Artiom 44:badcbe8766e9 715 break;
Artiom 44:badcbe8766e9 716
Sitkah 42:657b6a573e11 717 case COMPTEUR: //PEUT AFFICHER UN COMPTEUR DU TEMPS RESTANT AVANT LA FIN DE LA PARTIE OU BIEN TRES UTILE POUR PRINT DES VARIABLES CHAQUE SEC EX: gameEtat
Sitkah 38:76f886a1c8e6 718 cptf=gameTimer.read();
Sitkah 29:41e02746041d 719 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 38:76f886a1c8e6 720 cpt=(int)cptf;
Artiom 44:badcbe8766e9 721 if(cpt != cpt1) {
Sitkah 34:6aa4b46b102e 722 lcd.Clear(VERT);
Artiom 44:badcbe8766e9 723 // affichage_compteur(100-cpt);
Sitkah 40:21bb685b553b 724 //affichage_compteur(SCORE_PR);
Artiom 44:badcbe8766e9 725 #ifdef ROBOT_BIG
Artiom 44:badcbe8766e9 726 affichage_var(SCORE_GR);
Artiom 44:badcbe8766e9 727 #else
Artiom 44:badcbe8766e9 728 affichage_var(SCORE_PR);
Artiom 44:badcbe8766e9 729 #endif
Artiom 44:badcbe8766e9 730 if(liaison_pr.paquet_en_attente()) {
Sitkah 38:76f886a1c8e6 731 PaquetDomotique *paquet=liaison_pr.lire();
Artiom 44:badcbe8766e9 732 if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) {
Sitkah 38:76f886a1c8e6 733 SCORE_PR+=convertir_score(paquet);
Sitkah 38:76f886a1c8e6 734 }
Sitkah 38:76f886a1c8e6 735 delete paquet;
Sitkah 38:76f886a1c8e6 736 }
Sitkah 36:6dd30780bd8e 737 }
Sitkah 29:41e02746041d 738 cpt1=cpt;
Sitkah 29:41e02746041d 739 flag_timer=0;
Sitkah 29:41e02746041d 740
Sitkah 38:76f886a1c8e6 741 //affichage_debug(gameEtat);
Sitkah 34:6aa4b46b102e 742 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 743
Sitkah 29:41e02746041d 744 break;
Artiom 44:badcbe8766e9 745
Sitkah 42:657b6a573e11 746 case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL
Sitkah 29:41e02746041d 747 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 748 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 749 #ifdef ROBOT_BIG
Artiom 44:badcbe8766e9 750 // affichage_compteur(SCORE_GR);
Artiom 44:badcbe8766e9 751 affichage_var(SCORE_GR);
Artiom 44:badcbe8766e9 752 //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
Artiom 44:badcbe8766e9 753 #else
Artiom 44:badcbe8766e9 754 //affichage_compteur(SCORE_PR);
Artiom 44:badcbe8766e9 755 affichage_var(SCORE_PR);
Artiom 44:badcbe8766e9 756 #endif
Artiom 44:badcbe8766e9 757 while(1); // force le redemarage du robot
Sitkah 29:41e02746041d 758 //break;
Artiom 44:badcbe8766e9 759
Artiom 44:badcbe8766e9 760 }
Sitkah 29:41e02746041d 761 }
Sitkah 29:41e02746041d 762
Sitkah 29:41e02746041d 763
Sitkah 29:41e02746041d 764
Sitkah 29:41e02746041d 765 /****************************************************************************************/
antbig 4:88431b537477 766 /* FUNCTION NAME: automate_process */
antbig 4:88431b537477 767 /* DESCRIPTION : Automate de gestion de la stratégie du robot */
antbig 4:88431b537477 768 /****************************************************************************************/
Artiom 44:badcbe8766e9 769 void automate_process(void)
Artiom 44:badcbe8766e9 770 {
antbig 1:116040d14164 771 static unsigned char AX12_enchainement = 0;
antbig 1:116040d14164 772 static unsigned char MV_enchainement = 0;
antbig 0:ad97421fb1fb 773 signed char localData1 = 0;
antbig 0:ad97421fb1fb 774 signed short localData2 = 0;
antbig 0:ad97421fb1fb 775 unsigned short localData3 = 0;
ClementBreteau 14:c8fc06c4887f 776 //signed short localData4 = 0;
antbig 1:116040d14164 777 unsigned char localData5 = 0;
Artiom 44:badcbe8766e9 778
Artiom 44:badcbe8766e9 779
Sitkah 34:6aa4b46b102e 780 if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
antbig 0:ad97421fb1fb 781 gameTimer.stop();
antbig 0:ad97421fb1fb 782 gameTimer.reset();
antbig 0:ad97421fb1fb 783 gameEtat = ETAT_END;//Fin du temps
Sitkah 29:41e02746041d 784 etat=FIN;
antbig 0:ad97421fb1fb 785 }
Artiom 44:badcbe8766e9 786
antbig 0:ad97421fb1fb 787 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
antbig 0:ad97421fb1fb 788 lastEtat = gameEtat;
antbig 0:ad97421fb1fb 789 debugetatTimer.reset();
antbig 11:ed13a480ddca 790 sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
antbig 0:ad97421fb1fb 791 }
Artiom 44:badcbe8766e9 792
Artiom 44:badcbe8766e9 793 switch(gameEtat) {
Artiom 44:badcbe8766e9 794
Artiom 44:badcbe8766e9 795 case ETAT_CHECK_CARTES:
antbig 0:ad97421fb1fb 796 /*
antbig 0:ad97421fb1fb 797 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
antbig 0:ad97421fb1fb 798 */
antbig 0:ad97421fb1fb 799 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
antbig 11:ed13a480ddca 800 SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
Artiom 44:badcbe8766e9 801
antbig 0:ad97421fb1fb 802 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 803 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 804 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 805 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
Sitkah 29:41e02746041d 806 break;
Artiom 44:badcbe8766e9 807
antbig 0:ad97421fb1fb 808 case ETAT_CHECK_CARTES_WAIT_ACK:
antbig 0:ad97421fb1fb 809 /*
antbig 0:ad97421fb1fb 810 On attend l'ack de la carte en cours de vérification
antbig 0:ad97421fb1fb 811 */
antbig 0:ad97421fb1fb 812 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
antbig 0:ad97421fb1fb 813 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 814 cartesCheker.stop();
antbig 0:ad97421fb1fb 815 screenChecktry = 0;
antbig 0:ad97421fb1fb 816 countAliveCard++;
antbig 11:ed13a480ddca 817 checkCurrent++;
antbig 0:ad97421fb1fb 818 if(checkCurrent >= NOMBRE_CARTES) {
Sitkah 29:41e02746041d 819 printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
antbig 0:ad97421fb1fb 820 if(countAliveCard >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 821 gameEtat = ETAT_CONFIG;
Sitkah 38:76f886a1c8e6 822 SendRawId(ECRAN_ALL_CHECK);
Sitkah 29:41e02746041d 823 flag=1;
Artiom 44:badcbe8766e9 824
Sitkah 30:a1e37af4bbde 825 //tactile_printf("Selection couleur et strategie");
Artiom 44:badcbe8766e9 826 } else {
antbig 0:ad97421fb1fb 827 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 828 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 829 }
Artiom 44:badcbe8766e9 830 } else
antbig 0:ad97421fb1fb 831 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 832 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 833 cartesCheker.stop();
antbig 0:ad97421fb1fb 834 if(screenChecktry >=3) {
antbig 12:14729d584500 835 //printf("missing card %d\n",id_check[checkCurrent]);
antbig 0:ad97421fb1fb 836 screenChecktry = 0;
antbig 11:ed13a480ddca 837 checkCurrent++;
Artiom 44:badcbe8766e9 838
Artiom 44:badcbe8766e9 839 if(checkCurrent >= NOMBRE_CARTES) {
Artiom 44:badcbe8766e9 840 if(countAliveCard == NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 841 gameEtat = ETAT_CONFIG;
Artiom 44:badcbe8766e9 842 flag=1;
Artiom 44:badcbe8766e9 843 } else {
Sitkah 29:41e02746041d 844 gameEtat = ETAT_WAIT_FORCE;
antbig 0:ad97421fb1fb 845 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 846 }
Artiom 44:badcbe8766e9 847 } else
antbig 0:ad97421fb1fb 848 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 849
Artiom 44:badcbe8766e9 850 } else
antbig 0:ad97421fb1fb 851 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 852
antbig 0:ad97421fb1fb 853 }
Sitkah 29:41e02746041d 854 break;
antbig 0:ad97421fb1fb 855 case ETAT_WAIT_FORCE:
antbig 0:ad97421fb1fb 856 /*
antbig 0:ad97421fb1fb 857 Attente du forçage de la part de la carte IHM
antbig 0:ad97421fb1fb 858 */
antbig 0:ad97421fb1fb 859 if(waitingAckFrom == 0) {
antbig 0:ad97421fb1fb 860 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 861 }
Sitkah 29:41e02746041d 862 break;
antbig 0:ad97421fb1fb 863 case ETAT_CONFIG:
antbig 0:ad97421fb1fb 864 /*
antbig 0:ad97421fb1fb 865 Attente de l'odre de choix de mode,
antbig 0:ad97421fb1fb 866 Il est possible de modifier la couleur et l'id de la stratégie
antbig 0:ad97421fb1fb 867 Il est aussi possible d'envoyer les ordres de debug
antbig 0:ad97421fb1fb 868 */
ClementBreteau 14:c8fc06c4887f 869 modeTelemetre = 0;
Sitkah 29:41e02746041d 870 break;
antbig 1:116040d14164 871 case ETAT_GAME_INIT:
antbig 0:ad97421fb1fb 872 //On charge la liste des instructions
Artiom 44:badcbe8766e9 873
Sitkah 29:41e02746041d 874 loadAllInstruction(Strategie);//Mise en cache de toute les instructions
Sitkah 29:41e02746041d 875 led3=1;
Artiom 44:badcbe8766e9 876
Sitkah 37:fca332b64b42 877 SendRawId(GLOBAL_START);
Artiom 44:badcbe8766e9 878
antbig 0:ad97421fb1fb 879 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
Artiom 44:badcbe8766e9 880 if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) {
Sitkah 29:41e02746041d 881 SendRawId(DEBUG_FAKE_JAKE);
Artiom 44:badcbe8766e9 882 } else {
Artiom 44:badcbe8766e9 883 etat = AFF_WAIT_JACK;
Sitkah 29:41e02746041d 884 }
Sitkah 30:a1e37af4bbde 885 //tactile_printf("Attente du JACK.");
antbig 12:14729d584500 886 setAsservissementEtat(1);//On réactive l'asservissement
antbig 12:14729d584500 887 jack.mode(PullDown); // désactivation de la résistance interne du jack
antbig 8:0edc7dfb7f7e 888 jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
Artiom 44:badcbe8766e9 889
clementlignie 22:a466d08ac42b 890 localData2 = POSITION_DEBUT_T;
clementlignie 22:a466d08ac42b 891 localData3 = POSITION_DEBUT_Y;
clementlignie 22:a466d08ac42b 892 if(InversStrat == 1) {
clementlignie 22:a466d08ac42b 893 localData2 = -localData2;//Inversion theta
clementlignie 22:a466d08ac42b 894 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
clementlignie 22:a466d08ac42b 895 }
Sitkah 38:76f886a1c8e6 896 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2);
Artiom 44:badcbe8766e9 897
Sitkah 38:76f886a1c8e6 898 instruction = strat_instructions[actual_instruction];
Artiom 44:badcbe8766e9 899 //On effectue le traitement de l'instruction
Artiom 44:badcbe8766e9 900
Artiom 44:badcbe8766e9 901 break;
antbig 0:ad97421fb1fb 902 case ETAT_GAME_WAIT_FOR_JACK:
Artiom 44:badcbe8766e9 903 if(instruction.order==POSITION_DEBUT) {
Artiom 44:badcbe8766e9 904 switch(etat_pos) { // AUTOMATE PERMETTANT AU ROBOT DE SE POSITIONNER TOUT SEUL AU DEBUT DE LA PARTIE (Ne PAS RETIRER LE JACK PENDANT CE TEMPS !!!)
Sitkah 38:76f886a1c8e6 905 case RECALAGE_1:
Artiom 80:545a9bf561e1 906 SendRawId(RECALAGE_START);
Sitkah 38:76f886a1c8e6 907 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 908 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 909 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 910 GoStraight(3000, 1,MOITIEE_ROBOT, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
Artiom 44:badcbe8766e9 911 #else
Artiom 46:a9b6bcb30b1c 912 GoStraight(-3000, 1,MOITIEE_ROBOT, 0);
Artiom 44:badcbe8766e9 913 #endif
Sitkah 38:76f886a1c8e6 914 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 915 canProcessRx();
Sitkah 38:76f886a1c8e6 916 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 917 waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 918 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 919 canProcessRx();
Sitkah 38:76f886a1c8e6 920 etat_pos=RECULER_1;
Sitkah 38:76f886a1c8e6 921 break;
Artiom 44:badcbe8766e9 922
Sitkah 38:76f886a1c8e6 923 case RECULER_1:
Sitkah 38:76f886a1c8e6 924 waitingAckID = ASSERVISSEMENT_RECALAGE;
Sitkah 38:76f886a1c8e6 925 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 926 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 927 GoStraight(-100, 0, 0, 0);//-450
Artiom 44:badcbe8766e9 928 #else
Villanut 72:5b1b3e151e59 929 GoStraight(250, 0, 0, 0);
Artiom 44:badcbe8766e9 930 #endif
Sitkah 38:76f886a1c8e6 931 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 932 canProcessRx();
Sitkah 38:76f886a1c8e6 933 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 934 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 935 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 936 canProcessRx();
Sitkah 38:76f886a1c8e6 937 etat_pos=TOURNER;
Artiom 44:badcbe8766e9 938 break;
Artiom 44:badcbe8766e9 939
Sitkah 38:76f886a1c8e6 940 case TOURNER:
Sitkah 38:76f886a1c8e6 941 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 942 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 943 if(Cote==0) {
Sitkah 38:76f886a1c8e6 944 localData2 = 900;
Artiom 44:badcbe8766e9 945 } else {
Sitkah 38:76f886a1c8e6 946 localData2=-900;
Sitkah 38:76f886a1c8e6 947 }
Sitkah 38:76f886a1c8e6 948 Rotate(localData2);
Sitkah 38:76f886a1c8e6 949 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 950 canProcessRx();
Sitkah 38:76f886a1c8e6 951 waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 952 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 953 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 954 canProcessRx();
Sitkah 38:76f886a1c8e6 955 etat_pos=RECALAGE_2;
Artiom 44:badcbe8766e9 956 break;
Artiom 44:badcbe8766e9 957
Sitkah 38:76f886a1c8e6 958 case RECALAGE_2:
Sitkah 38:76f886a1c8e6 959 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 960 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 80:545a9bf561e1 961 if(Cote==1) {
Artiom 46:a9b6bcb30b1c 962 localData3=3000-(MOITIEE_ROBOT);
Artiom 80:545a9bf561e1 963 } else {
Sitkah 38:76f886a1c8e6 964 localData3=MOITIEE_ROBOT;
Artiom 80:545a9bf561e1 965 }
Artiom 44:badcbe8766e9 966 #ifdef ROBOT_SMALL
Sitkah 38:76f886a1c8e6 967 GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
Artiom 44:badcbe8766e9 968 #else
Sitkah 38:76f886a1c8e6 969 GoStraight(-3000, 2,localData3, 0);
Artiom 44:badcbe8766e9 970 #endif
Sitkah 38:76f886a1c8e6 971 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 972 canProcessRx();
Sitkah 38:76f886a1c8e6 973 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 974 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 975 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 976 canProcessRx();
Sitkah 38:76f886a1c8e6 977 etat_pos=RECULER_2;
Artiom 44:badcbe8766e9 978 break;
Artiom 44:badcbe8766e9 979
Artiom 44:badcbe8766e9 980 case RECULER_2:
Sitkah 38:76f886a1c8e6 981 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 982 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 983 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 984 GoStraight(-100, 0, 0, 0);
Artiom 44:badcbe8766e9 985 #else
Villanut 72:5b1b3e151e59 986 GoStraight(250, 0, 0, 0);
Artiom 44:badcbe8766e9 987 #endif
Sitkah 38:76f886a1c8e6 988 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 989 canProcessRx();
Sitkah 38:76f886a1c8e6 990 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 991 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 992 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 993 canProcessRx();
Sitkah 38:76f886a1c8e6 994 etat_pos=GOTOPOS;
Sitkah 38:76f886a1c8e6 995 break;
Artiom 44:badcbe8766e9 996
Sitkah 38:76f886a1c8e6 997 case GOTOPOS:
Sitkah 38:76f886a1c8e6 998 localData1 = -1;
Artiom 44:badcbe8766e9 999
Sitkah 38:76f886a1c8e6 1000 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Sitkah 38:76f886a1c8e6 1001 localData2 = -instruction.arg3;
Sitkah 38:76f886a1c8e6 1002 localData3 = 3000 - instruction.arg2;//Inversion du Y
Sitkah 38:76f886a1c8e6 1003 } else {
Sitkah 38:76f886a1c8e6 1004 localData3 = instruction.arg2;
Sitkah 38:76f886a1c8e6 1005 localData2 = instruction.arg3;
Sitkah 38:76f886a1c8e6 1006 }
Artiom 44:badcbe8766e9 1007
Sitkah 38:76f886a1c8e6 1008 GoToPosition(instruction.arg1,localData3,localData2,localData1);
Sitkah 38:76f886a1c8e6 1009 waitingAckID = ASSERVISSEMENT_XYT;
Sitkah 38:76f886a1c8e6 1010 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1011
Sitkah 38:76f886a1c8e6 1012 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1013 canProcessRx();
Sitkah 38:76f886a1c8e6 1014 waitingAckID_FIN=ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1015 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1016 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1017 canProcessRx();
Sitkah 38:76f886a1c8e6 1018 etat_pos=FIN_POS;
Artiom 44:badcbe8766e9 1019 break;
Sitkah 38:76f886a1c8e6 1020 case FIN_POS:
Sitkah 38:76f886a1c8e6 1021 actual_instruction = instruction.nextLineOK;
Artiom 44:badcbe8766e9 1022 break;
Sitkah 38:76f886a1c8e6 1023 }
Sitkah 38:76f886a1c8e6 1024 }
Artiom 44:badcbe8766e9 1025
Artiom 44:badcbe8766e9 1026
Artiom 44:badcbe8766e9 1027 break;
antbig 1:116040d14164 1028 case ETAT_GAME_START:
Artiom 62:c4863b4b2543 1029
antbig 1:116040d14164 1030 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1031
Artiom 44:badcbe8766e9 1032 if (ModeDemo == 0) {
Sitkah 34:6aa4b46b102e 1033 chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
ClementBreteau 14:c8fc06c4887f 1034 gameTimer.start();
Artiom 44:badcbe8766e9 1035 }
antbig 1:116040d14164 1036 gameTimer.reset();
antbig 12:14729d584500 1037 jack.fall(NULL);//On désactive l'interruption du jack
Artiom 44:badcbe8766e9 1038 //SendRawId(GLOBAL_START);
Sitkah 29:41e02746041d 1039 Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
Sitkah 30:a1e37af4bbde 1040 //tactile_printf("Start");//Pas vraiment utile mais bon
Artiom 44:badcbe8766e9 1041 break;
antbig 0:ad97421fb1fb 1042 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
kyxstark 87:349194e73b77 1043 flagNonRepriseErrorMot = 0;
antbig 0:ad97421fb1fb 1044 /*
antbig 0:ad97421fb1fb 1045 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 1046 */
antbig 0:ad97421fb1fb 1047 //printf("load next instruction\n");
Artiom 44:badcbe8766e9 1048
antbig 0:ad97421fb1fb 1049 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 1050 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 1051 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 1052 } else {
antbig 0:ad97421fb1fb 1053 instruction = strat_instructions[actual_instruction];
Artiom 44:badcbe8766e9 1054 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 1055 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 1056 }
antbig 0:ad97421fb1fb 1057 screenChecktry = 0;
antbig 28:acd18776ed2d 1058 ingnorInversionOnce = 0;
Artiom 44:badcbe8766e9 1059 break;
antbig 0:ad97421fb1fb 1060 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 1061 /*
antbig 0:ad97421fb1fb 1062 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 1063 */
antbig 0:ad97421fb1fb 1064 //debug_Instruction(instruction);
Sitkah 38:76f886a1c8e6 1065 //affichage_debug(gameEtat);
Artiom 62:c4863b4b2543 1066 rn42_Tx.printf("A");//lance l'electron
ClementBreteau 25:f140c93a8666 1067 actionPrecedente = instruction.order;
Artiom 44:badcbe8766e9 1068 switch(instruction.order) {
Villanut 75:1db1b929f13d 1069 case MV_BEZIER: {
Artiom 67:96f914f92d2d 1070 static vector< vector<short> >P1;
Artiom 67:96f914f92d2d 1071 static vector< vector<short> >C1;
Artiom 67:96f914f92d2d 1072 static vector< vector<short> >C2;
Artiom 67:96f914f92d2d 1073 static int i = 0;
Villanut 75:1db1b929f13d 1074
Artiom 67:96f914f92d2d 1075 //Ajoute une ligne aux tableaux pour chaques courbes de la trajectoire
Artiom 67:96f914f92d2d 1076 P1.push_back(vector<short>()); //Nouvelle ligne
Artiom 67:96f914f92d2d 1077 C1.push_back(vector<short>()); //Nouvelle ligne
Artiom 67:96f914f92d2d 1078 C2.push_back(vector<short>()); //Nouvelle ligne
Villanut 75:1db1b929f13d 1079
Artiom 67:96f914f92d2d 1080 P1[i].push_back(instruction.arg1); //Nouvelle colonne X
Artiom 67:96f914f92d2d 1081 C1[i].push_back(instruction.arg3); //Nouvelle colonne X
Artiom 67:96f914f92d2d 1082 C2[i].push_back(instruction.arg5); //Nouvelle colonne X
Villanut 75:1db1b929f13d 1083
Villanut 75:1db1b929f13d 1084 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Artiom 67:96f914f92d2d 1085 P1[i].push_back(3000-instruction.arg2); //Nouvelle colonne Y
Artiom 67:96f914f92d2d 1086 C1[i].push_back(3000-instruction.arg4); //Nouvelle colonne Y
Artiom 67:96f914f92d2d 1087 C2[i].push_back(3000-instruction.arg6); //Nouvelle colonne Y
Villanut 75:1db1b929f13d 1088 } else {
Artiom 67:96f914f92d2d 1089 P1[i].push_back(instruction.arg2); //Nouvelle colonne Y
Artiom 67:96f914f92d2d 1090 C1[i].push_back(instruction.arg4); //Nouvelle colonne Y
Artiom 67:96f914f92d2d 1091 C2[i].push_back(instruction.arg6); //Nouvelle colonne Y
Artiom 67:96f914f92d2d 1092 }
Villanut 75:1db1b929f13d 1093
Artiom 67:96f914f92d2d 1094 i++;
Villanut 75:1db1b929f13d 1095
Villanut 75:1db1b929f13d 1096 if(instruction.nextActionType == WAIT) { //Si il n'y a qu'une seule courbe ou que c'est la dernière courbe de la trajectoire
Artiom 67:96f914f92d2d 1097 //Passage des points dans des variables temporaires pour pouvoir clear les vector avant d'être bloqué dans l'attente de l'ack
Artiom 67:96f914f92d2d 1098 //Empeche les vector de ne pas être reset si l'ack n'est pas reçu avant la fin du match
Artiom 67:96f914f92d2d 1099 int nbCourbes = P1.size();
Artiom 67:96f914f92d2d 1100 short P1_temp[nbCourbes][2];
Artiom 67:96f914f92d2d 1101 short C1_temp[nbCourbes][2];
Artiom 67:96f914f92d2d 1102 short C2_temp[nbCourbes][2];
Villanut 75:1db1b929f13d 1103
Villanut 75:1db1b929f13d 1104 for(int j=0; j<nbCourbes; j++) {
Villanut 75:1db1b929f13d 1105 for(int i=0; i<2; i++) {
Artiom 67:96f914f92d2d 1106 P1_temp[j][i] = P1[j][i];
Artiom 67:96f914f92d2d 1107 C1_temp[j][i] = C1[j][i];
Artiom 67:96f914f92d2d 1108 C2_temp[j][i] = C2[j][i];
Artiom 67:96f914f92d2d 1109 }
Artiom 67:96f914f92d2d 1110 }
Villanut 75:1db1b929f13d 1111
Artiom 67:96f914f92d2d 1112 //Clear des tableaux de points pour la prochaine trajectoire
Artiom 67:96f914f92d2d 1113 P1.clear();
Artiom 67:96f914f92d2d 1114 C1.clear();
Artiom 67:96f914f92d2d 1115 C2.clear();
Artiom 67:96f914f92d2d 1116 i = 0;
Villanut 75:1db1b929f13d 1117
Artiom 67:96f914f92d2d 1118 //Calcul de la courbe et envoi des valeurs
Villanut 75:1db1b929f13d 1119 if(instruction.direction == FORWARD) {
Artiom 67:96f914f92d2d 1120 courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 0);
Villanut 75:1db1b929f13d 1121 } else if(instruction.direction == BACKWARD) {
Artiom 67:96f914f92d2d 1122 courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 1);
Artiom 67:96f914f92d2d 1123 }
Villanut 75:1db1b929f13d 1124
Artiom 67:96f914f92d2d 1125 waitingAckID = ASSERVISSEMENT_BEZIER;
Artiom 67:96f914f92d2d 1126 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 67:96f914f92d2d 1127 }
Artiom 67:96f914f92d2d 1128 break;
Artiom 67:96f914f92d2d 1129 }
Artiom 67:96f914f92d2d 1130
Villanut 75:1db1b929f13d 1131
Villanut 75:1db1b929f13d 1132
antbig 12:14729d584500 1133 case MV_COURBURE://C'est un rayon de courbure
ClementBreteau 26:2f4fcc2354f3 1134 actionPrecedente = MV_COURBURE;
antbig 6:eddfa414fd11 1135 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 6:eddfa414fd11 1136 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1137 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 6:eddfa414fd11 1138 MV_enchainement++;
antbig 6:eddfa414fd11 1139 localData5 = 1;
antbig 6:eddfa414fd11 1140 } else {
antbig 6:eddfa414fd11 1141 if(MV_enchainement > 0) {
antbig 6:eddfa414fd11 1142 localData5 = 2;
antbig 6:eddfa414fd11 1143 MV_enchainement = 0;
antbig 6:eddfa414fd11 1144 } else {
antbig 6:eddfa414fd11 1145 localData5 = 0;
antbig 6:eddfa414fd11 1146 }
antbig 0:ad97421fb1fb 1147 }
Artiom 84:44d6cd2cab99 1148 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Artiom 83:23e04b85ae06 1149 if(instruction.direction == LEFT) instruction.direction = RIGHT;
Artiom 83:23e04b85ae06 1150 else instruction.direction = LEFT;
Artiom 84:44d6cd2cab99 1151 }
Artiom 83:23e04b85ae06 1152
antbig 11:ed13a480ddca 1153 localData1 = ((instruction.direction == LEFT)?1:-1);
ClementBreteau 23:ab87d308eaf9 1154 localData2 = instruction.arg3;
Artiom 84:44d6cd2cab99 1155 /*if(InversStrat == 1 && ingnorInversionOnce == 0) {
antbig 12:14729d584500 1156 localData1 = -localData1;//Inversion de la direction
Artiom 84:44d6cd2cab99 1157 }*/
Artiom 84:44d6cd2cab99 1158
ClementBreteau 25:f140c93a8666 1159 BendRadius(instruction.arg1, localData2, localData1, localData5);
Artiom 76:b5eb99354389 1160 #ifdef ROBOT_SMALL
Artiom 76:b5eb99354389 1161 if(localData2>0) {
Artiom 76:b5eb99354389 1162 direction=1;
Artiom 76:b5eb99354389 1163 } else {
Artiom 76:b5eb99354389 1164 direction=-1;
Artiom 76:b5eb99354389 1165 }
Artiom 76:b5eb99354389 1166 #else
Artiom 76:b5eb99354389 1167 if(localData2>0) {
Artiom 76:b5eb99354389 1168 direction=-1;
Artiom 76:b5eb99354389 1169 } else {
Artiom 76:b5eb99354389 1170 direction=1;
Artiom 76:b5eb99354389 1171 }
Artiom 76:b5eb99354389 1172 #endif
Artiom 83:23e04b85ae06 1173
Artiom 83:23e04b85ae06 1174 /*
Artiom 81:ef50ec0ef328 1175 //target_theta_robot = localData2 - theta_robot;
Artiom 81:ef50ec0ef328 1176 target_theta_robot = (localData2 - theta_robot)%3600;
Artiom 81:ef50ec0ef328 1177 if(target_theta_robot > 1800) target_theta_robot = target_theta_robot-3600;
Artiom 81:ef50ec0ef328 1178
Artiom 83:23e04b85ae06 1179 else if(target_theta_robot <-1800) target_theta_robot = target_theta_robot+3600;*/
Artiom 83:23e04b85ae06 1180
Artiom 84:44d6cd2cab99 1181 //if(InversStrat == 0 || ingnorInversionOnce == 1) {
Artiom 83:23e04b85ae06 1182 if(instruction.direction == LEFT)
Artiom 83:23e04b85ae06 1183 {
Artiom 83:23e04b85ae06 1184 target_theta_robot = theta_robot + localData2;
Artiom 83:23e04b85ae06 1185 target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
Artiom 83:23e04b85ae06 1186 target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
Artiom 83:23e04b85ae06 1187 }
Artiom 83:23e04b85ae06 1188 else
Artiom 83:23e04b85ae06 1189 {
Artiom 83:23e04b85ae06 1190 target_theta_robot = theta_robot - localData2;
Artiom 83:23e04b85ae06 1191 target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
Artiom 83:23e04b85ae06 1192 target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
Artiom 83:23e04b85ae06 1193 }
Artiom 84:44d6cd2cab99 1194 /*}
Artiom 83:23e04b85ae06 1195 else
Artiom 83:23e04b85ae06 1196 {
Artiom 83:23e04b85ae06 1197 if(instruction.direction == LEFT)
Artiom 83:23e04b85ae06 1198 {
Artiom 84:44d6cd2cab99 1199 target_theta_robot = theta_robot - localData2;//-theta_robot - localData2;
Artiom 83:23e04b85ae06 1200 target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
Artiom 83:23e04b85ae06 1201 target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
Artiom 83:23e04b85ae06 1202 }
Artiom 83:23e04b85ae06 1203 else
Artiom 83:23e04b85ae06 1204 {
Artiom 84:44d6cd2cab99 1205 target_theta_robot = theta_robot + localData2;//-theta_robot + localData2;
Artiom 83:23e04b85ae06 1206 target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
Artiom 83:23e04b85ae06 1207 target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
Artiom 83:23e04b85ae06 1208 }
Artiom 83:23e04b85ae06 1209 }
Artiom 84:44d6cd2cab99 1210 */
Artiom 83:23e04b85ae06 1211 //target_theta_robot = theta_robot + localData1*localData2;
Artiom 83:23e04b85ae06 1212
Artiom 44:badcbe8766e9 1213
Artiom 44:badcbe8766e9 1214 break;
Artiom 83:23e04b85ae06 1215
Artiom 83:23e04b85ae06 1216
Artiom 83:23e04b85ae06 1217
antbig 12:14729d584500 1218 case MV_LINE://Ligne droite
antbig 0:ad97421fb1fb 1219 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 1220 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 1221 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 1222 MV_enchainement++;
antbig 1:116040d14164 1223 localData5 = 1;
antbig 1:116040d14164 1224 } else {
Artiom 44:badcbe8766e9 1225 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
antbig 1:116040d14164 1226 localData5 = 2;
antbig 1:116040d14164 1227 MV_enchainement = 0;
antbig 1:116040d14164 1228 } else {
antbig 1:116040d14164 1229 localData5 = 0;
antbig 1:116040d14164 1230 }
Artiom 44:badcbe8766e9 1231 }
antbig 1:116040d14164 1232 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 1233 GoStraight(localData2, 0, 0, localData5);
Artiom 44:badcbe8766e9 1234
Artiom 44:badcbe8766e9 1235 target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
ClementBreteau 26:2f4fcc2354f3 1236 target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
ClementBreteau 25:f140c93a8666 1237 target_theta_robot = theta_robot;
Artiom 44:badcbe8766e9 1238
Artiom 44:badcbe8766e9 1239 break;
antbig 12:14729d584500 1240 case MV_TURN: //Rotation sur place
Artiom 78:c0533a36da8f 1241
Artiom 62:c4863b4b2543 1242 localData2 = instruction.arg3;
Artiom 62:c4863b4b2543 1243
Artiom 62:c4863b4b2543 1244 if(InversStrat == 1 && ingnorInversionOnce == 0) {
kyxstark 58:faef8d1b8ed8 1245 localData2 = -localData2;
Artiom 44:badcbe8766e9 1246 }
Artiom 82:759ceb2a8a70 1247
Artiom 62:c4863b4b2543 1248
Artiom 62:c4863b4b2543 1249 if(instruction.direction == ABSOLUTE) {
Artiom 62:c4863b4b2543 1250 //C'est un rotation absolu, il faut la convertir en relative
Artiom 62:c4863b4b2543 1251
kyxstark 58:faef8d1b8ed8 1252 localData2 = (localData2 - theta_robot)%3600;
kyxstark 58:faef8d1b8ed8 1253 if(localData2 > 1800) localData2 = localData2-3600;
Artiom 62:c4863b4b2543 1254
kyxstark 58:faef8d1b8ed8 1255 else if(localData2 <-1800) localData2 = localData2+3600;
kyxstark 58:faef8d1b8ed8 1256 }
Artiom 62:c4863b4b2543 1257
Artiom 62:c4863b4b2543 1258
antbig 0:ad97421fb1fb 1259 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 1260 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Sitkah 34:6aa4b46b102e 1261 Rotate(localData2);
Artiom 44:badcbe8766e9 1262
Artiom 44:badcbe8766e9 1263 break;
antbig 0:ad97421fb1fb 1264 case MV_XYT:
antbig 0:ad97421fb1fb 1265 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 1266 localData1 = -1;
antbig 0:ad97421fb1fb 1267 } else {
antbig 0:ad97421fb1fb 1268 localData1 = 1;
antbig 0:ad97421fb1fb 1269 }
Artiom 44:badcbe8766e9 1270
antbig 28:acd18776ed2d 1271 if(InversStrat == 1 && ingnorInversionOnce == 0) {
antbig 12:14729d584500 1272 localData2 = -instruction.arg3;
antbig 2:8d8e2cf798a3 1273 localData3 = 3000 - instruction.arg2;//Inversion du Y
antbig 2:8d8e2cf798a3 1274 } else {
antbig 2:8d8e2cf798a3 1275 localData3 = instruction.arg2;
antbig 12:14729d584500 1276 localData2 = instruction.arg3;
antbig 2:8d8e2cf798a3 1277 }
Artiom 44:badcbe8766e9 1278
antbig 12:14729d584500 1279 GoToPosition(instruction.arg1,localData3,localData2,localData1);
antbig 0:ad97421fb1fb 1280 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 1281 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1282
Artiom 44:badcbe8766e9 1283 target_x_robot = instruction.arg1;
ClementBreteau 25:f140c93a8666 1284 target_y_robot = localData3;
ClementBreteau 25:f140c93a8666 1285 target_theta_robot = localData2;
Artiom 44:badcbe8766e9 1286
Artiom 44:badcbe8766e9 1287 break;
antbig 0:ad97421fb1fb 1288 case MV_RECALAGE:
Artiom 44:badcbe8766e9 1289 if(instruction.nextActionType == MECANIQUE) {
Sitkah 34:6aa4b46b102e 1290 instruction.nextActionType = WAIT;
Artiom 44:badcbe8766e9 1291
Sitkah 34:6aa4b46b102e 1292 waitingAckID = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1293 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1294
Sitkah 34:6aa4b46b102e 1295 localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
Artiom 44:badcbe8766e9 1296
Sitkah 34:6aa4b46b102e 1297 if(instruction.precision == RECALAGE_Y) {
Sitkah 34:6aa4b46b102e 1298 localData5 = 2;
Sitkah 34:6aa4b46b102e 1299 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Sitkah 34:6aa4b46b102e 1300 localData3 = 3000 - instruction.arg1;//Inversion du Y
Sitkah 34:6aa4b46b102e 1301 } else {
Sitkah 34:6aa4b46b102e 1302 localData3 = instruction.arg1;
Sitkah 34:6aa4b46b102e 1303 }
antbig 11:ed13a480ddca 1304 } else {
Sitkah 34:6aa4b46b102e 1305 localData5 = 1;
antbig 11:ed13a480ddca 1306 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 1307 }
Sitkah 34:6aa4b46b102e 1308 GoStraight(localData2, localData5, localData3, 0);
Artiom 44:badcbe8766e9 1309 } else { //CAPTEUR
Sitkah 34:6aa4b46b102e 1310 SendRawId(DATA_RECALAGE);
Sitkah 34:6aa4b46b102e 1311 waitingAckID = RECEPTION_RECALAGE;
Sitkah 34:6aa4b46b102e 1312 waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
Artiom 44:badcbe8766e9 1313
Sitkah 34:6aa4b46b102e 1314 // On attend que les variables soient actualisé
Sitkah 34:6aa4b46b102e 1315 while(!(waitingAckID == 0 && waitingAckFrom == 0))
Sitkah 34:6aa4b46b102e 1316 canProcessRx();
Sitkah 35:742dc6b200b0 1317 while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
Sitkah 35:742dc6b200b0 1318 canProcessRx();
Artiom 44:badcbe8766e9 1319
Artiom 44:badcbe8766e9 1320 if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
Sitkah 36:6dd30780bd8e 1321 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
Artiom 44:badcbe8766e9 1322 } else if(instruction.precision == RECALAGE_X) {
Artiom 44:badcbe8766e9 1323 SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
Artiom 44:badcbe8766e9 1324 } else if(instruction.precision == RECALAGE_T) {
Artiom 44:badcbe8766e9 1325 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
Sitkah 34:6aa4b46b102e 1326 }
Sitkah 34:6aa4b46b102e 1327 }
Artiom 44:badcbe8766e9 1328 break;
Artiom 44:badcbe8766e9 1329
antbig 0:ad97421fb1fb 1330 case ACTION:
ClementBreteau 18:cc5fec34ed9c 1331 int tempo = 0;
Sitkah 30:a1e37af4bbde 1332 waitingAckID= ACK_ACTION; //On veut un ack de type action
Sitkah 30:a1e37af4bbde 1333 waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
ClementBreteau 18:cc5fec34ed9c 1334 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
Artiom 80:545a9bf561e1 1335 // unsigned char test=(unsigned char) tempo;
Artiom 80:545a9bf561e1 1336 // SendMsgCan(0x5BD, &test,1);
Artiom 44:badcbe8766e9 1337 if(tempo == 1) {
antbig 0:ad97421fb1fb 1338 //L'action est spécifique
Sitkah 34:6aa4b46b102e 1339 if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
antbig 11:ed13a480ddca 1340 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 11:ed13a480ddca 1341 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1342 } else {
ClementBreteau 15:c2fc239e85df 1343 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 11:ed13a480ddca 1344 }
Artiom 44:badcbe8766e9 1345 #ifdef ROBOT_SMALL
Artiom 44:badcbe8766e9 1346 /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
Artiom 44:badcbe8766e9 1347 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
Artiom 44:badcbe8766e9 1348 #endif
antbig 11:ed13a480ddca 1349 return;
Artiom 44:badcbe8766e9 1350 #ifdef ROBOT_SMALL
ClementBreteau 18:cc5fec34ed9c 1351 } else if (tempo == 2) {
ClementBreteau 18:cc5fec34ed9c 1352 // on est dans le cas de l'avance selon le telemetre
ClementBreteau 18:cc5fec34ed9c 1353 waitingAckID = ASSERVISSEMENT_RECALAGE;
ClementBreteau 18:cc5fec34ed9c 1354 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1355
ClementBreteau 18:cc5fec34ed9c 1356 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
ClementBreteau 18:cc5fec34ed9c 1357 GoStraight(telemetreDistance, 0, 0, 0);
ClementBreteau 18:cc5fec34ed9c 1358 // on reset la distance du telemetre à 0
ClementBreteau 18:cc5fec34ed9c 1359 telemetreDistance = 5000;
Artiom 44:badcbe8766e9 1360 #endif
Artiom 44:badcbe8766e9 1361 } else {
antbig 0:ad97421fb1fb 1362 //C'est un AX12 qu'il faut bouger
ClementBreteau 15:c2fc239e85df 1363 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
ClementBreteau 15:c2fc239e85df 1364 //AX12_enchainement++;
Artiom 44:badcbe8766e9 1365
antbig 0:ad97421fb1fb 1366 }
Artiom 44:badcbe8766e9 1367 break;
antbig 0:ad97421fb1fb 1368 default:
antbig 0:ad97421fb1fb 1369 //Instruction inconnue, on l'ignore
Artiom 44:badcbe8766e9 1370 break;
Artiom 44:badcbe8766e9 1371 }
Artiom 44:badcbe8766e9 1372
antbig 0:ad97421fb1fb 1373 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 1374 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 1375 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 1376 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 1377 cartesCheker.start();
antbig 1:116040d14164 1378 if(AX12_enchainement > 0) {
ClementBreteau 15:c2fc239e85df 1379 //AX12_processChange();//Il faut lancer le déplacement des AX12
ClementBreteau 15:c2fc239e85df 1380 //AX12_enchainement = 0;
antbig 1:116040d14164 1381 }
Artiom 44:badcbe8766e9 1382 } else { //C'est un enchainement
Artiom 44:badcbe8766e9 1383 if(instruction.order == MV_LINE) {
Artiom 44:badcbe8766e9 1384 gameEtat = ETAT_GAME_WAIT_ACK;
Artiom 44:badcbe8766e9 1385 } else {
ClementBreteau 15:c2fc239e85df 1386 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 1387 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
ClementBreteau 15:c2fc239e85df 1388 }
antbig 0:ad97421fb1fb 1389 }
Artiom 44:badcbe8766e9 1390
Artiom 44:badcbe8766e9 1391 break;
antbig 0:ad97421fb1fb 1392 case ETAT_GAME_WAIT_ACK:
Sitkah 36:6dd30780bd8e 1393 canProcessRx();
Artiom 44:badcbe8766e9 1394
Sitkah 30:a1e37af4bbde 1395 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
Artiom 44:badcbe8766e9 1396 //if(true) {
antbig 0:ad97421fb1fb 1397 cartesCheker.stop();
antbig 0:ad97421fb1fb 1398 if(instruction.nextActionType == JUMP) {
antbig 11:ed13a480ddca 1399 if(instruction.jumpAction == JUMP_POSITION) {
antbig 11:ed13a480ddca 1400 gameEtat = ETAT_GAME_JUMP_POSITION;
Artiom 44:badcbe8766e9 1401 } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
antbig 0:ad97421fb1fb 1402 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 1403 cartesCheker.reset();//On reset le timeOut
Artiom 44:badcbe8766e9 1404 cartesCheker.start();
antbig 0:ad97421fb1fb 1405 }
Artiom 44:badcbe8766e9 1406 } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
Sitkah 41:b029ddc4d60e 1407 /*wait_ms(200);
Sitkah 40:21bb685b553b 1408 #ifdef ROBOT_BIG
Sitkah 40:21bb685b553b 1409 SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
Sitkah 40:21bb685b553b 1410 #else
Sitkah 40:21bb685b553b 1411 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
Sitkah 40:21bb685b553b 1412 #endif
Sitkah 41:b029ddc4d60e 1413 wait_ms(200);*/
Artiom 44:badcbe8766e9 1414
Sitkah 40:21bb685b553b 1415 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
Artiom 44:badcbe8766e9 1416 switch(instruction.order) {
Artiom 67:96f914f92d2d 1417 case MV_BEZIER:
Artiom 67:96f914f92d2d 1418 waitingAckID_FIN = ASSERVISSEMENT_BEZIER;
Artiom 67:96f914f92d2d 1419 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 67:96f914f92d2d 1420 break;
antbig 0:ad97421fb1fb 1421 case MV_COURBURE:
Sitkah 34:6aa4b46b102e 1422 waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
Sitkah 34:6aa4b46b102e 1423 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1424 break;
antbig 0:ad97421fb1fb 1425 case MV_LINE:
Sitkah 34:6aa4b46b102e 1426 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1427 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1428 break;
antbig 0:ad97421fb1fb 1429 case MV_TURN:
Sitkah 34:6aa4b46b102e 1430 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Sitkah 34:6aa4b46b102e 1431 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1432 break;
antbig 0:ad97421fb1fb 1433 case MV_XYT:
Sitkah 34:6aa4b46b102e 1434 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Sitkah 34:6aa4b46b102e 1435 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1436 break;
antbig 0:ad97421fb1fb 1437 case MV_RECALAGE:
Sitkah 34:6aa4b46b102e 1438 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1439 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1440 break;
antbig 0:ad97421fb1fb 1441 case ACTION:
Artiom 44:badcbe8766e9 1442
Artiom 44:badcbe8766e9 1443 if (modeTelemetre == 0) {
Artiom 44:badcbe8766e9 1444 if (telemetreDistance == 0) {
Artiom 44:badcbe8766e9 1445 waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
Sitkah 34:6aa4b46b102e 1446 waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
Artiom 44:badcbe8766e9 1447 } else if(telemetreDistance == 5000) {
ClementBreteau 18:cc5fec34ed9c 1448 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
Sitkah 34:6aa4b46b102e 1449 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1450 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
ClementBreteau 18:cc5fec34ed9c 1451 telemetreDistance = 0;
ClementBreteau 18:cc5fec34ed9c 1452 }
Artiom 44:badcbe8766e9 1453 } else { // si on attend la reponse du telemetre
Artiom 44:badcbe8766e9 1454 //modeTelemetre = 1;
Sitkah 34:6aa4b46b102e 1455 waitingAckID_FIN = OBJET_SUR_TABLE;
Artiom 44:badcbe8766e9 1456 waitingAckFrom_FIN = 0;
ClementBreteau 15:c2fc239e85df 1457 }
Artiom 44:badcbe8766e9 1458 break;
antbig 0:ad97421fb1fb 1459 default:
Artiom 44:badcbe8766e9 1460 break;
Artiom 44:badcbe8766e9 1461 }
Artiom 44:badcbe8766e9 1462 } else {
antbig 0:ad97421fb1fb 1463 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 1464 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1465 }
Artiom 44:badcbe8766e9 1466 } else if(cartesCheker.read_ms () > 1000) {
antbig 0:ad97421fb1fb 1467 cartesCheker.stop();
antbig 0:ad97421fb1fb 1468 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 1469 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 1470 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1471 } else {
Sitkah 34:6aa4b46b102e 1472 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 1473 }
antbig 0:ad97421fb1fb 1474 }
Artiom 44:badcbe8766e9 1475 break;
Artiom 44:badcbe8766e9 1476
antbig 0:ad97421fb1fb 1477 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 1478 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 1479 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 1480 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1481 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 1482 }
Artiom 44:badcbe8766e9 1483 break;
Artiom 44:badcbe8766e9 1484
ClementBreteau 15:c2fc239e85df 1485 case ETAT_GAME_JUMP_CONFIG:
ClementBreteau 15:c2fc239e85df 1486 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
Artiom 44:badcbe8766e9 1487 // 1 si l'instruction est plus grande que la position du robot
Artiom 44:badcbe8766e9 1488 // -1 si l'instruction est plus petite que la position du robot
Artiom 44:badcbe8766e9 1489 // 0 si l'instruction et position du robot sont proche de moins de 1cm
Artiom 44:badcbe8766e9 1490 if (abs(x_robot-instruction.JumpTimeOrX)<10) {
Artiom 44:badcbe8766e9 1491 depasX = 0;
Artiom 44:badcbe8766e9 1492 } else if(x_robot > instruction.JumpTimeOrX) {
Artiom 44:badcbe8766e9 1493 depasX = -1;
ClementBreteau 15:c2fc239e85df 1494 }
Artiom 44:badcbe8766e9 1495
Artiom 44:badcbe8766e9 1496 if(abs(y_robot-instruction.JumpY)<10) {
Artiom 44:badcbe8766e9 1497 depasY = 0;
Artiom 44:badcbe8766e9 1498 } else if(y_robot > instruction.JumpY) {
Artiom 44:badcbe8766e9 1499 depasY = -1;
ClementBreteau 15:c2fc239e85df 1500 }
Artiom 44:badcbe8766e9 1501
ClementBreteau 15:c2fc239e85df 1502 gameEtat = ETAT_GAME_JUMP_POSITION;
Artiom 44:badcbe8766e9 1503 break;
antbig 0:ad97421fb1fb 1504 case ETAT_GAME_JUMP_POSITION:
ClementBreteau 15:c2fc239e85df 1505 bool Xok = false, Yok = false;
Artiom 44:badcbe8766e9 1506
Artiom 44:badcbe8766e9 1507 if (depasX == 0) {
Artiom 44:badcbe8766e9 1508 Xok = true;
Artiom 44:badcbe8766e9 1509 } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) {
Artiom 44:badcbe8766e9 1510 Xok = true;
Artiom 44:badcbe8766e9 1511 }
Artiom 44:badcbe8766e9 1512
Artiom 44:badcbe8766e9 1513 if (depasY == 0) {
Artiom 44:badcbe8766e9 1514 Yok = true;
Artiom 44:badcbe8766e9 1515 } else if ((instruction.JumpY - y_robot)*depasY < -5) {
Artiom 44:badcbe8766e9 1516 Yok = true;
Artiom 44:badcbe8766e9 1517 }
Artiom 44:badcbe8766e9 1518
Artiom 44:badcbe8766e9 1519 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
Artiom 44:badcbe8766e9 1520 if (Xok && Yok) {
Artiom 44:badcbe8766e9 1521 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
Artiom 44:badcbe8766e9 1522 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
Artiom 44:badcbe8766e9 1523 }
Artiom 44:badcbe8766e9 1524
Artiom 44:badcbe8766e9 1525 break;
antbig 0:ad97421fb1fb 1526 case ETAT_GAME_WAIT_END_INSTRUCTION:
Artiom 44:badcbe8766e9 1527 canProcessRx();
Sitkah 35:742dc6b200b0 1528 if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 1529 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1530 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 1531 }
Artiom 44:badcbe8766e9 1532
Artiom 44:badcbe8766e9 1533 break;
Artiom 44:badcbe8766e9 1534
Artiom 44:badcbe8766e9 1535
antbig 5:dcd817534b57 1536 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
Artiom 44:badcbe8766e9 1537 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT) { //ça fait plus de 2s, il faut changer de stratégie
Artiom 62:c4863b4b2543 1538 if( needToStop()==2) {
Artiom 62:c4863b4b2543 1539 gameEtat = ETAT_EVITEMENT;
Artiom 62:c4863b4b2543 1540 /* if(Fevitement==1) {
Artiom 62:c4863b4b2543 1541 EvitEtat=0;
Artiom 62:c4863b4b2543 1542 Fevitement=0;
Artiom 62:c4863b4b2543 1543 }*/
Artiom 62:c4863b4b2543 1544 }
Artiom 62:c4863b4b2543 1545 if( needToStop()==1) {
Artiom 62:c4863b4b2543 1546 // code origine
Artiom 62:c4863b4b2543 1547 if(instruction.nextLineOK != instruction.nextLineError) {
Artiom 62:c4863b4b2543 1548 actual_instruction = instruction.nextLineError;
Artiom 62:c4863b4b2543 1549 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 62:c4863b4b2543 1550 }
Artiom 62:c4863b4b2543 1551 //--------------------------------------------
Artiom 62:c4863b4b2543 1552 }
antbig 5:dcd817534b57 1553 }
Artiom 44:badcbe8766e9 1554 break;
Artiom 44:badcbe8766e9 1555
Artiom 44:badcbe8766e9 1556
Artiom 83:23e04b85ae06 1557 case ETAT_TELEMETRE_BALANCE:
Artiom 83:23e04b85ae06 1558 /* SendRawId(ASSERVISSEMENT_STOP);
Artiom 83:23e04b85ae06 1559 waitingAckID_FIN = ASSERVISSEMENT_STOP;
Artiom 83:23e04b85ae06 1560 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 83:23e04b85ae06 1561 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 83:23e04b85ae06 1562 canProcessRx();*/
Artiom 83:23e04b85ae06 1563 ingnorBaliseOnce=1;
Artiom 83:23e04b85ae06 1564 SendRawId(DATA_RECALAGE);
Artiom 83:23e04b85ae06 1565 wait_us(150);
Artiom 83:23e04b85ae06 1566 canProcessRx();
Artiom 83:23e04b85ae06 1567 if( ((instruction.order == MV_LINE) && (instruction.direction == FORWARD)) || ((instruction.order == MV_XYT) && (instruction.direction == FORWARD)) ) {
Artiom 83:23e04b85ae06 1568 if(Cote==1) { //violet
Artiom 83:23e04b85ae06 1569 if((telemetreDistance_avant_droite+100)>(y_robot-1500)) {
Artiom 83:23e04b85ae06 1570 gameEtat=memGameEtat;
Artiom 83:23e04b85ae06 1571 } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
Artiom 83:23e04b85ae06 1572 timeoutWarning.reset();
Artiom 83:23e04b85ae06 1573 timeoutWarning.start();//Reset du timer utiliser par le timeout
Artiom 83:23e04b85ae06 1574 gameEtat = ETAT_WARNING_TIMEOUT;
Artiom 83:23e04b85ae06 1575 }
Artiom 83:23e04b85ae06 1576 } else {
Artiom 83:23e04b85ae06 1577 if((telemetreDistance_avant_gauche+100)>(1500-y_robot)) {
Artiom 83:23e04b85ae06 1578 gameEtat=memGameEtat;
Artiom 83:23e04b85ae06 1579 } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
Artiom 83:23e04b85ae06 1580 timeoutWarning.reset();
Artiom 83:23e04b85ae06 1581 timeoutWarning.start();//Reset du timer utiliser par le timeout
Artiom 83:23e04b85ae06 1582 gameEtat = ETAT_WARNING_TIMEOUT;
Artiom 83:23e04b85ae06 1583 }
Artiom 83:23e04b85ae06 1584 }
Artiom 83:23e04b85ae06 1585 }
Artiom 83:23e04b85ae06 1586 if( ((instruction.order == MV_LINE) && (instruction.direction == BACKWARD)) || ((instruction.order == MV_XYT) && (instruction.direction == BACKWARD)) ) {
Artiom 83:23e04b85ae06 1587 if(Cote==1) {
Artiom 83:23e04b85ae06 1588 if((telemetreDistance_arriere_droite-100)>(1500-y_robot)) {
Artiom 83:23e04b85ae06 1589 gameEtat=memGameEtat;
Artiom 83:23e04b85ae06 1590 } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
Artiom 83:23e04b85ae06 1591 timeoutWarning.reset();
Artiom 83:23e04b85ae06 1592 timeoutWarning.start();//Reset du timer utiliser par le timeout
Artiom 83:23e04b85ae06 1593 gameEtat = ETAT_WARNING_TIMEOUT;
Artiom 83:23e04b85ae06 1594 }
Artiom 83:23e04b85ae06 1595 } else {
Artiom 83:23e04b85ae06 1596 if((telemetreDistance_arriere_gauche-100)>(1500-y_robot)) {
Artiom 83:23e04b85ae06 1597 gameEtat=memGameEtat;
Artiom 83:23e04b85ae06 1598 } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
Artiom 83:23e04b85ae06 1599 timeoutWarning.reset();
Artiom 83:23e04b85ae06 1600 timeoutWarning.start();//Reset du timer utiliser par le timeout
Artiom 83:23e04b85ae06 1601 gameEtat = ETAT_WARNING_TIMEOUT;
Artiom 83:23e04b85ae06 1602 }
Artiom 83:23e04b85ae06 1603 }
Artiom 83:23e04b85ae06 1604 }
Artiom 83:23e04b85ae06 1605 break;
Artiom 44:badcbe8766e9 1606
antbig 5:dcd817534b57 1607 case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
antbig 12:14729d584500 1608 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
antbig 12:14729d584500 1609 //actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 1610 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
antbig 12:14729d584500 1611 }
Artiom 44:badcbe8766e9 1612 break;
antbig 5:dcd817534b57 1613 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
Artiom 44:badcbe8766e9 1614
Artiom 44:badcbe8766e9 1615
Artiom 44:badcbe8766e9 1616 switch(actionPrecedente) {
ClementBreteau 25:f140c93a8666 1617 case MV_LINE:
Artiom 44:badcbe8766e9 1618
ClementBreteau 25:f140c93a8666 1619 if(instruction.direction == BACKWARD) {
ClementBreteau 25:f140c93a8666 1620 localData1 = -1;
ClementBreteau 25:f140c93a8666 1621 } else {
ClementBreteau 25:f140c93a8666 1622 localData1 = 1;
ClementBreteau 25:f140c93a8666 1623 }
Artiom 81:ef50ec0ef328 1624 ingnorBaliseOnce = 0;
Artiom 81:ef50ec0ef328 1625 ingnorBalise = 0;
Artiom 82:759ceb2a8a70 1626
antbig 27:76ead555a63d 1627 debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
antbig 27:76ead555a63d 1628 waitingAckID = ASSERVISSEMENT_XYT;
antbig 27:76ead555a63d 1629 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1630 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 27:76ead555a63d 1631 instruction.order = MV_XYT;
antbig 28:acd18776ed2d 1632 instruction.arg1 = target_x_robot;
antbig 28:acd18776ed2d 1633 instruction.arg2 = target_y_robot;
antbig 28:acd18776ed2d 1634 instruction.arg3 = target_theta_robot;
antbig 28:acd18776ed2d 1635 instruction.direction = (localData1)?FORWARD:BACKWARD;
antbig 28:acd18776ed2d 1636 ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
Artiom 81:ef50ec0ef328 1637 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
Artiom 81:ef50ec0ef328 1638 //gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
Artiom 81:ef50ec0ef328 1639 break;
Artiom 82:759ceb2a8a70 1640
Artiom 82:759ceb2a8a70 1641
Artiom 82:759ceb2a8a70 1642 //
Artiom 82:759ceb2a8a70 1643 //return;
Artiom 44:badcbe8766e9 1644
ClementBreteau 25:f140c93a8666 1645 case MV_XYT:
Artiom 81:ef50ec0ef328 1646 ingnorBaliseOnce = 0;
Artiom 81:ef50ec0ef328 1647 ingnorBalise = 0;
Artiom 82:759ceb2a8a70 1648
Artiom 44:badcbe8766e9 1649 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
Sitkah 33:388aa0bf6af4 1650 ///////cv
ClementBreteau 25:f140c93a8666 1651 break;
Artiom 44:badcbe8766e9 1652
ClementBreteau 26:2f4fcc2354f3 1653 case MV_COURBURE:
antbig 28:acd18776ed2d 1654 //target_theta_robot = theta_robot - target_theta_robot;
antbig 28:acd18776ed2d 1655 //instruction.arg3 = instruction.arg3 - target_theta_robot;
Artiom 81:ef50ec0ef328 1656 ingnorBaliseOnce = 0;
Artiom 81:ef50ec0ef328 1657 ingnorBalise = 0;
Artiom 84:44d6cd2cab99 1658 short new_theta_courbe;
Artiom 84:44d6cd2cab99 1659
Artiom 44:badcbe8766e9 1660 if(instruction.direction == LEFT) {
Artiom 84:44d6cd2cab99 1661 new_theta_courbe = target_theta_robot - theta_robot;
Artiom 44:badcbe8766e9 1662 } else {
Artiom 84:44d6cd2cab99 1663 new_theta_courbe = theta_robot - target_theta_robot;
antbig 28:acd18776ed2d 1664 }
Artiom 83:23e04b85ae06 1665
Artiom 84:44d6cd2cab99 1666
Artiom 84:44d6cd2cab99 1667 if(instruction.arg3 >= 0)
Artiom 84:44d6cd2cab99 1668 {
Artiom 84:44d6cd2cab99 1669 if(new_theta_courbe<0)new_theta_courbe+=3600;
Artiom 84:44d6cd2cab99 1670 else if(new_theta_courbe>instruction.arg3)new_theta_courbe-=3600;
Artiom 84:44d6cd2cab99 1671 }
Artiom 84:44d6cd2cab99 1672 else if(instruction.arg3 < 0)
Artiom 84:44d6cd2cab99 1673 {
Artiom 84:44d6cd2cab99 1674 if(new_theta_courbe>0)new_theta_courbe-=3600;
Artiom 84:44d6cd2cab99 1675 else if(new_theta_courbe<instruction.arg3)new_theta_courbe+=3600;
Artiom 84:44d6cd2cab99 1676 }
Artiom 84:44d6cd2cab99 1677 /*
Artiom 83:23e04b85ae06 1678 if(InversStrat == 1) {
Artiom 83:23e04b85ae06 1679 target_theta_robot = - target_theta_robot;
Artiom 84:44d6cd2cab99 1680 }*/
Artiom 83:23e04b85ae06 1681 /* = (target_theta_robot)%3600;
antbig 28:acd18776ed2d 1682 if(target_theta_robot > 1800) {
antbig 28:acd18776ed2d 1683 target_theta_robot = target_theta_robot-3600;
antbig 28:acd18776ed2d 1684 }
Artiom 44:badcbe8766e9 1685 if(InversStrat == 1) {
Artiom 81:ef50ec0ef328 1686 target_theta_robot = - target_theta_robot;
Artiom 83:23e04b85ae06 1687 }*/
Artiom 84:44d6cd2cab99 1688 instruction.arg3 = new_theta_courbe;
Artiom 44:badcbe8766e9 1689
Artiom 44:badcbe8766e9 1690 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
Artiom 44:badcbe8766e9 1691
kyxstark 86:05a0c066bdbe 1692 if(InversStrat == 1 && ingnorInversionOnce == 0) {
kyxstark 86:05a0c066bdbe 1693 if(instruction.direction == LEFT) instruction.direction = RIGHT;
kyxstark 86:05a0c066bdbe 1694 else instruction.direction = LEFT;
kyxstark 86:05a0c066bdbe 1695 }
Artiom 83:23e04b85ae06 1696 /*
Artiom 83:23e04b85ae06 1697
Artiom 83:23e04b85ae06 1698 debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
Artiom 83:23e04b85ae06 1699 waitingAckID = ASSERVISSEMENT_XYT;
Artiom 83:23e04b85ae06 1700 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 83:23e04b85ae06 1701 gameEtat = ETAT_GAME_WAIT_ACK;
Artiom 83:23e04b85ae06 1702 instruction.order = MV_XYT;
Artiom 83:23e04b85ae06 1703 instruction.arg1 = target_x_robot;
Artiom 83:23e04b85ae06 1704 instruction.arg2 = target_y_robot;
Artiom 83:23e04b85ae06 1705 instruction.arg3 = target_theta_robot;
Artiom 83:23e04b85ae06 1706 instruction.direction = (localData1)?FORWARD:BACKWARD;
Artiom 83:23e04b85ae06 1707 ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
Artiom 83:23e04b85ae06 1708 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
Artiom 83:23e04b85ae06 1709 */
Artiom 83:23e04b85ae06 1710
ClementBreteau 25:f140c93a8666 1711 break;
Artiom 82:759ceb2a8a70 1712
antbig 27:76ead555a63d 1713 default:
antbig 27:76ead555a63d 1714 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 1715 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1716 break;
ClementBreteau 25:f140c93a8666 1717 }
Artiom 44:badcbe8766e9 1718
antbig 27:76ead555a63d 1719 //actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 1720 //gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1721 break;
antbig 5:dcd817534b57 1722 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
antbig 5:dcd817534b57 1723 actual_instruction = instruction.nextLineError;
antbig 5:dcd817534b57 1724 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 1725 ingnorBaliseOnce = 1;
Artiom 44:badcbe8766e9 1726 break;
Artiom 44:badcbe8766e9 1727
Sitkah 37:fca332b64b42 1728 case ETAT_EVITEMENT :
Artiom 62:c4863b4b2543 1729 /*
Artiom 62:c4863b4b2543 1730
Artiom 62:c4863b4b2543 1731 90°
Artiom 62:c4863b4b2543 1732 |
Artiom 62:c4863b4b2543 1733 |Violet
Artiom 62:c4863b4b2543 1734 |
Artiom 62:c4863b4b2543 1735 |
Artiom 62:c4863b4b2543 1736 |
Artiom 62:c4863b4b2543 1737 |
Artiom 62:c4863b4b2543 1738 |Jaune
Artiom 62:c4863b4b2543 1739 |
Artiom 62:c4863b4b2543 1740 |________________ 0° */
Artiom 62:c4863b4b2543 1741
Artiom 50:a5361ffeefc8 1742 char message[10]="toto";
Artiom 50:a5361ffeefc8 1743 char message1[10]="toto";
Artiom 50:a5361ffeefc8 1744 char message2[10]="toto";
Artiom 50:a5361ffeefc8 1745 char message3[10]="toto";
Artiom 53:e96acb11a51f 1746 /*
Artiom 44:badcbe8766e9 1747 static short x_terrain=3000;
Artiom 44:badcbe8766e9 1748 static short y_terrain=1500;
Artiom 53:e96acb11a51f 1749 */
Artiom 62:c4863b4b2543 1750
Artiom 53:e96acb11a51f 1751 static short y_terrain=3000;
Artiom 53:e96acb11a51f 1752 static short x_terrain=1500;
Artiom 44:badcbe8766e9 1753
Artiom 44:badcbe8766e9 1754 static float x_cote_droit[3]= {0};
Artiom 44:badcbe8766e9 1755 static float y_cote_droit[3]= {0};
Artiom 50:a5361ffeefc8 1756
Artiom 44:badcbe8766e9 1757 static float x_cote_gauche[3]= {0};
Artiom 44:badcbe8766e9 1758 static float y_cote_gauche[3]= {0};
Artiom 44:badcbe8766e9 1759 static short cote=0;
Artiom 44:badcbe8766e9 1760 //--------------------------
Artiom 50:a5361ffeefc8 1761 static float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
Artiom 53:e96acb11a51f 1762 int proxy=400;//distance entre point de controle et obstacle/adversaire
Artiom 51:aa6e09f2cfec 1763 int proximity=300;//distance entre l'objectif et obstacle/adversaire
Artiom 53:e96acb11a51f 1764 short taille_petit=150;// distance proxymité max mur
Artiom 44:badcbe8766e9 1765 //---------------------------*
Artiom 44:badcbe8766e9 1766 static unsigned short distance=50000;//valeur impossible
Artiom 44:badcbe8766e9 1767 static unsigned short distance_prev=50000;
Artiom 50:a5361ffeefc8 1768 static signed short theta_adversaire;
Artiom 62:c4863b4b2543 1769
Artiom 44:badcbe8766e9 1770 switch(EvitEtat) {
Artiom 44:badcbe8766e9 1771 case 0:
Artiom 62:c4863b4b2543 1772
Artiom 62:c4863b4b2543 1773
Artiom 50:a5361ffeefc8 1774 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 1775 lcd.DisplayStringAt(0, LINE(2),(unsigned char *)"EVITEMENT ",LEFT_MODE);
Artiom 50:a5361ffeefc8 1776
Artiom 44:badcbe8766e9 1777 ingnorBalise=1;
Artiom 50:a5361ffeefc8 1778 Rotate(450); //on tourne a gauche pour scanner
Artiom 44:badcbe8766e9 1779 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1780 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1781 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1782 canProcessRx();
Artiom 44:badcbe8766e9 1783
Artiom 44:badcbe8766e9 1784 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1785 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 50:a5361ffeefc8 1786 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1787 canProcessRx();
Artiom 50:a5361ffeefc8 1788
kyxstark 73:bf4d6d9db13b 1789 SendSpeed(50);
kyxstark 73:bf4d6d9db13b 1790 SendAccel(1500,1500);
Artiom 50:a5361ffeefc8 1791 waitingAckID = ASSERVISSEMENT_CONFIG;
Artiom 44:badcbe8766e9 1792 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1793 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1794 canProcessRx();
Artiom 50:a5361ffeefc8 1795
Artiom 50:a5361ffeefc8 1796 Rotate(-900);//on tourne a droite pour scanner
Artiom 44:badcbe8766e9 1797 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1798 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 50:a5361ffeefc8 1799 wait_us(150);
Artiom 44:badcbe8766e9 1800 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1801 canProcessRx();
Artiom 44:badcbe8766e9 1802
Artiom 44:badcbe8766e9 1803 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1804 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 50:a5361ffeefc8 1805 waitingId = RECEPTION_RECALAGE;
Artiom 50:a5361ffeefc8 1806 SendRawId(DATA_RECALAGE);
Artiom 51:aa6e09f2cfec 1807 wait_us(150);
Artiom 44:badcbe8766e9 1808 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
Artiom 44:badcbe8766e9 1809 canProcessRx();
Artiom 51:aa6e09f2cfec 1810 wait_ms(10);
Artiom 50:a5361ffeefc8 1811 if(waitingId == 0) {
Artiom 50:a5361ffeefc8 1812 distance=telemetreDistance_avant_droite; //on sauvegarde notre distance au robot
Artiom 50:a5361ffeefc8 1813 if(distance<=distance_prev) {
Artiom 50:a5361ffeefc8 1814 distance_prev=distance;
Artiom 51:aa6e09f2cfec 1815 dist_robot_adversaire=distance+100;
Artiom 50:a5361ffeefc8 1816 theta_adversaire=theta_robot;
Artiom 50:a5361ffeefc8 1817 }
Artiom 50:a5361ffeefc8 1818 waitingId = RECEPTION_RECALAGE;
Artiom 50:a5361ffeefc8 1819 SendRawId(DATA_RECALAGE);
Artiom 50:a5361ffeefc8 1820 wait_us(150);
Artiom 44:badcbe8766e9 1821 }
Artiom 44:badcbe8766e9 1822 }
Artiom 51:aa6e09f2cfec 1823
kyxstark 73:bf4d6d9db13b 1824 SendSpeed(300);//vitesse inintiale SendSpeed(600,5000,5000)
kyxstark 73:bf4d6d9db13b 1825 SendAccel(5000,5000);
Artiom 51:aa6e09f2cfec 1826 waitingAckID = ASSERVISSEMENT_CONFIG;
Artiom 51:aa6e09f2cfec 1827 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 51:aa6e09f2cfec 1828 wait_us(150);
Artiom 51:aa6e09f2cfec 1829 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 51:aa6e09f2cfec 1830 canProcessRx();
Artiom 62:c4863b4b2543 1831
Artiom 62:c4863b4b2543 1832 /* Rotate(theta_adversaire); //on tourne a gauche pour scanner
Artiom 62:c4863b4b2543 1833 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 62:c4863b4b2543 1834 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 62:c4863b4b2543 1835 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 62:c4863b4b2543 1836 canProcessRx();
Artiom 62:c4863b4b2543 1837
Artiom 62:c4863b4b2543 1838 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 62:c4863b4b2543 1839 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 62:c4863b4b2543 1840 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 62:c4863b4b2543 1841 canProcessRx();*/
Artiom 51:aa6e09f2cfec 1842
Artiom 51:aa6e09f2cfec 1843 EvitEtat=1;
Artiom 50:a5361ffeefc8 1844
Artiom 50:a5361ffeefc8 1845 break;
Artiom 50:a5361ffeefc8 1846
Artiom 50:a5361ffeefc8 1847 case 1:
Artiom 44:badcbe8766e9 1848 short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600;
Artiom 44:badcbe8766e9 1849 // On passe le résultat entre -1800 et 1800
Artiom 51:aa6e09f2cfec 1850 if (ang_target > 1800) ang_target = (ang_target - 3600);
Artiom 44:badcbe8766e9 1851
Artiom 44:badcbe8766e9 1852 // float dist_target = (short)sqrt((target_x_robot - x_robot)*(target_x_robot - x_robot)+(target_y_robot - y_robot)*(target_y_robot - y_robot));
Artiom 62:c4863b4b2543 1853 /*
Artiom 62:c4863b4b2543 1854 float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
Artiom 62:c4863b4b2543 1855 float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
Artiom 62:c4863b4b2543 1856
Artiom 62:c4863b4b2543 1857 x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 62:c4863b4b2543 1858 y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 62:c4863b4b2543 1859 x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 62:c4863b4b2543 1860 y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 62:c4863b4b2543 1861
Artiom 62:c4863b4b2543 1862 x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 62:c4863b4b2543 1863 y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 62:c4863b4b2543 1864 x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 62:c4863b4b2543 1865 y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 62:c4863b4b2543 1866
Artiom 62:c4863b4b2543 1867 x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 62:c4863b4b2543 1868 y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 62:c4863b4b2543 1869 x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 62:c4863b4b2543 1870 y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 62:c4863b4b2543 1871 */
Artiom 44:badcbe8766e9 1872 float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
Artiom 44:badcbe8766e9 1873 float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
Artiom 44:badcbe8766e9 1874
Artiom 54:8996a5b18d9b 1875 x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300.0)*M_PI/1800);
Artiom 53:e96acb11a51f 1876 y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 53:e96acb11a51f 1877 x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 53:e96acb11a51f 1878 y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 53:e96acb11a51f 1879
Artiom 53:e96acb11a51f 1880 x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 53:e96acb11a51f 1881 y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 53:e96acb11a51f 1882 x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 53:e96acb11a51f 1883 y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 53:e96acb11a51f 1884
Artiom 53:e96acb11a51f 1885 x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 53:e96acb11a51f 1886 y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 53:e96acb11a51f 1887 x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 53:e96acb11a51f 1888 y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 44:badcbe8766e9 1889
Artiom 44:badcbe8766e9 1890 SendRawId(0x0D0);//calcul
Artiom 44:badcbe8766e9 1891
Artiom 44:badcbe8766e9 1892 //for(int i=0; i<3; i++) printf("point:%d | gauche(%.1f , %.1f) | droite(%.1f , %.1f)\n\r",i,x_cote_gauche[i],y_cote_gauche[i],x_cote_droit[i],y_cote_droit[i]) ;
Artiom 44:badcbe8766e9 1893 //printf("------------\n\r");
Artiom 44:badcbe8766e9 1894 //-------------------------process------------------------------------------------
Artiom 44:badcbe8766e9 1895
Artiom 44:badcbe8766e9 1896 bool cote_droit=false, cote_gauche=false;
Artiom 44:badcbe8766e9 1897
Artiom 44:badcbe8766e9 1898 for (int i=0; i<3; i++) {
Artiom 44:badcbe8766e9 1899 if (x_cote_droit[i]>taille_petit && x_cote_droit[i]<x_terrain-taille_petit && y_cote_droit[i] >taille_petit && y_cote_droit[i] < y_terrain-taille_petit) {
Artiom 44:badcbe8766e9 1900 cote_droit=true;
Artiom 44:badcbe8766e9 1901 cote=1;
Artiom 44:badcbe8766e9 1902 } else {
Artiom 44:badcbe8766e9 1903 cote_droit=false;
Artiom 44:badcbe8766e9 1904 break;
Artiom 44:badcbe8766e9 1905 }
Artiom 44:badcbe8766e9 1906
Artiom 44:badcbe8766e9 1907 if (x_cote_gauche[i]>taille_petit && x_cote_gauche[i]<x_terrain-taille_petit && y_cote_gauche[i] >taille_petit && y_cote_gauche[i] < y_terrain-taille_petit) {
Artiom 44:badcbe8766e9 1908 cote_gauche=true;
Artiom 44:badcbe8766e9 1909 cote=-1;
Artiom 44:badcbe8766e9 1910 } else {
Artiom 44:badcbe8766e9 1911 cote_gauche=false;
Artiom 44:badcbe8766e9 1912 break;
Artiom 44:badcbe8766e9 1913 }
Artiom 44:badcbe8766e9 1914 }
Artiom 44:badcbe8766e9 1915
Artiom 44:badcbe8766e9 1916 if(!cote_droit && !cote_gauche)cote=0;
Artiom 44:badcbe8766e9 1917
Artiom 44:badcbe8766e9 1918 if (cote_droit && cote_gauche) {
Artiom 44:badcbe8766e9 1919 if (ang_target<=0) {
Artiom 44:badcbe8766e9 1920 cote = -1;// cote gauche
Artiom 44:badcbe8766e9 1921 SendRawId(0x1D0);
Artiom 44:badcbe8766e9 1922 } else if (ang_target>0) {
Artiom 44:badcbe8766e9 1923 cote = 1; //cote droite
Artiom 44:badcbe8766e9 1924 SendRawId(0x2D0);
Artiom 44:badcbe8766e9 1925 }
Artiom 44:badcbe8766e9 1926 }
Artiom 44:badcbe8766e9 1927
Artiom 44:badcbe8766e9 1928 if ( ang_target>600 || ang_target<-600)cote=0;
Artiom 51:aa6e09f2cfec 1929 if (!cote_droit && !cote_gauche)cote=0;
Artiom 44:badcbe8766e9 1930 //--------------------test target --------------------------------------
Artiom 44:badcbe8766e9 1931 if ((x_robot_adversaire >= target_x_robot-proximity && x_robot_adversaire <= target_x_robot+proximity)&&(y_robot_adversaire >= target_y_robot-proximity && y_robot_adversaire <= target_y_robot+proximity)) cote=0;
Artiom 44:badcbe8766e9 1932
Artiom 50:a5361ffeefc8 1933 EvitEtat = 2;
Artiom 44:badcbe8766e9 1934
Artiom 44:badcbe8766e9 1935 break;
Artiom 44:badcbe8766e9 1936
Artiom 50:a5361ffeefc8 1937 case 2 ://on attend la fin de la première rotation pour activer la balise
Artiom 44:badcbe8766e9 1938 //ingnorBalise=1;
Artiom 44:badcbe8766e9 1939 SendRawId(0x0D1);//init evitement
Artiom 44:badcbe8766e9 1940 if(cote!=0) {
Artiom 44:badcbe8766e9 1941 for(int i=0; i<3; i++) {
Artiom 44:badcbe8766e9 1942 if(cote==-1) {
Artiom 51:aa6e09f2cfec 1943 GoToPosition(x_cote_droit[i],y_cote_droit[i],theta_robot,1);
Artiom 51:aa6e09f2cfec 1944 //GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
Artiom 44:badcbe8766e9 1945 SendRawId(0x1D1);//evitement a gauche
Artiom 44:badcbe8766e9 1946 } else if(cote==1) {
Artiom 51:aa6e09f2cfec 1947 GoToPosition(x_cote_gauche[i],y_cote_gauche[i],theta_robot,1);
Artiom 51:aa6e09f2cfec 1948 //GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
Artiom 44:badcbe8766e9 1949 SendRawId(0x2D1);//evitement a droite
Artiom 44:badcbe8766e9 1950 }
Artiom 44:badcbe8766e9 1951 waitingAckID = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1952 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1953 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1954 canProcessRx();
Artiom 44:badcbe8766e9 1955
Artiom 44:badcbe8766e9 1956 Fevitement=1;
Artiom 44:badcbe8766e9 1957 ingnorBalise=1;
Artiom 44:badcbe8766e9 1958 waitingAckID_FIN = ASSERVISSEMENT_XYT_ROTATE;
Artiom 44:badcbe8766e9 1959 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1960 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1961 canProcessRx();
Artiom 44:badcbe8766e9 1962
Artiom 44:badcbe8766e9 1963 ingnorBalise=0;
Artiom 44:badcbe8766e9 1964 waitingAckID_FIN = ASSERVISSEMENT_XYT_LINE;
Artiom 44:badcbe8766e9 1965 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1966 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1967 canProcessRx();
Artiom 44:badcbe8766e9 1968
Artiom 44:badcbe8766e9 1969 ingnorBalise=1;
Sitkah 38:76f886a1c8e6 1970 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Sitkah 38:76f886a1c8e6 1971 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1972 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1973 canProcessRx();
Artiom 44:badcbe8766e9 1974
Sitkah 38:76f886a1c8e6 1975 }
Artiom 44:badcbe8766e9 1976 } else {
Artiom 44:badcbe8766e9 1977 SendRawId(0x3D1);//cote=0 evitement non possible
Artiom 44:badcbe8766e9 1978 EvitEtat=0;
Artiom 44:badcbe8766e9 1979 gameEtat=ETAT_WARNING_END_LAST_INSTRUCTION;
Artiom 44:badcbe8766e9 1980 ingnorBalise=0;
Artiom 44:badcbe8766e9 1981 Fevitement=0;
Artiom 44:badcbe8766e9 1982 }
Artiom 51:aa6e09f2cfec 1983 EvitEtat=3;
Artiom 44:badcbe8766e9 1984 break;
Artiom 44:badcbe8766e9 1985
Artiom 51:aa6e09f2cfec 1986 case 3://on va vers l'objectif initial
Artiom 44:badcbe8766e9 1987 SendRawId(0x0D2);
Artiom 44:badcbe8766e9 1988 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
Artiom 44:badcbe8766e9 1989 waitingAckID = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1990 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1991 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1992 canProcessRx();
Artiom 44:badcbe8766e9 1993
Artiom 44:badcbe8766e9 1994 Fevitement=1;
Artiom 44:badcbe8766e9 1995 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1996 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1997 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1998 canProcessRx();
Artiom 44:badcbe8766e9 1999
Artiom 51:aa6e09f2cfec 2000 EvitEtat=4;
Artiom 44:badcbe8766e9 2001 break;
Artiom 44:badcbe8766e9 2002
Artiom 50:a5361ffeefc8 2003 case 4: //on charge l'instruction suivante et sort de l'evitement
Artiom 44:badcbe8766e9 2004 actual_instruction++;//on charge l'instruction suivante
Artiom 44:badcbe8766e9 2005 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 2006 EvitEtat = 0;
Artiom 44:badcbe8766e9 2007 ingnorBalise=0;
Artiom 44:badcbe8766e9 2008 Fevitement=0;
Artiom 44:badcbe8766e9 2009 break;
Artiom 50:a5361ffeefc8 2010
Artiom 50:a5361ffeefc8 2011 case 10:
Artiom 50:a5361ffeefc8 2012 wait(0.2);
Artiom 50:a5361ffeefc8 2013
Artiom 50:a5361ffeefc8 2014 sprintf(message,"%d ",theta_adversaire);
Artiom 50:a5361ffeefc8 2015 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2016 lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"Theta_adv : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2017 lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
Artiom 50:a5361ffeefc8 2018
Artiom 50:a5361ffeefc8 2019 sprintf(message1,"%04d mm",(short)dist_robot_adversaire);
Artiom 50:a5361ffeefc8 2020 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2021 lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"Dist_adv : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2022 lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
Artiom 50:a5361ffeefc8 2023 break;
Sitkah 37:fca332b64b42 2024 }
Artiom 44:badcbe8766e9 2025
Sitkah 37:fca332b64b42 2026 break;
Artiom 44:badcbe8766e9 2027
Artiom 44:badcbe8766e9 2028 case ETAT_END:
Artiom 44:badcbe8766e9 2029 if (ModeDemo) {
ClementBreteau 14:c8fc06c4887f 2030 gameEtat = ETAT_CHECK_CARTE_SCREEN;
ClementBreteau 14:c8fc06c4887f 2031 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 2032 } else {
ClementBreteau 14:c8fc06c4887f 2033 gameEtat = ETAT_END_LOOP;
ClementBreteau 14:c8fc06c4887f 2034 }
Artiom 44:badcbe8766e9 2035 break;
antbig 0:ad97421fb1fb 2036 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 2037 //Rien, on tourne en rond
Artiom 44:badcbe8766e9 2038
Artiom 44:badcbe8766e9 2039 break;
antbig 0:ad97421fb1fb 2040 default:
Artiom 44:badcbe8766e9 2041
Artiom 44:badcbe8766e9 2042 break;
Artiom 44:badcbe8766e9 2043 }
Artiom 44:badcbe8766e9 2044 }
Sitkah 29:41e02746041d 2045
Sitkah 29:41e02746041d 2046
Sitkah 29:41e02746041d 2047
Sitkah 29:41e02746041d 2048
Sitkah 29:41e02746041d 2049
antbig 4:88431b537477 2050 /****************************************************************************************/
antbig 4:88431b537477 2051 /* FUNCTION NAME: canProcessRx */
Sitkah 29:41e02746041d 2052 /* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
antbig 4:88431b537477 2053 /****************************************************************************************/
antbig 0:ad97421fb1fb 2054 void canProcessRx(void)
Sitkah 29:41e02746041d 2055 {
antbig 0:ad97421fb1fb 2056 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Sitkah 31:833fc481b002 2057 char message[10]="toto";
Sitkah 31:833fc481b002 2058 char message1[10]="toto";
Sitkah 31:833fc481b002 2059 char message2[10]="toto";
Sitkah 31:833fc481b002 2060 char message3[10]="toto";
Artiom 51:aa6e09f2cfec 2061 char message4[10]="toto";
antbig 0:ad97421fb1fb 2062 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 50:a5361ffeefc8 2063
antbig 0:ad97421fb1fb 2064 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 2065 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 50:a5361ffeefc8 2066
Artiom 50:a5361ffeefc8 2067 if(FIFO_max_occupation<FIFO_occupation) {
antbig 0:ad97421fb1fb 2068 FIFO_max_occupation=FIFO_occupation;
Artiom 50:a5361ffeefc8 2069 //SendRawId(
Artiom 50:a5361ffeefc8 2070 }
Artiom 50:a5361ffeefc8 2071
antbig 0:ad97421fb1fb 2072 if(FIFO_occupation!=0) {
Sitkah 42:657b6a573e11 2073 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 50:a5361ffeefc8 2074
Artiom 50:a5361ffeefc8 2075 if (waitingId == identifiant) waitingId = 0;
Sitkah 29:41e02746041d 2076 switch(identifiant) {
Artiom 44:badcbe8766e9 2077
Sitkah 29:41e02746041d 2078 case ALIVE_MOTEUR:
Artiom 44:badcbe8766e9 2079 if (etat == ATT) {
Artiom 44:badcbe8766e9 2080
Artiom 44:badcbe8766e9 2081 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2082 lcd.FillRect(0,400,400,150);
Artiom 44:badcbe8766e9 2083 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 2084 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2085 lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
Sitkah 29:41e02746041d 2086 }
Sitkah 29:41e02746041d 2087 break;
Artiom 44:badcbe8766e9 2088
Sitkah 29:41e02746041d 2089 case ALIVE_BALISE:
Artiom 44:badcbe8766e9 2090 if (etat == ATT) {
Artiom 44:badcbe8766e9 2091
Artiom 44:badcbe8766e9 2092 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2093 lcd.FillRect(0,600,400,150); //carte AX12
Artiom 44:badcbe8766e9 2094 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 2095 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2096 lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
Artiom 44:badcbe8766e9 2097 }
Sitkah 29:41e02746041d 2098 break;
Sitkah 29:41e02746041d 2099
Sitkah 29:41e02746041d 2100 case RESET_IHM:
Sitkah 29:41e02746041d 2101 etat = CHOIX;
Sitkah 29:41e02746041d 2102 break;
Sitkah 29:41e02746041d 2103
antbig 4:88431b537477 2104 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
ClementBreteau 23:ab87d308eaf9 2105 case GLOBAL_JACK:
antbig 1:116040d14164 2106 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 2107 gameEtat = ETAT_GAME_START;
ClementBreteau 23:ab87d308eaf9 2108 SendRawId(ACKNOWLEDGE_JACK);
antbig 1:116040d14164 2109 }
Artiom 44:badcbe8766e9 2110 break;
Artiom 44:badcbe8766e9 2111
Sitkah 30:a1e37af4bbde 2112 case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
ClementBreteau 23:ab87d308eaf9 2113 case ALIVE_ACTIONNEURS_ARRIERE:
Sitkah 30:a1e37af4bbde 2114 case ALIVE_HERKULEX:
antbig 0:ad97421fb1fb 2115 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 2116 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 2117 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 2118 }
Sitkah 38:76f886a1c8e6 2119 flag=1;
Artiom 44:badcbe8766e9 2120 break;
kyxstark 87:349194e73b77 2121
kyxstark 87:349194e73b77 2122 case INSTRUCTION_ERROR_MOTEUR//erreur asservissement
kyxstark 87:349194e73b77 2123 if(flagNonRepriseErrorMot)
kyxstark 87:349194e73b77 2124 {
kyxstark 87:349194e73b77 2125 actual_instruction = instruction.nextLineError;
kyxstark 87:349194e73b77 2126 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
kyxstark 87:349194e73b77 2127 flagNonRepriseErrorMot = 0;
kyxstark 87:349194e73b77 2128 }
kyxstark 87:349194e73b77 2129 else
kyxstark 87:349194e73b77 2130 {
kyxstark 87:349194e73b77 2131 flagNonRepriseErrorMot = 1;
kyxstark 87:349194e73b77 2132 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
kyxstark 87:349194e73b77 2133 }
kyxstark 87:349194e73b77 2134 break;
Artiom 44:badcbe8766e9 2135
Artiom 44:badcbe8766e9 2136 /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
Artiom 44:badcbe8766e9 2137 case ACKNOWLEDGE_HERKULEX:
Sitkah 42:657b6a573e11 2138 case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
Artiom 44:badcbe8766e9 2139
Artiom 44:badcbe8766e9 2140 case ACKNOWLEDGE_TELEMETRE:
Artiom 44:badcbe8766e9 2141 /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
Sitkah 34:6aa4b46b102e 2142 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 2143 case INSTRUCTION_END_BALISE:
Artiom 44:badcbe8766e9 2144 case ACK_FIN_ACTION:
antbig 0:ad97421fb1fb 2145 case INSTRUCTION_END_MOTEUR:
Artiom 44:badcbe8766e9 2146 unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
Artiom 44:badcbe8766e9 2147 memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
Artiom 44:badcbe8766e9 2148
Artiom 83:23e04b85ae06 2149 //on desactive la balise dans les rotations XYT
Artiom 62:c4863b4b2543 2150 if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1;
Artiom 62:c4863b4b2543 2151 if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0;
Artiom 83:23e04b85ae06 2152
Artiom 83:23e04b85ae06 2153 //on desactive la balise dans les rotations
Artiom 83:23e04b85ae06 2154 if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=1;
Artiom 83:23e04b85ae06 2155 if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=0;
kyxstark 87:349194e73b77 2156
Artiom 84:44d6cd2cab99 2157
Artiom 83:23e04b85ae06 2158 // SendMsgCan(0x666,&ingnorBalise,1);
Artiom 83:23e04b85ae06 2159
Artiom 44:badcbe8766e9 2160 if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
antbig 0:ad97421fb1fb 2161 waitingAckFrom = 0;
Artiom 44:badcbe8766e9 2162 waitingAckID = 0;
Artiom 44:badcbe8766e9 2163 }
Artiom 44:badcbe8766e9 2164 if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
Artiom 44:badcbe8766e9 2165 waitingAckFrom_FIN = 0;
Artiom 44:badcbe8766e9 2166 waitingAckID_FIN = 0;
antbig 0:ad97421fb1fb 2167 }
Artiom 44:badcbe8766e9 2168
Artiom 44:badcbe8766e9 2169 /*
Artiom 44:badcbe8766e9 2170 if((waitingAckFrom == msgRxBuffer[FIFO_lecture].id) &&
Artiom 44:badcbe8766e9 2171 ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] | (((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID) )
Artiom 44:badcbe8766e9 2172 {
Artiom 44:badcbe8766e9 2173 waitingAckFrom = 0;
Artiom 44:badcbe8766e9 2174 waitingAckID = 0;
Artiom 44:badcbe8766e9 2175 }
Artiom 44:badcbe8766e9 2176 if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
Artiom 44:badcbe8766e9 2177 |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
Artiom 44:badcbe8766e9 2178 {
Artiom 44:badcbe8766e9 2179 waitingAckFrom_FIN = 0;
Artiom 44:badcbe8766e9 2180 waitingAckID_FIN = 0;
Artiom 44:badcbe8766e9 2181 }
Artiom 44:badcbe8766e9 2182 */
Artiom 44:badcbe8766e9 2183 break;
Artiom 44:badcbe8766e9 2184
Artiom 44:badcbe8766e9 2185
Artiom 78:c0533a36da8f 2186 #ifdef ROBOT_BIG
Artiom 78:c0533a36da8f 2187 #define point_balance 12
Artiom 78:c0533a36da8f 2188 #define point_accelerateur 10
Artiom 78:c0533a36da8f 2189 case NB_PALETS_BLEU: //nb de palets bleu mis dans l'accelerateur
Artiom 78:c0533a36da8f 2190 unsigned short palets_bleu=(unsigned short)msgRxBuffer[FIFO_lecture].data[0];
Artiom 78:c0533a36da8f 2191 SCORE_GR+=palets_bleu*point_balance;
Artiom 78:c0533a36da8f 2192 //SCORE_GLOBAL=SCORE_GR+SCORE_PR;
Artiom 78:c0533a36da8f 2193 //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
Artiom 78:c0533a36da8f 2194 break;
Artiom 78:c0533a36da8f 2195
Artiom 78:c0533a36da8f 2196 case NB_PALETS_VERTS://nb de palets vert/rouge mis dans l'accelerateur
Artiom 78:c0533a36da8f 2197 unsigned short palets_verts=(unsigned short)msgRxBuffer[FIFO_lecture].data[0];
Artiom 78:c0533a36da8f 2198 SCORE_GR+=palets_verts*point_accelerateur;
Artiom 78:c0533a36da8f 2199 //SCORE_GLOBAL=SCORE_GR+SCORE_PR;
Artiom 78:c0533a36da8f 2200 //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
Artiom 78:c0533a36da8f 2201 break;
Artiom 78:c0533a36da8f 2202 #endif
antbig 0:ad97421fb1fb 2203 case ODOMETRIE_BIG_POSITION:
antbig 0:ad97421fb1fb 2204 case ODOMETRIE_SMALL_POSITION:
Artiom 62:c4863b4b2543 2205
antbig 0:ad97421fb1fb 2206 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 2207 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 2208 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
Artiom 44:badcbe8766e9 2209 break;
Artiom 44:badcbe8766e9 2210
Sitkah 30:a1e37af4bbde 2211 case ACK_ACTION:
Sitkah 30:a1e37af4bbde 2212 if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
Sitkah 30:a1e37af4bbde 2213 waitingAckFrom = 0;
Sitkah 30:a1e37af4bbde 2214 waitingAckID = 0;
Sitkah 30:a1e37af4bbde 2215 }
Sitkah 30:a1e37af4bbde 2216 break;
Artiom 44:badcbe8766e9 2217
ClementBreteau 16:7321fb3bb396 2218 case BALISE_DANGER :
ClementBreteau 16:7321fb3bb396 2219 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
Artiom 44:badcbe8766e9 2220 break;
Artiom 44:badcbe8766e9 2221
antbig 5:dcd817534b57 2222 case BALISE_STOP:
antbig 5:dcd817534b57 2223 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
Artiom 44:badcbe8766e9 2224
Sitkah 37:fca332b64b42 2225 signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
Artiom 44:badcbe8766e9 2226 signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
Artiom 44:badcbe8766e9 2227
Artiom 44:badcbe8766e9 2228 if(debut_angle_detection > fin_angle_detection) {
Sitkah 38:76f886a1c8e6 2229 angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
Sitkah 38:76f886a1c8e6 2230 if(angle_moyen_balise_IR > 15.0f)
Sitkah 38:76f886a1c8e6 2231 angle_moyen_balise_IR-=15.0f;
Artiom 44:badcbe8766e9 2232 } else
Sitkah 37:fca332b64b42 2233 angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
Artiom 44:badcbe8766e9 2234
Artiom 44:badcbe8766e9 2235 #ifdef ROBOT_BIG
Artiom 80:545a9bf561e1 2236 /* float seuil_bas_avant = 12.0; // >=
Artiom 80:545a9bf561e1 2237 float seuil_haut_avant = 1.0; // <=0.0
Artiom 80:545a9bf561e1 2238 float seuil_bas_arriere = 4.0;
Artiom 80:545a9bf561e1 2239 float seuil_haut_arriere = 8.0;*/
Artiom 81:ef50ec0ef328 2240 float seuil_bas_avant = 12.0;
Artiom 80:545a9bf561e1 2241 float seuil_haut_avant = 15.0;
Artiom 80:545a9bf561e1 2242 float seuil_bas_arriere = 5.0;
Artiom 80:545a9bf561e1 2243 float seuil_haut_arriere = 7.0;
Artiom 80:545a9bf561e1 2244
Artiom 80:545a9bf561e1 2245 if (angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)Send2Short(0x667,2,2);
Artiom 80:545a9bf561e1 2246 if (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)Send2Short(0x667,1,1);
Artiom 78:c0533a36da8f 2247 #else
Sitkah 38:76f886a1c8e6 2248 float seuil_bas_avant = 13.0;
Sitkah 38:76f886a1c8e6 2249 float seuil_haut_avant = 15.0;
Sitkah 38:76f886a1c8e6 2250 float seuil_bas_arriere = 5.0;
Sitkah 38:76f886a1c8e6 2251 float seuil_haut_arriere = 7.0;
Artiom 80:545a9bf561e1 2252
Artiom 80:545a9bf561e1 2253 if (angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)Send2Short(0x667,2,2);
Artiom 80:545a9bf561e1 2254 if (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)Send2Short(0x667,1,1);
Artiom 78:c0533a36da8f 2255 #endif
Artiom 63:bcfe62e3f1d2 2256 if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 83:23e04b85ae06 2257 || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 83:23e04b85ae06 2258 || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 83:23e04b85ae06 2259 || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 83:23e04b85ae06 2260 || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 83:23e04b85ae06 2261 || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere )
Artiom 83:23e04b85ae06 2262 { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
Artiom 83:23e04b85ae06 2263
Artiom 83:23e04b85ae06 2264 if( (needToStop() !=0) && (ingnorBaliseOnce ==0) && (ingnorBalise==0) ) {
Artiom 83:23e04b85ae06 2265
Artiom 83:23e04b85ae06 2266 if( needToStop() == 3) {//on regarde les telemetres
Artiom 83:23e04b85ae06 2267 memGameEtat = gameEtat;
Artiom 83:23e04b85ae06 2268 gameEtat = ETAT_TELEMETRE_BALANCE;
Artiom 83:23e04b85ae06 2269 }
Artiom 83:23e04b85ae06 2270 else if( (gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT) ) {
ClementBreteau 16:7321fb3bb396 2271 SendRawId(ASSERVISSEMENT_STOP);
Artiom 83:23e04b85ae06 2272 timeoutWarning.reset();
Artiom 83:23e04b85ae06 2273 timeoutWarning.start();//Reset du timer utiliser par le timeout
antbig 27:76ead555a63d 2274 gameEtat = ETAT_WARNING_TIMEOUT;
antbig 12:14729d584500 2275 }
Artiom 83:23e04b85ae06 2276 ingnorBaliseOnce = 0;
antbig 12:14729d584500 2277 }
Artiom 82:759ceb2a8a70 2278 } else if(gameEtat==ETAT_WARNING_TIMEOUT) {
Artiom 83:23e04b85ae06 2279 /* if (!(instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 82:759ceb2a8a70 2280 || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 82:759ceb2a8a70 2281 || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 82:759ceb2a8a70 2282 || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 82:759ceb2a8a70 2283 || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 82:759ceb2a8a70 2284 || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere) ) {
Artiom 83:23e04b85ae06 2285 */
Artiom 82:759ceb2a8a70 2286 timeoutWarningWaitEnd.reset();
Artiom 82:759ceb2a8a70 2287 timeoutWarningWaitEnd.start();
Artiom 82:759ceb2a8a70 2288 gameEtat = ETAT_WARING_END_BALISE_WAIT;
Artiom 83:23e04b85ae06 2289 // }
antbig 28:acd18776ed2d 2290 }
Artiom 82:759ceb2a8a70 2291
Artiom 44:badcbe8766e9 2292 break;
Artiom 44:badcbe8766e9 2293
antbig 5:dcd817534b57 2294 case BALISE_END_DANGER:
ClementBreteau 16:7321fb3bb396 2295 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
antbig 5:dcd817534b57 2296 if(gameEtat == ETAT_WARNING_TIMEOUT) {
antbig 5:dcd817534b57 2297 timeoutWarningWaitEnd.reset();
antbig 5:dcd817534b57 2298 timeoutWarningWaitEnd.start();
antbig 5:dcd817534b57 2299 gameEtat = ETAT_WARING_END_BALISE_WAIT;
antbig 5:dcd817534b57 2300 }
Artiom 82:759ceb2a8a70 2301
Artiom 44:badcbe8766e9 2302 break;
Artiom 44:badcbe8766e9 2303
Sitkah 30:a1e37af4bbde 2304 case RECEPTION_DATA:
Sitkah 30:a1e37af4bbde 2305 telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
Sitkah 38:76f886a1c8e6 2306 telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
Sitkah 30:a1e37af4bbde 2307 waitingAckFrom = 0;
Sitkah 30:a1e37af4bbde 2308 waitingAckID = 0;
Sitkah 30:a1e37af4bbde 2309 break;
Artiom 44:badcbe8766e9 2310
Sitkah 31:833fc481b002 2311 case RECEPTION_RECALAGE:
Artiom 83:23e04b85ae06 2312 wait_us(150);flagReceptionTelemetres = 1;
Artiom 50:a5361ffeefc8 2313 // telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
Artiom 50:a5361ffeefc8 2314 memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
Artiom 49:d83a4851a257 2315 telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
Sitkah 34:6aa4b46b102e 2316 telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
Artiom 49:d83a4851a257 2317 telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
Artiom 44:badcbe8766e9 2318
Artiom 44:badcbe8766e9 2319
Artiom 44:badcbe8766e9 2320 if(ModeDemo==1) {
Artiom 62:c4863b4b2543 2321 sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt);
Sitkah 31:833fc481b002 2322 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2323 lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2324 lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
Artiom 44:badcbe8766e9 2325
Artiom 62:c4863b4b2543 2326 sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt);
Sitkah 31:833fc481b002 2327 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2328 lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2329 lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
Artiom 62:c4863b4b2543 2330
Artiom 62:c4863b4b2543 2331
Artiom 62:c4863b4b2543 2332 sprintf(message4,"%04d",theta_robot);
Artiom 51:aa6e09f2cfec 2333 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 51:aa6e09f2cfec 2334 lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
Artiom 51:aa6e09f2cfec 2335 lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
Artiom 51:aa6e09f2cfec 2336
Artiom 44:badcbe8766e9 2337
Artiom 62:c4863b4b2543 2338 sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt);
Sitkah 31:833fc481b002 2339 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2340 lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2341 lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
Artiom 44:badcbe8766e9 2342
Artiom 62:c4863b4b2543 2343 sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt);
Sitkah 31:833fc481b002 2344 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2345 lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2346 lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
Sitkah 31:833fc481b002 2347 }
Sitkah 30:a1e37af4bbde 2348 break;
Artiom 62:c4863b4b2543 2349 case RECEPTION_TELEMETRE_LOGIQUE:
Artiom 62:c4863b4b2543 2350
Artiom 62:c4863b4b2543 2351 DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0];
Artiom 62:c4863b4b2543 2352 DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1];
Artiom 62:c4863b4b2543 2353 DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2];
Artiom 62:c4863b4b2543 2354 DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3];
Artiom 62:c4863b4b2543 2355
Artiom 62:c4863b4b2543 2356 break;
Sitkah 30:a1e37af4bbde 2357 case RECEPTION_COULEUR:
Artiom 44:badcbe8766e9 2358 if (blocage_balise==0) {
Artiom 44:badcbe8766e9 2359 couleur1=msgRxBuffer[FIFO_lecture].data[0];
Artiom 44:badcbe8766e9 2360 couleur2=msgRxBuffer[FIFO_lecture].data[1];
Artiom 44:badcbe8766e9 2361 couleur3=msgRxBuffer[FIFO_lecture].data[2];
Artiom 44:badcbe8766e9 2362
Artiom 44:badcbe8766e9 2363 /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
Artiom 44:badcbe8766e9 2364 lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
Artiom 44:badcbe8766e9 2365 lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
Sitkah 41:b029ddc4d60e 2366 }
Artiom 44:badcbe8766e9 2367
Artiom 44:badcbe8766e9 2368 break;
Artiom 62:c4863b4b2543 2369 /*
Artiom 62:c4863b4b2543 2370 case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
Artiom 62:c4863b4b2543 2371 actual_instruction = instruction.nextLineError;
Artiom 62:c4863b4b2543 2372 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 62:c4863b4b2543 2373 // waitingAckID_FIN=0;
Artiom 62:c4863b4b2543 2374 // waitingAckFrom_FIN=0;
Artiom 62:c4863b4b2543 2375 SendRawId(0x40);
Artiom 62:c4863b4b2543 2376 break;*/
Sitkah 29:41e02746041d 2377 }
antbig 0:ad97421fb1fb 2378 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 2379 }
antbig 0:ad97421fb1fb 2380 }
Sitkah 29:41e02746041d 2381
Sitkah 29:41e02746041d 2382
Sitkah 29:41e02746041d 2383
Sitkah 29:41e02746041d 2384
Sitkah 29:41e02746041d 2385
Sitkah 29:41e02746041d 2386 /****************************************************************************************/
Sitkah 29:41e02746041d 2387 /* FUNCTION NAME: Bouton_Strat */
Sitkah 29:41e02746041d 2388 /* DESCRIPTION : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!) */
Sitkah 29:41e02746041d 2389 /****************************************************************************************/
Sitkah 29:41e02746041d 2390 signed char Bouton_Strat (void)
Artiom 44:badcbe8766e9 2391 {
Sitkah 29:41e02746041d 2392 Button STRAT_1 (0, 30, 190, 110, strat_sd[0]);
Sitkah 29:41e02746041d 2393 Button STRAT_2 (210, 30, 190, 110, strat_sd[1]);
Sitkah 29:41e02746041d 2394 Button STRAT_3 (0, 150, 190, 110, strat_sd[2]);
Sitkah 29:41e02746041d 2395 Button STRAT_4 (210, 150, 190, 110, strat_sd[3]);
Sitkah 29:41e02746041d 2396 Button STRAT_5 (0, 270, 190, 110,strat_sd[4]);
Sitkah 29:41e02746041d 2397 Button STRAT_6 (210, 270, 190, 110, strat_sd[5]);
Sitkah 29:41e02746041d 2398 Button STRAT_7 (0, 390, 190, 110, strat_sd[6]);
Sitkah 29:41e02746041d 2399 Button STRAT_8 (210, 390, 190, 110, strat_sd[7]);
Sitkah 29:41e02746041d 2400 Button STRAT_9 (0, 510, 190, 110, strat_sd[8]);
Sitkah 29:41e02746041d 2401 Button STRAT_10 (210, 510, 190, 110, strat_sd[9]);
Sitkah 29:41e02746041d 2402 Button RETOUR (0, 680, 400, 110, "--Precedent--");
Sitkah 29:41e02746041d 2403 //Definition des boutons
Artiom 44:badcbe8766e9 2404
Sitkah 29:41e02746041d 2405 Ack_strat = 0;
Sitkah 29:41e02746041d 2406 Strat = 0;
Sitkah 29:41e02746041d 2407 STRAT_1.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2408 STRAT_2.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2409 STRAT_3.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2410 STRAT_4.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2411 STRAT_5.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2412 STRAT_6.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2413 STRAT_7.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2414 STRAT_8.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 2415 STRAT_9.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 2416 STRAT_10.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2417 RETOUR.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 2418
Artiom 44:badcbe8766e9 2419 while(Ack_strat == 0) {
Sitkah 29:41e02746041d 2420 canProcessRx();
Sitkah 29:41e02746041d 2421 CANMessage msgTx=CANMessage();
Sitkah 30:a1e37af4bbde 2422 //msgTx.id=ECRAN_CHOICE_STRAT;
Sitkah 29:41e02746041d 2423 if (RETOUR.Touched())
Sitkah 29:41e02746041d 2424 return -1;
Sitkah 29:41e02746041d 2425 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 2426 //////////////////////////////STRATEGIE N°1
Artiom 44:badcbe8766e9 2427 if (STRAT_1.Touched()) {
Artiom 44:badcbe8766e9 2428 Strat = 0;
Artiom 44:badcbe8766e9 2429 //msgTx.data[0] = 0x1;
Artiom 44:badcbe8766e9 2430 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2431 while(STRAT_1.Touched());
Artiom 44:badcbe8766e9 2432 Ack_strat =1;
Artiom 44:badcbe8766e9 2433 }
Artiom 44:badcbe8766e9 2434 /////////////////////////////STRATEGIE N°2
Artiom 44:badcbe8766e9 2435 if (STRAT_2.Touched()) {
Artiom 44:badcbe8766e9 2436 Strat = 1;
Artiom 44:badcbe8766e9 2437 //msgTx.data[0] = 0x2;
Artiom 44:badcbe8766e9 2438 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2439 while(STRAT_2.Touched());
Artiom 44:badcbe8766e9 2440 Ack_strat =1;
Artiom 44:badcbe8766e9 2441 }
Artiom 44:badcbe8766e9 2442 //////////////////////////////STRATEGIE N°3
Artiom 44:badcbe8766e9 2443 if (STRAT_3.Touched()) {
Artiom 44:badcbe8766e9 2444 Strat = 2;
Artiom 44:badcbe8766e9 2445 //msgTx.data[0] = 0x3;
Artiom 44:badcbe8766e9 2446 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2447 while(STRAT_3.Touched());
Artiom 44:badcbe8766e9 2448 Ack_strat =1;
Artiom 44:badcbe8766e9 2449 }
Artiom 44:badcbe8766e9 2450 /////////////////////////////STRATEGIE N°4
Artiom 44:badcbe8766e9 2451 if (STRAT_4.Touched()) {
Artiom 44:badcbe8766e9 2452 Strat = 3;
Artiom 44:badcbe8766e9 2453 //msgTx.data[0] = 0x4;
Artiom 44:badcbe8766e9 2454 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2455 while(STRAT_4.Touched());
Artiom 44:badcbe8766e9 2456 Ack_strat =1;
Artiom 44:badcbe8766e9 2457 }
Artiom 44:badcbe8766e9 2458 ///////////////////////////////STRATEGIE N°5
Artiom 44:badcbe8766e9 2459 if (STRAT_5.Touched()) {
Artiom 44:badcbe8766e9 2460 Strat = 4;
Artiom 44:badcbe8766e9 2461 //msgTx.data[0] = 0x5;
Artiom 44:badcbe8766e9 2462 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2463 while(STRAT_5.Touched());
Artiom 44:badcbe8766e9 2464 Ack_strat =1;
Artiom 44:badcbe8766e9 2465 }
Artiom 44:badcbe8766e9 2466 ////////////////////////////////STRATEGIE N°6
Artiom 44:badcbe8766e9 2467 if (STRAT_6.Touched()) {
Artiom 44:badcbe8766e9 2468 Strat = 5;
Artiom 44:badcbe8766e9 2469 //msgTx.data[0] = 0x6;
Artiom 44:badcbe8766e9 2470 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2471 while(STRAT_6.Touched());
Artiom 44:badcbe8766e9 2472 Ack_strat =1;
Artiom 44:badcbe8766e9 2473 }
Artiom 44:badcbe8766e9 2474 /////////////////////////////////STRATEGIE N°7
Artiom 44:badcbe8766e9 2475 if (STRAT_7.Touched()) {
Artiom 44:badcbe8766e9 2476 Strat = 6;
Artiom 44:badcbe8766e9 2477 //msgTx.data[0] = 0x7;
Artiom 44:badcbe8766e9 2478 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2479 while(STRAT_7.Touched());
Artiom 44:badcbe8766e9 2480 Ack_strat =1;
Artiom 44:badcbe8766e9 2481 }
Artiom 44:badcbe8766e9 2482 /////////////////////////////////STRATEGIE N°8
Artiom 44:badcbe8766e9 2483 if (STRAT_8.Touched()) {
Artiom 44:badcbe8766e9 2484 Strat = 7;
Artiom 44:badcbe8766e9 2485 //msgTx.data[0] = 0x8;
Artiom 44:badcbe8766e9 2486 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2487 while(STRAT_8.Touched());
Artiom 44:badcbe8766e9 2488 Ack_strat =1;
Artiom 44:badcbe8766e9 2489 }
Artiom 44:badcbe8766e9 2490 /////////////////////////////////STRATEGIE N°9
Artiom 44:badcbe8766e9 2491 if (STRAT_9.Touched()) {
Artiom 44:badcbe8766e9 2492 Strat = 8;
Artiom 44:badcbe8766e9 2493 //msgTx.data[0] = 0x9;
Artiom 44:badcbe8766e9 2494 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2495 while(STRAT_9.Touched());
Artiom 44:badcbe8766e9 2496 Ack_strat =1;
Artiom 44:badcbe8766e9 2497 }
Artiom 44:badcbe8766e9 2498 ///////////////////////////////////STRATEGIE N°10
Artiom 44:badcbe8766e9 2499 if (STRAT_10.Touched()) {
Artiom 44:badcbe8766e9 2500 Strat = 9;
Artiom 44:badcbe8766e9 2501 //msgTx.data[0] = 0xA;
Artiom 44:badcbe8766e9 2502 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2503 while(STRAT_10.Touched());
Artiom 44:badcbe8766e9 2504 Ack_strat =1;
Artiom 44:badcbe8766e9 2505 }
Artiom 44:badcbe8766e9 2506
Sitkah 29:41e02746041d 2507 }
Artiom 44:badcbe8766e9 2508 return Strat;
Artiom 44:badcbe8766e9 2509
Sitkah 29:41e02746041d 2510 }
Sitkah 29:41e02746041d 2511
Sitkah 29:41e02746041d 2512 void affichage_compteur (int nombre)
Sitkah 29:41e02746041d 2513 {
Sitkah 38:76f886a1c8e6 2514 int dizaine=0,unite=0,centaine=0;
Artiom 80:545a9bf561e1 2515 centaine = nombre/100;
Sitkah 29:41e02746041d 2516 dizaine = nombre/10;
Sitkah 29:41e02746041d 2517 unite = nombre-(10*dizaine);
Sitkah 38:76f886a1c8e6 2518 print_segment(unite,-50);
Sitkah 40:21bb685b553b 2519 print_segment(dizaine,100);
Artiom 44:badcbe8766e9 2520 if(centaine!=0) {
Sitkah 38:76f886a1c8e6 2521 print_segment(centaine,350);
Sitkah 38:76f886a1c8e6 2522 }
Artiom 44:badcbe8766e9 2523
Sitkah 29:41e02746041d 2524 }
Sitkah 29:41e02746041d 2525
Sitkah 29:41e02746041d 2526
Sitkah 29:41e02746041d 2527 //****print_segment***
Sitkah 29:41e02746041d 2528 //Dessine en 7 segment le nombre en parametre
Artiom 44:badcbe8766e9 2529 // A
Sitkah 29:41e02746041d 2530 // =====
Sitkah 29:41e02746041d 2531 // | |
Sitkah 29:41e02746041d 2532 // B | G | E
Sitkah 29:41e02746041d 2533 // |=====|
Sitkah 29:41e02746041d 2534 // C | | F
Sitkah 29:41e02746041d 2535 // | |
Sitkah 29:41e02746041d 2536 // =====
Sitkah 29:41e02746041d 2537 // D
Sitkah 29:41e02746041d 2538 /*
Sitkah 29:41e02746041d 2539 position pour le chiffre des unites
Sitkah 29:41e02746041d 2540 lcd.FillRect(460,75,120,25);// A
Artiom 44:badcbe8766e9 2541 lcd.FillRect(435,100,25,120);// B
Sitkah 29:41e02746041d 2542 lcd.FillRect(435,245,25,120);// C
Sitkah 29:41e02746041d 2543 lcd.FillRect(460,365,120,25);// D
Sitkah 29:41e02746041d 2544 lcd.FillRect(580,100,25,120);// E
Sitkah 29:41e02746041d 2545 lcd.FillRect(580,245,25,120);// F
Sitkah 29:41e02746041d 2546 lcd.FillRect(460,220,120,25);// G
Sitkah 29:41e02746041d 2547
Sitkah 29:41e02746041d 2548 position pour le chiffre des dizaines
Sitkah 29:41e02746041d 2549 lcd.FillRect(260,75,120,25);// A
Artiom 44:badcbe8766e9 2550 lcd.FillRect(235,100,25,120);// B
Sitkah 29:41e02746041d 2551 lcd.FillRect(235,245,25,120);// C
Sitkah 29:41e02746041d 2552 lcd.FillRect(260,365,120,25);// D
Sitkah 29:41e02746041d 2553 lcd.FillRect(380,100,25,120);// E
Sitkah 29:41e02746041d 2554 lcd.FillRect(380,245,25,120);// F
Sitkah 29:41e02746041d 2555 lcd.FillRect(260,220,120,25);// G
Sitkah 29:41e02746041d 2556 */
Artiom 44:badcbe8766e9 2557
Sitkah 29:41e02746041d 2558 void print_segment(int nombre, int decalage)
Artiom 44:badcbe8766e9 2559 {
Artiom 44:badcbe8766e9 2560
Artiom 44:badcbe8766e9 2561 switch(nombre) {
Sitkah 29:41e02746041d 2562 case 0:
Artiom 44:badcbe8766e9 2563 lcd.FillRect(240-decalage,75,120,25);
Artiom 44:badcbe8766e9 2564 lcd.FillRect(215-decalage,100,25,120);
Artiom 44:badcbe8766e9 2565 lcd.FillRect(215-decalage,245,25,120);
Artiom 44:badcbe8766e9 2566 lcd.FillRect(360-decalage,245,25,120);
Artiom 44:badcbe8766e9 2567 lcd.FillRect(360-decalage,100,25,120);
Artiom 44:badcbe8766e9 2568 lcd.FillRect(240-decalage,365,120,25);
Artiom 44:badcbe8766e9 2569 break;
Artiom 44:badcbe8766e9 2570
Sitkah 29:41e02746041d 2571 case 1:
Artiom 44:badcbe8766e9 2572 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2573 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2574 break;
Artiom 44:badcbe8766e9 2575
Sitkah 29:41e02746041d 2576 case 2:
Artiom 44:badcbe8766e9 2577 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2578 lcd.FillRect(215-decalage,245,25,120);// C
Artiom 44:badcbe8766e9 2579 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2580 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2581 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2582 break;
Artiom 44:badcbe8766e9 2583
Sitkah 29:41e02746041d 2584 case 3:
Artiom 44:badcbe8766e9 2585 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2586 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2587 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2588 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2589 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2590 break;
Artiom 44:badcbe8766e9 2591
Sitkah 29:41e02746041d 2592 case 4:
Artiom 44:badcbe8766e9 2593 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2594 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2595 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2596 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2597 break;
Artiom 44:badcbe8766e9 2598
Sitkah 29:41e02746041d 2599 case 5:
Artiom 44:badcbe8766e9 2600 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2601 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2602 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2603 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2604 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2605 break;
Artiom 44:badcbe8766e9 2606
Sitkah 29:41e02746041d 2607 case 6:
Artiom 44:badcbe8766e9 2608 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2609 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2610 lcd.FillRect(215-decalage,245,25,120);// C
Artiom 44:badcbe8766e9 2611 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2612 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2613 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2614 break;
Artiom 44:badcbe8766e9 2615
Sitkah 29:41e02746041d 2616 case 7:
Artiom 44:badcbe8766e9 2617 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2618 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2619 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2620 break;
Artiom 44:badcbe8766e9 2621
Sitkah 29:41e02746041d 2622 case 8:
Artiom 44:badcbe8766e9 2623 lcd.FillRect(240-decalage,75,120,25); // A
Artiom 44:badcbe8766e9 2624 lcd.FillRect(215-decalage,100,25,120);
Artiom 44:badcbe8766e9 2625 lcd.FillRect(215-decalage,245,25,120);
Artiom 44:badcbe8766e9 2626 lcd.FillRect(360-decalage,245,25,120);//...
Artiom 44:badcbe8766e9 2627 lcd.FillRect(360-decalage,100,25,120);
Artiom 44:badcbe8766e9 2628 lcd.FillRect(240-decalage,365,120,25);
Artiom 44:badcbe8766e9 2629 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2630 break;
Artiom 44:badcbe8766e9 2631
Sitkah 29:41e02746041d 2632 case 9:
Artiom 44:badcbe8766e9 2633 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2634 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2635 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2636 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2637 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2638 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2639 break;
Sitkah 29:41e02746041d 2640 }
Sitkah 29:41e02746041d 2641 }
Sitkah 29:41e02746041d 2642
Sitkah 29:41e02746041d 2643 void effacer_segment(long couleur)
Sitkah 29:41e02746041d 2644 {
Sitkah 29:41e02746041d 2645 lcd.SetTextColor(couleur);
Sitkah 29:41e02746041d 2646 lcd.FillRect(240-200,75,120,25); // A
Sitkah 29:41e02746041d 2647 lcd.FillRect(215-200,100,25,120);
Sitkah 29:41e02746041d 2648 lcd.FillRect(215-200,245,25,120);
Sitkah 29:41e02746041d 2649 lcd.FillRect(360-200,245,25,120);//...
Sitkah 29:41e02746041d 2650 lcd.FillRect(360-200,100,25,120);
Sitkah 29:41e02746041d 2651 lcd.FillRect(240-200,365,120,25);
Sitkah 29:41e02746041d 2652 lcd.FillRect(240-200,220,120,25);// G
Artiom 44:badcbe8766e9 2653
Sitkah 29:41e02746041d 2654 lcd.FillRect(240,75,120,25); // A
Sitkah 29:41e02746041d 2655 lcd.FillRect(215,100,25,120);
Sitkah 29:41e02746041d 2656 lcd.FillRect(215,245,25,120);
Sitkah 29:41e02746041d 2657 lcd.FillRect(360,245,25,120);//...
Sitkah 29:41e02746041d 2658 lcd.FillRect(360,100,25,120);
Sitkah 29:41e02746041d 2659 lcd.FillRect(240,365,120,25);
Sitkah 29:41e02746041d 2660 lcd.FillRect(240,220,120,25);// G
Sitkah 29:41e02746041d 2661 }
Sitkah 36:6dd30780bd8e 2662
Sitkah 36:6dd30780bd8e 2663 short recalageAngulaireCapteur(void)
Sitkah 36:6dd30780bd8e 2664 {
Sitkah 36:6dd30780bd8e 2665 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2666 unsigned int moyennageTelemetre = 0;
Sitkah 36:6dd30780bd8e 2667 unsigned short angleAvant = 0;
Sitkah 36:6dd30780bd8e 2668 unsigned short angleArriere = 0;
Sitkah 36:6dd30780bd8e 2669 unsigned short orientationArrondie = 0;
Artiom 44:badcbe8766e9 2670
Sitkah 36:6dd30780bd8e 2671 unsigned short position_avant_gauche=0;
Sitkah 36:6dd30780bd8e 2672 unsigned short position_avant_droite=0;
Sitkah 36:6dd30780bd8e 2673 unsigned short position_arriere_gauche=0;
Sitkah 36:6dd30780bd8e 2674 unsigned short position_arriere_droite=0;
Villanut 75:1db1b929f13d 2675
Artiom 67:96f914f92d2d 2676 unsigned short tempo= telemetreDistance_arriere_gauche;
Artiom 67:96f914f92d2d 2677 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
Artiom 67:96f914f92d2d 2678 telemetreDistance_arriere_droite=tempo;
Villanut 75:1db1b929f13d 2679
Villanut 75:1db1b929f13d 2680
Villanut 75:1db1b929f13d 2681
Sitkah 36:6dd30780bd8e 2682 if(theta_robot >= 450 && theta_robot <= 1350)
Artiom 44:badcbe8766e9 2683 orientationArrondie = 90;
Sitkah 36:6dd30780bd8e 2684 else if(theta_robot <= -450 && theta_robot >= -1350)
Sitkah 36:6dd30780bd8e 2685 orientationArrondie = 270;
Sitkah 36:6dd30780bd8e 2686 else if(theta_robot <= 450 && theta_robot >= -450)
Sitkah 36:6dd30780bd8e 2687 orientationArrondie = 0;
Sitkah 36:6dd30780bd8e 2688 else if(theta_robot >= 1350 && theta_robot <= -1350)
Sitkah 36:6dd30780bd8e 2689 orientationArrondie = 180;
Artiom 44:badcbe8766e9 2690
Sitkah 36:6dd30780bd8e 2691 // Calcul de position pour faire la vérification de cohérence
Artiom 44:badcbe8766e9 2692 if(orientationArrondie == 90 || orientationArrondie == 270) {
Sitkah 36:6dd30780bd8e 2693 position_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2694 position_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2695 position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2696 position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
Artiom 44:badcbe8766e9 2697
Artiom 44:badcbe8766e9 2698 } else if(orientationArrondie == 0 || orientationArrondie == 180) {
Sitkah 36:6dd30780bd8e 2699 position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2700 position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2701 position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2702 position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2703 }
Artiom 44:badcbe8766e9 2704
Artiom 44:badcbe8766e9 2705
Artiom 44:badcbe8766e9 2706 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
Artiom 44:badcbe8766e9 2707 if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2708 if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2709 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
Artiom 44:badcbe8766e9 2710 angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2711 else
Sitkah 36:6dd30780bd8e 2712 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
Artiom 44:badcbe8766e9 2713
Sitkah 36:6dd30780bd8e 2714 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2715 moyennageTelemetre += angleArriere;
Sitkah 36:6dd30780bd8e 2716 }
Sitkah 36:6dd30780bd8e 2717 }
Artiom 44:badcbe8766e9 2718 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
Artiom 44:badcbe8766e9 2719 if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2720 if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2721 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
Artiom 44:badcbe8766e9 2722 angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2723 else
Sitkah 36:6dd30780bd8e 2724 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
Artiom 44:badcbe8766e9 2725
Sitkah 36:6dd30780bd8e 2726 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2727 moyennageTelemetre += angleArriere;
Sitkah 36:6dd30780bd8e 2728 }
Sitkah 36:6dd30780bd8e 2729 }
Sitkah 36:6dd30780bd8e 2730 }
Artiom 44:badcbe8766e9 2731
Artiom 44:badcbe8766e9 2732 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
Artiom 44:badcbe8766e9 2733 if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2734 if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2735 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2736 angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
Artiom 44:badcbe8766e9 2737 else
Artiom 44:badcbe8766e9 2738 angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2739
Artiom 44:badcbe8766e9 2740 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Artiom 44:badcbe8766e9 2741 moyennageTelemetre += angleAvant;
Artiom 44:badcbe8766e9 2742 }
Artiom 44:badcbe8766e9 2743 }
Artiom 44:badcbe8766e9 2744 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
Artiom 44:badcbe8766e9 2745 if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2746 if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
Artiom 44:badcbe8766e9 2747 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2748 angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
Artiom 44:badcbe8766e9 2749 else
Artiom 44:badcbe8766e9 2750 angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2751
Sitkah 36:6dd30780bd8e 2752 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2753 moyennageTelemetre += angleAvant;
Sitkah 36:6dd30780bd8e 2754 }
Sitkah 36:6dd30780bd8e 2755 }
Sitkah 36:6dd30780bd8e 2756 }
Artiom 44:badcbe8766e9 2757
Sitkah 36:6dd30780bd8e 2758 angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2759
Artiom 44:badcbe8766e9 2760 if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
Artiom 44:badcbe8766e9 2761 if(orientationArrondie == 0) {
Sitkah 36:6dd30780bd8e 2762 angleRecalage -= 900;
Artiom 44:badcbe8766e9 2763
Sitkah 36:6dd30780bd8e 2764 /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2765 distanceRecalage = *);
Artiom 44:badcbe8766e9 2766 else
Artiom 44:badcbe8766e9 2767 distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
Artiom 44:badcbe8766e9 2768 } else if(orientationArrondie == 90) {
Artiom 44:badcbe8766e9 2769 angleRecalage += 0;
Artiom 44:badcbe8766e9 2770 } else if(orientationArrondie == 180) {
Artiom 44:badcbe8766e9 2771 angleRecalage += 900;
Artiom 44:badcbe8766e9 2772 } else if(orientationArrondie == 270) {
Sitkah 36:6dd30780bd8e 2773 angleRecalage += 1800;
Sitkah 36:6dd30780bd8e 2774 }
Sitkah 36:6dd30780bd8e 2775 }
Artiom 44:badcbe8766e9 2776
Sitkah 36:6dd30780bd8e 2777 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
Sitkah 36:6dd30780bd8e 2778 }
Sitkah 36:6dd30780bd8e 2779
Sitkah 36:6dd30780bd8e 2780 short recalageDistanceX(void)
Sitkah 36:6dd30780bd8e 2781 {
Sitkah 36:6dd30780bd8e 2782 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2783 unsigned int moyennageTelemetre = 0;
Artiom 78:c0533a36da8f 2784
Artiom 78:c0533a36da8f 2785 unsigned short tempo= telemetreDistance_arriere_gauche;
Artiom 77:641c7f1a827c 2786 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
Artiom 77:641c7f1a827c 2787 telemetreDistance_arriere_droite=tempo;
Artiom 44:badcbe8766e9 2788
Sitkah 36:6dd30780bd8e 2789 telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2790 telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2791 telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2792 telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
Artiom 44:badcbe8766e9 2793
Artiom 44:badcbe8766e9 2794 if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2795 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2796 moyennageTelemetre += telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2797 }
Artiom 44:badcbe8766e9 2798 if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2799 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2800 moyennageTelemetre += telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2801 }
Artiom 44:badcbe8766e9 2802 if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2803 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2804 moyennageTelemetre += telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2805 }
Artiom 44:badcbe8766e9 2806 if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2807 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2808 moyennageTelemetre += telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2809 }
Artiom 44:badcbe8766e9 2810
Sitkah 36:6dd30780bd8e 2811 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2812
Sitkah 36:6dd30780bd8e 2813 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
Sitkah 36:6dd30780bd8e 2814 }
Sitkah 36:6dd30780bd8e 2815
Sitkah 36:6dd30780bd8e 2816 short recalageDistanceY(void)
Sitkah 36:6dd30780bd8e 2817 {
Sitkah 36:6dd30780bd8e 2818 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2819 unsigned int moyennageTelemetre = 0;
Artiom 78:c0533a36da8f 2820
Artiom 78:c0533a36da8f 2821 unsigned short tempo= telemetreDistance_arriere_gauche;
Artiom 77:641c7f1a827c 2822 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
Artiom 77:641c7f1a827c 2823 telemetreDistance_arriere_droite=tempo;
Artiom 44:badcbe8766e9 2824
Sitkah 36:6dd30780bd8e 2825 telemetreDistance_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2826 telemetreDistance_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2827 telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2828 telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
Artiom 44:badcbe8766e9 2829
Artiom 44:badcbe8766e9 2830 if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2831 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2832 moyennageTelemetre += telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2833 }
Artiom 44:badcbe8766e9 2834 if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2835 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2836 moyennageTelemetre += telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2837 }
Artiom 44:badcbe8766e9 2838 if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2839 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2840 moyennageTelemetre += telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2841 }
Artiom 44:badcbe8766e9 2842 if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2843 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2844 moyennageTelemetre += telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2845 }
Artiom 44:badcbe8766e9 2846
Sitkah 36:6dd30780bd8e 2847 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2848
Artiom 44:badcbe8766e9 2849 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
Sitkah 36:6dd30780bd8e 2850 }
Sitkah 36:6dd30780bd8e 2851