carte_strategie_2019 - carte_strategie_2019

CRAC Team » Code » carte_strategie_2019

CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@90:2a3e2dca09a0, 2019-06-01 (annotated)

Committer:: kyxstark
Date:: Sat Jun 01 01:42:42 2019 +0000
Revision:: 90:2a3e2dca09a0
Parent:: 89:d8f8f4e12d5c
Child:: 91:42ae63e5daf5

gestion asserv erreur, a tester dans toutes les conditions;

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Strategie/Strategie.cpp@90:2a3e2dca09a0, 2019-06-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning