carte_straegie_2019 - Programme carte strategie (disco)

CRAC Team » Code » carte_straegie_2019

CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@42:657b6a573e11, 2018-05-17 (annotated)

Committer:: Sitkah
Date:: Thu May 17 13:08:31 2018 +0000
Revision:: 42:657b6a573e11
Parent:: 41:b029ddc4d60e
Child:: 44:badcbe8766e9

useless

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Strategie/Strategie.cpp@42:657b6a573e11, 2018-05-17 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning