CRAC Team / Mbed 2 deprecated carte_strategie_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@80:545a9bf561e1, 2019-05-29 (annotated)

Committer:
Artiom
Date:
Wed May 29 16:08:16 2019 +0000
Revision:
80:545a9bf561e1
Parent:
79:d9c64cd4588c
Child:
81:ef50ec0ef328
angle balise GR corrige

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