code_carte_IHM_sans_bug - code de la carte IHM avant les bugs et avant le t…

CRAC Team » Code » code_carte_IHM_sans_bug

CRAC Team / Mbed 2 deprecated code_carte_IHM_sans_bug

code de la carte IHM avant les bugs et avant le travail effectué avec Melchior

Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Strategie/Strategie.cpp@5:81aac085516f, 2020-01-31 (annotated)

Committer:: gabrieltetar
Date:: Fri Jan 31 14:37:29 2020 +0000
Revision:: 5:81aac085516f
Parent:: 1:7e925468f9d9
Child:: 7:44eec996a76e

Suppression de la discrimination entre gros et petit robot

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	17 Jul 2021
Imports:	2
Forks:	0
Commits:	39
Dependents:	0
Dependencies:	6
Followers:	96

Strategie/Strategie.cpp@5:81aac085516f, 2020-01-31 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning