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@40:21bb685b553b, 2018-05-10 (annotated)

Committer:: Sitkah
Date:: Thu May 10 18:49:25 2018 +0000
Revision:: 40:21bb685b553b
Parent:: 38:76f886a1c8e6
Child:: 41:b029ddc4d60e

Avant match 3;

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

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@40:21bb685b553b, 2018-05-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning