carte_strategie_2019 - carte_strategie_2019

CRAC Team » Code » carte_strategie_2019

CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@41:b029ddc4d60e, 2018-05-11 (annotated)

Committer:: Sitkah
Date:: Fri May 11 12:09:26 2018 +0000
Revision:: 41:b029ddc4d60e
Parent:: 40:21bb685b553b
Child:: 42:657b6a573e11

ed

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	25 May 2019
Imports:	17
Forks:	0
Commits:	92
Dependents:	0
Dependencies:	6
Followers:	96

Strategie/Strategie.cpp@41:b029ddc4d60e, 2018-05-11 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning