carte_strategie_2019

CRAC Team » Code » carte_strategie_2019

CRAC Team / Mbed 2 deprecated carte_strategie_2019

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@73:bf4d6d9db13b, 2019-05-27 (annotated)

Committer:: kyxstark
Date:: Mon May 27 10:40:19 2019 +0000
Revision:: 73:bf4d6d9db13b
Parent:: 72:5b1b3e151e59
Child:: 75:1db1b929f13d

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

Repository toolbox

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

Important changes to repositories hosted on mbed.com

Strategie/Strategie.cpp@73:bf4d6d9db13b, 2019-05-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

Strategie/Strategie.cpp@73:bf4d6d9db13b, 2019-05-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning