CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 44:badcbe8766e9
- Parent:
- 42:657b6a573e11
- Child:
- 45:4f93e99bac6e
--- a/Strategie/Strategie.cpp Thu May 17 13:08:31 2018 +0000
+++ b/Strategie/Strategie.cpp Tue May 07 17:37:46 2019 +0000
@@ -1,6 +1,6 @@
- #include "global.h"
+#include "global.h"
#include <string.h>
-#include <sstream>
+#include <sstream>
//#include "StrategieManager.h"
@@ -13,11 +13,10 @@
#define BLANC 0xFF000000
#define ORANGE 0xFFFFA500
#define NOIR 0xFF000000
-#define DIY_GREY 0xFFDFDFDF
+#define DIY_GREY 0xFFDFDFDF
char tableau_aff[10][50];
-char tableau_etat[22][50]=
-{
+char tableau_etat[22][50]= {
"Check_carte_screen",
"Check_carte_screen_wait_ack",
"Check_cartes",
@@ -59,7 +58,7 @@
Timer debugetatTimer;
Timer timeoutWarning;
Timer timeoutWarningWaitEnd;
-Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
+Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
unsigned char screenChecktry = 0;
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};
@@ -85,9 +84,15 @@
unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
-unsigned char countAliveCard = 0;
+unsigned char countAliveCard = 0;
unsigned char ligne=0;
+
+int Fevitement=0;
+int EvitEtat= 0;
+int stop_evitement=0;
+
+
float angle_moyen_balise_IR = 0.0;
@@ -97,15 +102,17 @@
unsigned char countRobotNear = 0;//Le nombre de robot à proximité
-unsigned char ingnorBaliseOnce = 0;
+unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
+unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
+unsigned char robot_arrete = 0;
unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
struct S_Instruction instruction;
char couleur1, couleur2, couleur3;
-float cptf;
-int cpt,cpt1;
+float cptf;
+int cpt,cpt1;
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;
T_etat etat = INIT;
@@ -114,41 +121,41 @@
E_Stratposdebut etat_pos=RECALAGE_1;
/////////////////DEFINITION DES BOUTONS////////////////////
- Button COTE_VERT(0, 25, 400, 300, "VERT");
- Button COTE_ORANGE(0, 350, 400, 300, "ORANGE");
- Button RETOUR (0, 680, 400, 110, "--Precedent--");
- Button LANCER (0, 200, 400, 200, "--LANCER--");
- Button CHECK (0, 420, 400, 200, "Valider");
- Button MATCH (0, 50, 400, 320, "Match");
- Button DEMONSTRATION (0, 400, 400, 320, "Demo");
- Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
- Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
- Button TEST_COULEURS(0,245,400,100,"Test capteurs");
- Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
- Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
- Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
- Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
- Button EPURATION(0, 150, 400, 100, "epuration");
- Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
- Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
- Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
- Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
- Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
- Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
- Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
- Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
- Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
- Button TRI(0, 25, 400, 100, "Test tri");
- Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
- Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
- Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
- Button SUIVANT(0,380,200,100,"Suivant");
- Button COLOR_ORANGE (0, 230, 190, 110,"");
- Button COLOR_JAUNE (210, 230, 190, 110,"");
- Button COLOR_VERT (0, 350, 190, 110,"");
- Button COLOR_BLEU (210, 350, 190, 110,"");
- Button COLOR_NOIR (105, 470, 190, 110,"");
- ////////////////////////////////////////////////////////////
+Button COTE_VERT(0, 25, 400, 300, "VERT");
+Button COTE_ORANGE(0, 350, 400, 300, "ORANGE");
+Button RETOUR (0, 680, 400, 110, "--Precedent--");
+Button LANCER (0, 200, 400, 200, "--LANCER--");
+Button CHECK (0, 420, 400, 200, "Valider");
+Button MATCH (0, 50, 400, 320, "Match");
+Button DEMONSTRATION (0, 400, 400, 320, "Demo");
+Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
+Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
+Button TEST_COULEURS(0,245,400,100,"Test capteurs");
+Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
+Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
+Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
+Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
+Button EPURATION(0, 150, 400, 100, "epuration");
+Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
+Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
+Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
+Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
+Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
+Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
+Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
+Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
+Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
+Button TRI(0, 25, 400, 100, "Test tri");
+Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
+Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
+Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
+Button SUIVANT(0,380,200,100,"Suivant");
+Button COLOR_ORANGE (0, 230, 190, 110,"");
+Button COLOR_JAUNE (210, 230, 190, 110,"");
+Button COLOR_VERT (0, 350, 190, 110,"");
+Button COLOR_BLEU (210, 350, 190, 110,"");
+Button COLOR_NOIR (105, 470, 190, 110,"");
+////////////////////////////////////////////////////////////
void SendRawId (unsigned short id);
void SelectionStrat (unsigned char numeroStrat);
@@ -184,8 +191,8 @@
#endif
-
-
+
+
/****************************************************************************************/
@@ -198,7 +205,7 @@
SendRawId(GLOBAL_GAME_END);//Indication fin de match
etat=FIN;
gameTimer.stop();//Arret du timer
-
+
while(1);//On bloque la programme dans l'interruption
}
@@ -227,59 +234,58 @@
{
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
-
- switch (Strategie+1)
- {
+
+ switch (Strategie+1) {
case 0x1 :
//description de Strategie n°1
lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x2 :
//description de Strategie n°2
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x3 :
//description de Strategie n°3
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x4 :
//description de Strategie n°4
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x5 :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x6 :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x7 :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x8 :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0x9 :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
-
+
case 0xA :
//description de Strategie n°5
lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
break;
- }
+ }
}
void Setflag(void)
@@ -288,64 +294,65 @@
}
-//Affiche une variable sur l'écran tactile//
-void affichage_var(double Var){
+//Affiche une variable sur l'écran tactile//
+void affichage_var(double Var)
+{
if(ligne==7)
ligne=0;
char aff[10]="toto";
sprintf(aff,"%lf ",Var);
lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE);
//ligne++;
-
+
}
/****************************************************************************************/
/* FUNCTION NAME: affichage_debug */
/* DESCRIPTION : Affiche l'état de gameEtat sur l'écran lcd */
-/****************************************************************************************/
-void affichage_debug(int Var){
+/****************************************************************************************/
+void affichage_debug(int Var)
+{
int i;
int conv=(int)Var;
SUIVANT.Draw(ROUGE, 0);
- for(i=0;i<9;i++){
+ for(i=0; i<9; i++) {
strcpy(tableau_aff[i],"");
strcpy(tableau_aff[i],tableau_aff[i+1]);
}
strcpy(tableau_aff[9],tableau_etat[conv]);
- for(i=0;i<10;i++){
+ for(i=0; i<10; i++) {
lcd.SetBackColor(VERT);
lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
- }
+ }
/*while(!ack_bluetooth){ // mode pas à pas en bluetooth ou via écran
//liaison_bluetooth();
}
ack_bluetooth=0;*/
/*while(SUIVANT.Touched()==0);
while(SUIVANT.Touched());*/
-}
+}
/****************************************************************************************/
/* FUNCTION NAME: automate_etat_ihm */
/* DESCRIPTION : Automate de gestion de l'affichage */
-/****************************************************************************************/
+/****************************************************************************************/
void automate_etat_ihm(void)
{
int j;
- if (j==0){
+ if (j==0) {
ts.Init(lcd.GetXSize(), lcd.GetYSize());
j++;
}
- ts.GetState(&TS_State);
- switch (etat)
- {
+ ts.GetState(&TS_State);
+ switch (etat) {
case INIT : //intialise l'écran et passe à l'attente d'initialisation des cartes
- ts.GetState(&TS_State);
+ ts.GetState(&TS_State);
canProcessRx();
-
-
-
-
+
+
+
+
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
@@ -355,32 +362,31 @@
lcd.SetTextColor(DIY_GREY);
lcd.FillRect(0,400,400,150); //carte moteur
lcd.FillRect(0,600,400,150); //Balise
-
+
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.SetBackColor(DIY_GREY);
lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
- lcd.DisplayStringAt(110,650 , (uint8_t *)"Balise", LEFT_MODE);
+ lcd.DisplayStringAt(110,650, (uint8_t *)"Balise", LEFT_MODE);
////////////////////////////////////////
-
- FORCE_LAUNCH.Draw(0xFFFF0000, 0);
-
+
+ FORCE_LAUNCH.Draw(0xFFFF0000, 0);
+
etat=ATT;
break;
-
+
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...)
- if (flag==1){
+ if (flag==1) {
etat = CHOIX;
gameEtat = ETAT_CONFIG;
- }
- else if (FORCE_LAUNCH.Touched()){
+ } else if (FORCE_LAUNCH.Touched()) {
etat = CHOIX;
gameEtat = ETAT_CONFIG;
while(FORCE_LAUNCH.Touched());
}
-
+
break;
-
-
+
+
case CHOIX : //Match ou DEMO
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
@@ -388,25 +394,22 @@
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE);
DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
MATCH.Draw(0xFFF01010, 0);
- while(etat == CHOIX)
- {
+ while(etat == CHOIX) {
canProcessRx();
- if(DEMONSTRATION.Touched())
- {
+ if(DEMONSTRATION.Touched()) {
etat = DEMO;
while(DEMONSTRATION.Touched());
}
-
- if(MATCH.Touched())
- {
+
+ if(MATCH.Touched()) {
etat = SELECT_SIDE;
while(MATCH.Touched());
}
}
break;
-
- case DEMO :
+
+ case DEMO :
lcd.Clear(LCD_COLOR_WHITE);
RETOUR.Draw(0xFFFF0000, 0);
TEST_HERKULEX.Draw(VERT, 0);
@@ -416,13 +419,11 @@
TEST_IMMEUBLE.Draw(VERT,0);
TEST_TRIEUR.Draw(VERT,0);
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config//
- InversStrat = 0;//Pas d'inversion de la couleur
+ InversStrat = 0;//Pas d'inversion de la couleur
}
- while (etat == DEMO) ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
- {
+ while (etat == DEMO) { ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
canProcessRx();
- if(TEST_HERKULEX.Touched())
- {
+ if(TEST_HERKULEX.Touched()) {
//Strat = 0x10;
while(TEST_HERKULEX.Touched());
CANMessage trame_Tx = CANMessage();
@@ -434,27 +435,24 @@
can2.write(trame_Tx);
TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
etat = TEST_SERVO;
- lcd.Clear(LCD_COLOR_WHITE);
+ lcd.Clear(LCD_COLOR_WHITE);
ModeDemo=1;
- }
- else if(TEST_LASER.Touched())
- {
- //Strat = 0x11;
- while(TEST_LASER.Touched());
+ } else if(TEST_LASER.Touched()) {
+ //Strat = 0x11;
+ while(TEST_LASER.Touched());
TEST_LASER.Draw(0xFFF0F0F0, 0);
etat = TEST_TELEMETRE;
- }
- else if (TEST_COULEURS.Touched()){
+ } else if (TEST_COULEURS.Touched()) {
while(TEST_COULEURS.Touched());
- TEST_LASER.Draw(0xFFF0F0F0, 0);
- etat =TEST_CAPTEURS ;
+ TEST_LASER.Draw(0xFFF0F0F0, 0);
+ etat =TEST_CAPTEURS ;
}
-
- else if (TEST_TIR_BALLE.Touched()){
+
+ else if (TEST_TIR_BALLE.Touched()) {
while(TEST_TIR_BALLE.Touched());
- TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
- etat =TEST_TIR ;
- lcd.Clear(LCD_COLOR_WHITE);
+ TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
+ etat =TEST_TIR ;
+ lcd.Clear(LCD_COLOR_WHITE);
CANMessage trame_Tx = CANMessage();
trame_Tx.len = 1;
trame_Tx.format = CANStandard;
@@ -462,24 +460,21 @@
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=0x2;
can2.write(trame_Tx);
- ModeDemo=1;
- }
- else if(TEST_IMMEUBLE.Touched()){
- while(TEST_IMMEUBLE.Touched());
- TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
- etat =DEMO_IMMEUBLE;
- lcd.Clear(LCD_COLOR_WHITE);
- }
- else if(TEST_TRIEUR.Touched()){
+ ModeDemo=1;
+ } else if(TEST_IMMEUBLE.Touched()) {
+ while(TEST_IMMEUBLE.Touched());
+ TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
+ etat =DEMO_IMMEUBLE;
+ lcd.Clear(LCD_COLOR_WHITE);
+ } else if(TEST_TRIEUR.Touched()) {
while(TEST_TRIEUR.Touched());
etat=DEMO_TRIEUR;
lcd.Clear(LCD_COLOR_WHITE);
- }
- if(RETOUR.Touched())
- {
+ }
+ if(RETOUR.Touched()) {
etat = CHOIX;
while(RETOUR.Touched());
-
+
}
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
Ack_strat = 1;
@@ -487,21 +482,20 @@
}
}
break;
- ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
- case DEMO_TRIEUR:
+ ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
+ case DEMO_TRIEUR:
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
TRI.Draw(VERT, 0);
AIGUILLEUR_D.Draw(VERT, 0);
AIGUILLEUR_G.Draw(VERT, 0);
AIGUILLEUR_CTRE.Draw(VERT, 0);
- while(etat==DEMO_TRIEUR){
- if(RETOUR.Touched()){
+ while(etat==DEMO_TRIEUR) {
+ if(RETOUR.Touched()) {
while (RETOUR.Touched());
etat=DEMO;
- }
- else if(TRI.Touched()){
- while (TRI.Touched());
+ } else if(TRI.Touched()) {
+ while (TRI.Touched());
SendRawId(AIGUILLEUR_CENTRE);
wait(0.5);
SendRawId(AIGUILLEUR_DROITE);
@@ -509,86 +503,78 @@
SendRawId(AIGUILLEUR_GAUCHE);
wait(0.5);
SendRawId(AIGUILLEUR_CENTRE);
-
+
break;
- }
- else if(AIGUILLEUR_D.Touched()){
- while (AIGUILLEUR_D.Touched());
+ } else if(AIGUILLEUR_D.Touched()) {
+ while (AIGUILLEUR_D.Touched());
SendRawId(AIGUILLEUR_DROITE);
break;
- }
- else if(AIGUILLEUR_G.Touched()){
+ } else if(AIGUILLEUR_G.Touched()) {
while (AIGUILLEUR_G.Touched());
- SendRawId(AIGUILLEUR_GAUCHE);
+ SendRawId(AIGUILLEUR_GAUCHE);
break;
-
- }
- else if(BRAS_ABEILLE_OFF.Touched()){
- while (BRAS_ABEILLE_OFF.Touched());
+
+ } else if(BRAS_ABEILLE_OFF.Touched()) {
+ while (BRAS_ABEILLE_OFF.Touched());
SendRawId(BRAS_ABEILLE_DOWN);
- break;
- }
- else if(AIGUILLEUR_CTRE.Touched()){
- while (AIGUILLEUR_CTRE.Touched());
- SendRawId(AIGUILLEUR_CENTRE);
- break;
- }
-
+ break;
+ } else if(AIGUILLEUR_CTRE.Touched()) {
+ while (AIGUILLEUR_CTRE.Touched());
+ SendRawId(AIGUILLEUR_CENTRE);
+ break;
+ }
+
}
break;
case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
int color=0;
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
-
+
CANMessage msgTx=CANMessage();
msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
msgTx.len=3;
msgTx.format=CANStandard;
msgTx.type=CANData;
-
-
- while(etat==DEMO_IMMEUBLE){
- switch(color){
+
+
+ while(etat==DEMO_IMMEUBLE) {
+ switch(color) {
case 0:
-
+
RETOUR.Draw(ROUGE,0);
COLOR_NOIR.Draw(NOIR,1);
COLOR_ORANGE.Draw(ORANGE,0);
COLOR_JAUNE.Draw(JAUNE,0);
COLOR_VERT.Draw(VERT,0);
COLOR_BLEU.Draw(BLEU,0);
-
+
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(NOIR);
lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
- while(color==0){
- if(COLOR_ORANGE.Touched()){
+ while(color==0) {
+ if(COLOR_ORANGE.Touched()) {
while(COLOR_ORANGE.Touched());
COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
+ msgTx.data[color]=1;
color++;
- }
- else if (COLOR_NOIR.Touched()){
+ } else if (COLOR_NOIR.Touched()) {
while(COLOR_NOIR.Touched());
COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
+ msgTx.data[color]=2;
color++;
- }
- else if (COLOR_VERT.Touched()){
+ } else if (COLOR_VERT.Touched()) {
while(COLOR_VERT.Touched());
COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
+
msgTx.data[color]=3;
color++;
- }
- else if (COLOR_JAUNE.Touched()){
+ } else if (COLOR_JAUNE.Touched()) {
while(COLOR_JAUNE.Touched());
COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
msgTx.data[color]=4;
color++;
- }
- else if (COLOR_BLEU.Touched()){
+ } else if (COLOR_BLEU.Touched()) {
while(COLOR_BLEU.Touched());
COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
msgTx.data[color]=5;
@@ -596,109 +582,101 @@
}
}
break;
-
+
case 1:
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(NOIR);
lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
- if(COLOR_ORANGE.Touched()){
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- }
- else if (COLOR_NOIR.Touched()){
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- }
- else if (COLOR_VERT.Touched()){
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- }
- else if (COLOR_JAUNE.Touched()){
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- }
- else if (COLOR_BLEU.Touched()){
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
+ if(COLOR_ORANGE.Touched()) {
+ while(COLOR_ORANGE.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=1;
+ color++;
+ } else if (COLOR_NOIR.Touched()) {
+ while(COLOR_NOIR.Touched());
+ COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=2;
+ color++;
+ } else if (COLOR_VERT.Touched()) {
+ while(COLOR_VERT.Touched());
+ COLOR_VERT.Draw(LCD_COLOR_WHITE);
+
+ msgTx.data[color]=3;
+ color++;
+ } else if (COLOR_JAUNE.Touched()) {
+ while(COLOR_JAUNE.Touched());
+ COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=4;
+ color++;
+ } else if (COLOR_BLEU.Touched()) {
+ while(COLOR_BLEU.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=5;
+ color++;
+ }
break;
-
+
case 2:
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(NOIR);
lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
- if(COLOR_ORANGE.Touched()){
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- }
- else if (COLOR_NOIR.Touched()){
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- }
- else if (COLOR_VERT.Touched()){
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- }
- else if (COLOR_JAUNE.Touched()){
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- }
- else if (COLOR_BLEU.Touched()){
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
+ if(COLOR_ORANGE.Touched()) {
+ while(COLOR_ORANGE.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=1;
+ color++;
+ } else if (COLOR_NOIR.Touched()) {
+ while(COLOR_NOIR.Touched());
+ COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=2;
+ color++;
+ } else if (COLOR_VERT.Touched()) {
+ while(COLOR_VERT.Touched());
+ COLOR_VERT.Draw(LCD_COLOR_WHITE);
+
+ msgTx.data[color]=3;
+ color++;
+ } else if (COLOR_JAUNE.Touched()) {
+ while(COLOR_JAUNE.Touched());
+ COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=4;
+ color++;
+ } else if (COLOR_BLEU.Touched()) {
+ while(COLOR_BLEU.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=5;
+ color++;
+ }
break;
case 3:
lcd.Clear(LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(NOIR);
-
+
lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
RETOUR.Draw(ROUGE,0);
can2.write(msgTx);
color++;
break;
-
+
case 4:
- if(RETOUR.Touched()){
+ if(RETOUR.Touched()) {
while(RETOUR.Touched());
- etat=DEMO;
+ etat=DEMO;
}
break;
}
- if(RETOUR.Touched()){
+ if(RETOUR.Touched()) {
while(RETOUR.Touched());
etat=DEMO;
}
}
break;
-
-
-
-
-
+
+
+
+
+
case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
@@ -709,45 +687,39 @@
INTERRUPTEUR_ON.Draw(VERT, 0);
INTERRUPTEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(0xFFFF0000,0);
- while(etat==TEST_SERVO){
- if(RETOUR.Touched()){
+ while(etat==TEST_SERVO) {
+ if(RETOUR.Touched()) {
while (RETOUR.Touched());
etat=DEMO;
- }
- else if(ABAISSE_BLOC.Touched()){
- while (ABAISSE_BLOC.Touched());
+ } else if(ABAISSE_BLOC.Touched()) {
+ while (ABAISSE_BLOC.Touched());
SendRawId(BAISSER_ATTRAPE_BLOC);
break;
- }
- else if(RELEVE_BLOC.Touched()){
- while (RELEVE_BLOC.Touched());
+ } else if(RELEVE_BLOC.Touched()) {
+ while (RELEVE_BLOC.Touched());
SendRawId(RELEVER_ATTRAPE_BLOC);
break;
- }
- else if(BRAS_ABEILLE_ON.Touched()){
+ } else if(BRAS_ABEILLE_ON.Touched()) {
while (BRAS_ABEILLE_ON.Touched());
- SendRawId(BRAS_ABEILLE_UP);
+ SendRawId(BRAS_ABEILLE_UP);
break;
-
- }
- else if(BRAS_ABEILLE_OFF.Touched()){
- while (BRAS_ABEILLE_OFF.Touched());
+
+ } else if(BRAS_ABEILLE_OFF.Touched()) {
+ while (BRAS_ABEILLE_OFF.Touched());
SendRawId(BRAS_ABEILLE_DOWN);
- break;
- }
- else if(INTERRUPTEUR_ON.Touched()){
- while (INTERRUPTEUR_ON.Touched());
- SendRawId(ALLUMER_PANNEAU_UP);
- break;
- }
- else if(INTERRUPTEUR_OFF.Touched()){
- while (INTERRUPTEUR_OFF.Touched());
- SendRawId(ALLUMER_PANNEAU_DOWN);
+ break;
+ } else if(INTERRUPTEUR_ON.Touched()) {
+ while (INTERRUPTEUR_ON.Touched());
+ SendRawId(ALLUMER_PANNEAU_UP);
+ break;
+ } else if(INTERRUPTEUR_OFF.Touched()) {
+ while (INTERRUPTEUR_OFF.Touched());
+ SendRawId(ALLUMER_PANNEAU_DOWN);
break;
}
- }
+ }
break;
-
+
case TEST_TIR: // TEST DES FONCTIONS LIEES AUX TIRS
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
@@ -756,79 +728,73 @@
LANCEUR_ON.Draw(VERT, 0);
LANCEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(ROUGE, 0);
- while(etat==TEST_TIR){
- if(TIR_CHATEAU.Touched()){
+ while(etat==TEST_TIR) {
+ if(TIR_CHATEAU.Touched()) {
while (TIR_CHATEAU.Touched());
SendRawId(INCLINAISON_CHATEAU);
break;
- }
- else if (EPURATION.Touched()){
+ } else if (EPURATION.Touched()) {
while (EPURATION.Touched());
SendRawId(INCLINAISON_EPURATION);
break;
- }
- else if(LANCEUR_ON.Touched()){
+ } else if(LANCEUR_ON.Touched()) {
while (LANCEUR_ON.Touched());
CANMessage msgTx=CANMessage();
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
-
+
msgTx.len=1;
msgTx.data[0]=0;
can2.write(msgTx);
break;
- }
- else if(LANCEUR_OFF.Touched()){
+ } else if(LANCEUR_OFF.Touched()) {
while (LANCEUR_OFF.Touched());
SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
break;
- }
- else if (RETOUR.Touched()){
+ } else if (RETOUR.Touched()) {
while (RETOUR.Touched());
etat=DEMO;
-
+
}
}
- break;
-
-
-
+ break;
+
+
+
case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
ModeDemo=1;
lcd.Clear(LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
RETOUR.Draw(0xFFFF0000, 0);
- while(etat==TEST_TELEMETRE){
+ while(etat==TEST_TELEMETRE) {
SendRawId(DATA_RECALAGE);
wait(0.1);
canProcessRx();
- if(RETOUR.Touched()){
- while( RETOUR.Touched());
- etat=DEMO;
- lcd.Clear(LCD_COLOR_WHITE);
+ if(RETOUR.Touched()) {
+ while( RETOUR.Touched());
+ etat=DEMO;
+ lcd.Clear(LCD_COLOR_WHITE);
}
}
break; ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////
-
-
+
+
case SELECT_SIDE : // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS
lcd.Clear(LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
-
+
lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
COTE_VERT.Draw(VERT, 0);
COTE_ORANGE.Draw(ORANGE, 0);
RETOUR.Draw(LCD_COLOR_RED, 0);
-
-
- while (etat == SELECT_SIDE)
- {
+
+
+ while (etat == SELECT_SIDE) {
canProcessRx();
- if(COTE_VERT.Touched())
- {
+ if(COTE_VERT.Touched()) {
Cote = 0x0;
InversStrat = Cote;
etat = TACTIQUE;
@@ -840,11 +806,10 @@
trame_Tx.data[0]=Cote;
can2.write(trame_Tx);
while(COTE_VERT.Touched());
-
+
}
-
- if(COTE_ORANGE.Touched())
- {
+
+ if(COTE_ORANGE.Touched()) {
Cote = 0x1;
InversStrat= Cote;
etat = TACTIQUE;
@@ -857,120 +822,109 @@
can2.write(trame_Tx);
while(COTE_ORANGE.Touched());
}
-
- if(RETOUR.Touched())
- {
+
+ if(RETOUR.Touched()) {
etat = CHOIX;
while(RETOUR.Touched());
}
}
-
+
break;
-
+
case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE
- if (Cote == 0){
+ if (Cote == 0) {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
- }
- else if (Cote == 1){
+ } else if (Cote == 1) {
lcd.Clear(ORANGE);
lcd.SetBackColor(ORANGE);
- }
- else {
+ } else {
lcd.Clear(BLEU);
lcd.SetBackColor(BLEU);
- }
-
- lcd.SetTextColor(LCD_COLOR_BLACK);
-
+ }
+
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+
lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE);
-
+
Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
- if (Strategie == -1)
- {
+ if (Strategie == -1) {
etat = SELECT_SIDE;
+ } else {
+ etat = DETAILS;
}
- else
- {
- etat = DETAILS;
- }
wait(0.1);
break;
-
+
case DETAILS : //SECONDE VALIDATION DE LA STRAT
lcd.Clear(LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
CHECK.Draw(VERT);
RETOUR.Draw(LCD_COLOR_RED);
-
+
SelectionStrat(Strategie); //affiche la stratégie selectionnée
-
- while (etat == DETAILS)
- {
+
+ while (etat == DETAILS) {
canProcessRx();
- if (CHECK.Touched())
- {
- if(gameEtat == ETAT_CONFIG) {
- gameEtat = ETAT_GAME_INIT;
- etat=LECTURE;
-
- }
- while(CHECK.Touched());
+ if (CHECK.Touched()) {
+ if(gameEtat == ETAT_CONFIG) {
+ gameEtat = ETAT_GAME_INIT;
+ etat=LECTURE;
+
}
-
- if(RETOUR.Touched())
- {
- etat = TACTIQUE;
- while(RETOUR.Touched());
- }
- }
+ while(CHECK.Touched());
+ }
+
+ if(RETOUR.Touched()) {
+ etat = TACTIQUE;
+ while(RETOUR.Touched());
+ }
+ }
break;
-
-
+
+
case LECTURE :
- break;
+ break;
case AFF_WAIT_JACK : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK
lcd.Clear(BLANC);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
-
- if (Cote == 0){
+
+ if (Cote == 0) {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
- }
- else if (Cote == 1){
+ } else if (Cote == 1) {
lcd.Clear(ORANGE);
lcd.SetBackColor(ORANGE);
- }
- else {
+ } else {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
}
canProcessRx();
- lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
etat=WAIT_JACK;
break;
-
+
case WAIT_JACK: //VERITABLE ATTENTE DU JACK
- break;
-
+ break;
+
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
cptf=gameTimer.read();
lcd.SetTextColor(LCD_COLOR_BLACK);
cpt=(int)cptf;
- if(cpt != cpt1){
+ if(cpt != cpt1) {
lcd.Clear(VERT);
- // affichage_compteur(100-cpt);
+ // affichage_compteur(100-cpt);
//affichage_compteur(SCORE_PR);
- #ifdef ROBOT_BIG
- affichage_var(SCORE_GR);
- #else
- affichage_var(SCORE_PR);
- #endif
- if(liaison_pr.paquet_en_attente()){
+#ifdef ROBOT_BIG
+ affichage_var(SCORE_GR);
+#else
+ affichage_var(SCORE_PR);
+#endif
+ if(liaison_pr.paquet_en_attente()) {
PaquetDomotique *paquet=liaison_pr.lire();
- if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE){
+ if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) {
SCORE_PR+=convertir_score(paquet);
}
delete paquet;
@@ -981,24 +935,24 @@
//affichage_debug(gameEtat);
lcd.SetBackColor(LCD_COLOR_WHITE);
-
+
break;
-
+
case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL
lcd.Clear (LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
- #ifdef ROBOT_BIG
- // affichage_compteur(SCORE_GR);
- affichage_var(SCORE_GR);
- //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
- #else
- //affichage_compteur(SCORE_PR);
- affichage_var(SCORE_PR);
- #endif
- while(1); // force le redemarage du robot
+#ifdef ROBOT_BIG
+ // affichage_compteur(SCORE_GR);
+ affichage_var(SCORE_GR);
+ //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
+#else
+ //affichage_compteur(SCORE_PR);
+ affichage_var(SCORE_PR);
+#endif
+ while(1); // force le redemarage du robot
//break;
-
- }
+
+ }
}
@@ -1007,7 +961,8 @@
/* FUNCTION NAME: automate_process */
/* DESCRIPTION : Automate de gestion de la stratégie du robot */
/****************************************************************************************/
-void automate_process(void){
+void automate_process(void)
+{
static unsigned char AX12_enchainement = 0;
static unsigned char MV_enchainement = 0;
signed char localData1 = 0;
@@ -1015,36 +970,36 @@
unsigned short localData3 = 0;
//signed short localData4 = 0;
unsigned char localData5 = 0;
-
+
+
if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
gameTimer.stop();
gameTimer.reset();
gameEtat = ETAT_END;//Fin du temps
etat=FIN;
}
-
+
if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
lastEtat = gameEtat;
debugetatTimer.reset();
sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
}
-
- switch(gameEtat)
- {
-
- case ETAT_CHECK_CARTES:
+
+ switch(gameEtat) {
+
+ case ETAT_CHECK_CARTES:
/*
Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
*/
waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
-
+
screenChecktry++;//On incrèment le conteur de tentative de 1
cartesCheker.reset();//On reset le timeOut
cartesCheker.start();//On lance le timer pour le timeout
gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
break;
-
+
case ETAT_CHECK_CARTES_WAIT_ACK:
/*
On attend l'ack de la carte en cours de vérification
@@ -1061,41 +1016,35 @@
gameEtat = ETAT_CONFIG;
SendRawId(ECRAN_ALL_CHECK);
flag=1;
-
+
//tactile_printf("Selection couleur et strategie");
- }
- else {
+ } else {
gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
waitingAckFrom = ECRAN_ALL_CHECK;
}
- }
- else
+ } else
gameEtat = ETAT_CHECK_CARTES;
- }
- else if(cartesCheker.read_ms () > 100) {
+ } else if(cartesCheker.read_ms () > 100) {
cartesCheker.stop();
if(screenChecktry >=3) {
//printf("missing card %d\n",id_check[checkCurrent]);
screenChecktry = 0;
checkCurrent++;
-
- if(checkCurrent >= NOMBRE_CARTES){
- if(countAliveCard == NOMBRE_CARTES){
+
+ if(checkCurrent >= NOMBRE_CARTES) {
+ if(countAliveCard == NOMBRE_CARTES) {
gameEtat = ETAT_CONFIG;
- flag=1;
- }
- else{
+ flag=1;
+ } else {
gameEtat = ETAT_WAIT_FORCE;
waitingAckFrom = ECRAN_ALL_CHECK;
}
- }
- else
+ } else
gameEtat = ETAT_CHECK_CARTES;
-
- }
- else
+
+ } else
gameEtat = ETAT_CHECK_CARTES;
-
+
}
break;
case ETAT_WAIT_FORCE:
@@ -1116,26 +1065,23 @@
break;
case ETAT_GAME_INIT:
//On charge la liste des instructions
-
+
loadAllInstruction(Strategie);//Mise en cache de toute les instructions
led3=1;
-
+
SendRawId(GLOBAL_START);
-
+
gameEtat = ETAT_GAME_WAIT_FOR_JACK;
- if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE)
- {
+ if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) {
SendRawId(DEBUG_FAKE_JAKE);
+ } else {
+ etat = AFF_WAIT_JACK;
}
- else
- {
- etat = AFF_WAIT_JACK;
- }
//tactile_printf("Attente du JACK.");
setAsservissementEtat(1);//On réactive l'asservissement
jack.mode(PullDown); // désactivation de la résistance interne du jack
jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
-
+
localData2 = POSITION_DEBUT_T;
localData3 = POSITION_DEBUT_Y;
if(InversStrat == 1) {
@@ -1143,108 +1089,107 @@
localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
}
SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2);
-
+
instruction = strat_instructions[actual_instruction];
- //On effectue le traitement de l'instruction
-
- break;
+ //On effectue le traitement de l'instruction
+
+ break;
case ETAT_GAME_WAIT_FOR_JACK:
- if(instruction.order==POSITION_DEBUT){
- 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 !!!)
+ if(instruction.order==POSITION_DEBUT) {
+ 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 !!!)
case RECALAGE_1:
waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- #ifdef ROBOT_SMALL
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+#ifdef ROBOT_SMALL
GoStraight(3000, 1,MOITIEE_ROBOT-5, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
- #else
+#else
GoStraight(-3000, 1,MOITIEE_ROBOT-5, 0);
- #endif
+#endif
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
- waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=RECULER_1;
break;
-
+
case RECULER_1:
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- #ifdef ROBOT_SMALL
- GoStraight(-450, 0, 0, 0);
- #else
+#ifdef ROBOT_SMALL
+ GoStraight(-40, 0, 0, 0);//-450
+#else
GoStraight(150, 0, 0, 0);
- #endif
+#endif
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=TOURNER;
- break;
-
+ break;
+
case TOURNER:
waitingAckID = ASSERVISSEMENT_ROTATION;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- if(Cote==0){
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ if(Cote==0) {
localData2 = 900;
- }
- else{
+ } else {
localData2=-900;
}
Rotate(localData2);
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=RECALAGE_2;
- break;
-
+ break;
+
case RECALAGE_2:
waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(Cote==1)
- localData3=3000-(MOITIEE_ROBOT-5);
+ localData3=3000-(MOITIEE_ROBOT-5);
else
localData3=MOITIEE_ROBOT;
- #ifdef ROBOT_SMALL
+#ifdef ROBOT_SMALL
GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
- #else
+#else
GoStraight(-3000, 2,localData3, 0);
- #endif
+#endif
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=RECULER_2;
- break;
-
- case RECULER_2:
+ break;
+
+ case RECULER_2:
waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- #ifdef ROBOT_SMALL
- GoStraight(-200, 0, 0, 0);
- #else
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+#ifdef ROBOT_SMALL
+ GoStraight(-40, 0, 0, 0);
+#else
GoStraight(200, 0, 0, 0);
- #endif
+#endif
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=GOTOPOS;
break;
-
+
case GOTOPOS:
localData1 = -1;
-
+
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -instruction.arg3;
localData3 = 3000 - instruction.arg2;//Inversion du Y
@@ -1252,58 +1197,58 @@
localData3 = instruction.arg2;
localData2 = instruction.arg3;
}
-
+
GoToPosition(instruction.arg1,localData3,localData2,localData1);
waitingAckID = ASSERVISSEMENT_XYT;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
+
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_XYT;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=FIN_POS;
- break;
+ break;
case FIN_POS:
actual_instruction = instruction.nextLineOK;
- break;
+ break;
}
}
-
-
- break;
+
+
+ break;
case ETAT_GAME_START:
-
+
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
-
- if (ModeDemo == 0){
+
+ if (ModeDemo == 0) {
chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
gameTimer.start();
- }
+ }
gameTimer.reset();
jack.fall(NULL);//On désactive l'interruption du jack
- //SendRawId(GLOBAL_START);
+ //SendRawId(GLOBAL_START);
Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
//tactile_printf("Start");//Pas vraiment utile mais bon
- break;
+ break;
case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
/*
Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
*/
//printf("load next instruction\n");
-
+
if(actual_instruction >= nb_instructions || actual_instruction == 255) {
gameEtat = ETAT_END;
//Il n'y a plus d'instruction, fin du jeu
} else {
instruction = strat_instructions[actual_instruction];
- //On effectue le traitement de l'instruction
+ //On effectue le traitement de l'instruction
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
}
screenChecktry = 0;
ingnorInversionOnce = 0;
- break;
+ break;
case ETAT_GAME_PROCESS_INSTRUCTION:
/*
Traitement de l'instruction, envoie de la trame CAN
@@ -1311,13 +1256,12 @@
//debug_Instruction(instruction);
//affichage_debug(gameEtat);
actionPrecedente = instruction.order;
- switch(instruction.order)
- {
+ switch(instruction.order) {
case MV_COURBURE://C'est un rayon de courbure
actionPrecedente = MV_COURBURE;
waitingAckID = ASSERVISSEMENT_COURBURE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- if(instruction.nextActionType == ENCHAINEMENT){
+ if(instruction.nextActionType == ENCHAINEMENT) {
MV_enchainement++;
localData5 = 1;
} else {
@@ -1330,22 +1274,21 @@
}
localData1 = ((instruction.direction == LEFT)?1:-1);
localData2 = instruction.arg3;
- if(InversStrat == 1 && ingnorInversionOnce == 0)
- {
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData1 = -localData1;//Inversion de la direction
}
BendRadius(instruction.arg1, localData2, localData1, localData5);
-
-
+
+
target_theta_robot = localData2 - theta_robot;
/*
if(instruction.direction == LEFT){
-
+
}else{
target_theta_robot = theta_robot + localData2;
}*/
-
- break;
+
+ break;
case MV_LINE://Ligne droite
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -1353,49 +1296,49 @@
MV_enchainement++;
localData5 = 1;
} else {
- if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
+ if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
localData5 = 2;
MV_enchainement = 0;
} else {
localData5 = 0;
}
- }
+ }
localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
GoStraight(localData2, 0, 0, localData5);
-
- target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
+
+ target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
target_theta_robot = theta_robot;
-
- break;
+
+ break;
case MV_TURN: //Rotation sur place
if(instruction.direction == RELATIVE) {
localData2 = instruction.arg3;
} else {//C'est un rotation absolu, il faut la convertir en relative
localData2 = instruction.arg3;
-
+
localData2 = (localData2 - theta_robot)%3600;
if(localData2 > 1800) {
localData2 = localData2-3600;
}
-
+
}
if(InversStrat == 1 && ingnorInversionOnce == 0) {
- localData2 = -localData2;
- }
+ localData2 = -localData2;
+ }
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Rotate(localData2);
-
-
- break;
+
+
+ break;
case MV_XYT:
if(instruction.direction == BACKWARD) {
localData1 = -1;
} else {
localData1 = 1;
}
-
+
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -instruction.arg3;
localData3 = 3000 - instruction.arg2;//Inversion du Y
@@ -1403,26 +1346,25 @@
localData3 = instruction.arg2;
localData2 = instruction.arg3;
}
-
+
GoToPosition(instruction.arg1,localData3,localData2,localData1);
waitingAckID = ASSERVISSEMENT_XYT;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
- target_x_robot = instruction.arg1;
+
+ target_x_robot = instruction.arg1;
target_y_robot = localData3;
target_theta_robot = localData2;
-
- break;
+
+ break;
case MV_RECALAGE:
- if(instruction.nextActionType == MECANIQUE)
- {
+ if(instruction.nextActionType == MECANIQUE) {
instruction.nextActionType = WAIT;
-
+
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
+
localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
-
+
if(instruction.precision == RECALAGE_Y) {
localData5 = 2;
if(InversStrat == 1 && ingnorInversionOnce == 0) {
@@ -1435,78 +1377,70 @@
localData3 = instruction.arg1;
}
GoStraight(localData2, localData5, localData3, 0);
- }
- else //CAPTEUR
- {
+ } else { //CAPTEUR
SendRawId(DATA_RECALAGE);
waitingAckID = RECEPTION_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
-
+
// On attend que les variables soient actualisé
while(!(waitingAckID == 0 && waitingAckFrom == 0))
canProcessRx();
while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
canProcessRx();
-
- if(instruction.precision == RECALAGE_Y) // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
- {
+
+ if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
+ } else if(instruction.precision == RECALAGE_X) {
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
+ } else if(instruction.precision == RECALAGE_T) {
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
}
- else if(instruction.precision == RECALAGE_X)
- {
- SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
- }
- else if(instruction.precision == RECALAGE_T)
- {
- SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
- }
}
- break;
-
+ break;
+
case ACTION:
int tempo = 0;
waitingAckID= ACK_ACTION; //On veut un ack de type action
waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
- if(tempo == 1){
+ if(tempo == 1) {
//L'action est spécifique
if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- }
- else {
+ } else {
gameEtat = ETAT_GAME_WAIT_ACK;
}
- #ifdef ROBOT_SMALL
- /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
- #endif
+#ifdef ROBOT_SMALL
+ /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
+#endif
return;
- #ifdef ROBOT_SMALL
+#ifdef ROBOT_SMALL
} else if (tempo == 2) {
// on est dans le cas de l'avance selon le telemetre
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
+
localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
GoStraight(telemetreDistance, 0, 0, 0);
// on reset la distance du telemetre à 0
telemetreDistance = 5000;
- #endif
- }else{
+#endif
+ } else {
//C'est un AX12 qu'il faut bouger
//AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
//AX12_enchainement++;
-
+
}
- break;
+ break;
default:
//Instruction inconnue, on l'ignore
- break;
- }
-
-
-
+ break;
+ }
+
+
+
if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
screenChecktry++;//On incrèment le conteur de tentative de 1
@@ -1516,35 +1450,31 @@
//AX12_processChange();//Il faut lancer le déplacement des AX12
//AX12_enchainement = 0;
}
- }
- else {//C'est un enchainement
- if(instruction.order == MV_LINE){
- gameEtat = ETAT_GAME_WAIT_ACK;
- }
- else{
+ } else { //C'est un enchainement
+ if(instruction.order == MV_LINE) {
+ gameEtat = ETAT_GAME_WAIT_ACK;
+ } else {
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
}
}
-
- break;
+
+ break;
case ETAT_GAME_WAIT_ACK:
canProcessRx();
-
+
if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
- //if(true) {
+ //if(true) {
cartesCheker.stop();
if(instruction.nextActionType == JUMP) {
if(instruction.jumpAction == JUMP_POSITION) {
gameEtat = ETAT_GAME_JUMP_POSITION;
- }
- else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time
+ } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
gameEtat = ETAT_GAME_JUMP_TIME;
cartesCheker.reset();//On reset le timeOut
- cartesCheker.start();
+ cartesCheker.start();
}
- }
- else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
+ } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
/*wait_ms(200);
#ifdef ROBOT_BIG
SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
@@ -1552,177 +1482,170 @@
SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
#endif
wait_ms(200);*/
-
+
gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
- switch(instruction.order)
- {
+ switch(instruction.order) {
case MV_COURBURE:
waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- break;
+ break;
case MV_LINE:
waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- break;
+ break;
case MV_TURN:
waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- break;
+ break;
case MV_XYT:
waitingAckID_FIN = ASSERVISSEMENT_XYT;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- break;
+ break;
case MV_RECALAGE:
waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- break;
+ break;
case ACTION:
-
- if (modeTelemetre == 0){
- if (telemetreDistance == 0){
- waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
+
+ if (modeTelemetre == 0) {
+ if (telemetreDistance == 0) {
+ waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
- }else if(telemetreDistance == 5000){
+ } else if(telemetreDistance == 5000) {
// on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
telemetreDistance = 0;
}
- }else{ // si on attend la reponse du telemetre
- //modeTelemetre = 1;
+ } else { // si on attend la reponse du telemetre
+ //modeTelemetre = 1;
waitingAckID_FIN = OBJET_SUR_TABLE;
- waitingAckFrom_FIN = 0;
+ waitingAckFrom_FIN = 0;
}
- break;
+ break;
default:
- break;
- }
- }
- else {
+ break;
+ }
+ } else {
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
}
- }
- else if(cartesCheker.read_ms () > 1000){
+ } else if(cartesCheker.read_ms () > 1000) {
cartesCheker.stop();
if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- }
- else {
+ } else {
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
}
}
- break;
-
+ break;
+
case ETAT_GAME_JUMP_TIME:
if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
cartesCheker.stop();//On arrete le timer
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
}
- break;
-
+ break;
+
case ETAT_GAME_JUMP_CONFIG:
signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
- // 1 si l'instruction est plus grande que la position du robot
- // -1 si l'instruction est plus petite que la position du robot
- // 0 si l'instruction et position du robot sont proche de moins de 1cm
- if (abs(x_robot-instruction.JumpTimeOrX)<10){
- depasX = 0;
- }else if(x_robot > instruction.JumpTimeOrX){
- depasX = -1;
+ // 1 si l'instruction est plus grande que la position du robot
+ // -1 si l'instruction est plus petite que la position du robot
+ // 0 si l'instruction et position du robot sont proche de moins de 1cm
+ if (abs(x_robot-instruction.JumpTimeOrX)<10) {
+ depasX = 0;
+ } else if(x_robot > instruction.JumpTimeOrX) {
+ depasX = -1;
}
-
- if(abs(y_robot-instruction.JumpY)<10){
- depasY = 0;
- }else if(y_robot > instruction.JumpY){
- depasY = -1;
+
+ if(abs(y_robot-instruction.JumpY)<10) {
+ depasY = 0;
+ } else if(y_robot > instruction.JumpY) {
+ depasY = -1;
}
-
+
gameEtat = ETAT_GAME_JUMP_POSITION;
- break;
+ break;
case ETAT_GAME_JUMP_POSITION:
bool Xok = false, Yok = false;
-
- if (depasX == 0){
- Xok = true;
- }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
- Xok = true;
- }
-
- if (depasY == 0){
- Yok = true;
- }else if ((instruction.JumpY - y_robot)*depasY < -5){
- Yok = true;
- }
-
- // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
- if (Xok && Yok){
- actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
- }
-
- break;
+
+ if (depasX == 0) {
+ Xok = true;
+ } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) {
+ Xok = true;
+ }
+
+ if (depasY == 0) {
+ Yok = true;
+ } else if ((instruction.JumpY - y_robot)*depasY < -5) {
+ Yok = true;
+ }
+
+ // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
+ if (Xok && Yok) {
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+
+ break;
case ETAT_GAME_WAIT_END_INSTRUCTION:
- canProcessRx();
+ canProcessRx();
if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
}
-
- break;
-
-
+
+ break;
+
+
case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
- if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
- {
+ if(timeoutWarning.read_ms() >= BALISE_TIMEOUT) { //ça fait plus de 2s, il faut changer de stratégie
+ gameEtat = ETAT_EVITEMENT;
+ if(Fevitement==1) {
+ EvitEtat= 0;
+ Fevitement=0;
+ }
+
+ /*-------------------------------------
+ code origine
+
if(instruction.nextLineOK != instruction.nextLineError)
{
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- }
+ }----------------------------------------*/
+
}
- break;
+ break;
+
+
+
+
case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
//actual_instruction = instruction.nextLineError;
gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
}
- break;
+ break;
case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
-/*
-#ifdef ROBOT_BIG
- actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
-#else
- actual_instruction = instruction.nextLineError;
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
-#endif
- gameEtat = ETAT_END;*/
-
- /*wait_ms(200);
- #ifdef ROBOT_BIG
- SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
- #else
- SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
- #endif
- wait_ms(200);*/
-
- switch(actionPrecedente){
+
+
+ switch(actionPrecedente) {
case MV_LINE:
-
+
if(instruction.direction == BACKWARD) {
localData1 = -1;
} else {
localData1 = 1;
}
-
+
GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
waitingAckID = ASSERVISSEMENT_XYT;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- gameEtat = ETAT_GAME_WAIT_ACK;
+ gameEtat = ETAT_GAME_WAIT_ACK;
instruction.order = MV_XYT;
instruction.arg1 = target_x_robot;
instruction.arg2 = target_y_robot;
@@ -1730,115 +1653,319 @@
instruction.direction = (localData1)?FORWARD:BACKWARD;
ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
return;
-
+
case MV_XYT:
- gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
+ gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
///////cv
break;
-
+
case MV_COURBURE:
//target_theta_robot = theta_robot - target_theta_robot;
//instruction.arg3 = instruction.arg3 - target_theta_robot;
- if(instruction.direction == LEFT){
+ if(instruction.direction == LEFT) {
target_theta_robot = target_theta_robot - theta_robot;
- }else{
+ } else {
target_theta_robot = theta_robot + target_theta_robot;
}
-
-
+
+
target_theta_robot = (target_theta_robot)%3600;
if(target_theta_robot > 1800) {
target_theta_robot = target_theta_robot-3600;
}
- if(InversStrat == 1) {
+ if(InversStrat == 1) {
target_theta_robot = -target_theta_robot;
}
instruction.arg3 = target_theta_robot;
-
-
- gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
-
+
+
+ gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
+
break;
default:
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- break;
+ break;
}
-
+
//actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
//gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- break;
+ break;
case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ingnorBaliseOnce = 1;
- break;
-
+ break;
+
case ETAT_EVITEMENT :
- E_Evitement EvitEtat= ETAT_INIT_EVITEMENT;
- while(1)
- {
- canProcessRx();
-
- switch(EvitEtat)
- {
- case ETAT_INIT_EVITEMENT:
- EvitEtat = ETAT_ESTIMATION_POSITION;
- break;
+
+ /*disposition des waypoint sur le terrain
+ on considère le terrain comme un rectangle de 3m par 1m50 , on exclus la zone de la rampe
+ ______________
+ | | format(x,y)en mm
+ Y| x x | (1000, 2250) (1500 , 2250)
+ | |
+ | x x | (1000 , 1500) (1500 , 1500)
+ | |
+ | x x | (1000 , 750) (1500 , 750)
+ | |
+ |______________|
+ X-->
+ */
+ static short x_terrain=3000;
+ static short y_terrain=1500;
+
+ static float x_cote_droit[3]= {0};
+ static float y_cote_droit[3]= {0};
+ static float x_cote_gauche[3]= {0};
+ static float y_cote_gauche[3]= {0};
+ static short cote=0;
+//---------------------------
+ /* short x_robot=500;
+ short y_robot=500;
+ short theta_robot=0;
+ //---------------------------
+ short target_x_robot=2000;
+ short target_y_robot=1000;
+ short target_theta_robot=1000;*/
+//--------------------------
+ float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
+ int proxy=400;//distance entre point de controle et obstacle/adversaire
+ int proximity=400;//distance entre l'objectif et obstacle/adversaire
+ short taille_petit=150;// distance proxymité max mur
+//---------------------------*
+ static unsigned short distance=50000;//valeur impossible
+ static unsigned short distance_prev=50000;
+ static unsigned short theta1,theta2,theta_adversaire;
+
+ switch(EvitEtat) {
+ case 0:
+
+ /*SendRawId(DATA_RECALAGE);
+ wait(0.1);
+ canProcessRx();
+ unsigned short distance_adverse=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
+ */
+ ingnorBalise=1;
+ Rotate(300); //on tourne a gauche pour scanner
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
+ canProcessRx();
+ SendRawId(DATA_RECALAGE);
+ distance=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
+ if(distance<=distance_prev) {
+ distance_prev=distance;
+ theta1=theta_robot;
+ }
+ }
+ Rotate(-600);//on tourne a droite pour scanner
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
- case ETAT_ESTIMATION_POSITION :
- /*wait_ms(100);
- Rotate(3600);
- EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK;
- waitingAckID = ASSERVISSEMENT_ROTATION;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;*/
- GoToPosition(1400,300,0,1);
- waitingAckID = ASSERVISSEMENT_XYT;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- break;
-
- case ETAT_ESTIMATION_POSITION_ROTATION_ACK:
- if(waitingAckID == 0 && waitingAckFrom == 0)
- {
- EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK_END;
+ Rotate(300);//on revien au centre
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
+ canProcessRx();
+ SendRawId(DATA_RECALAGE);
+ distance=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
+ if(distance<=distance_prev) {
+ distance_prev=distance;
+ theta2=theta_robot;
+ }
+ }
+ theta_adversaire=(theta1+theta2)/2;//version avec telemetre
+ //theta_adversaire=theta_robot;//version sans telemetre (robot en face
+
+ short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600;
+ // On passe le résultat entre -1800 et 1800
+ if (ang_target > 1800) ang_target = (ang_target- 3600);
+
+ // 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));
+
+ float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
+ float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
+
+ x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+ y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+
+ x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+ y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+
+ x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
+ y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
+
+ SendRawId(0x0D0);//calcul
+
+ //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]) ;
+ //printf("------------\n\r");
+ //-------------------------process------------------------------------------------
+
+ bool cote_droit=false, cote_gauche=false;
+
+
+ for (int i=0; i<3; i++) {
+ 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) {
+ cote_droit=true;
+ cote=1;
+ } else {
+ cote_droit=false;
+ break;
+ }
+
+ 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) {
+ cote_gauche=true;
+ cote=-1;
+ } else {
+ cote_gauche=false;
+ break;
+ }
+ }
+
+ if(!cote_droit && !cote_gauche)cote=0;
+
+ if (cote_droit && cote_gauche) {
+ if (ang_target<=0) {
+ cote = -1;// cote gauche
+ SendRawId(0x1D0);
+ } else if (ang_target>0) {
+ cote = 1; //cote droite
+ SendRawId(0x2D0);
+ }
+ }
+
+ if ( ang_target>600 || ang_target<-600)cote=0;
+
+
+ if (!cote_droit && !cote_gauche) {
+ cote=0;
+ }
+
+ //--------------------test target --------------------------------------
+
+ 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;
+
+
+ EvitEtat = 1;
+
+ break;
+
+ case 1 ://on attend la fin de la première rotation pour activer la balise
+ //ingnorBalise=1;
+ SendRawId(0x0D1);//init evitement
+ if(cote!=0) {
+ for(int i=0; i<3; i++) {
+ if(cote==-1) {
+ GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
+ SendRawId(0x1D1);//evitement a gauche
+ } else if(cote==1) {
+ GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
+ SendRawId(0x2D1);//evitement a droite
+ }
+ waitingAckID = ASSERVISSEMENT_XYT;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ Fevitement=1;
+ ingnorBalise=1;
+ waitingAckID_FIN = ASSERVISSEMENT_XYT_ROTATE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+
+ ingnorBalise=0;
+ waitingAckID_FIN = ASSERVISSEMENT_XYT_LINE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+
+ ingnorBalise=1;
waitingAckID_FIN = ASSERVISSEMENT_XYT;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+
}
- break;
-
- case ETAT_ESTIMATION_POSITION_ROTATION_ACK_END:
- if(waitingAckID == 0 && waitingAckFrom == 0)
- EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK_END;
- break;
-
- case ETAT_FIN_EVITEMENT:
- gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
- break;
- }
+ } else {
+ SendRawId(0x3D1);//cote=0 evitement non possible
+ EvitEtat=0;
+ gameEtat=ETAT_WARNING_END_LAST_INSTRUCTION;
+ ingnorBalise=0;
+ Fevitement=0;
+ }
+ EvitEtat=2;
+ break;
+
+ case 2:
+ SendRawId(0x0D2);
+ GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
+ waitingAckID = ASSERVISSEMENT_XYT;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ Fevitement=1;
+ waitingAckID_FIN = ASSERVISSEMENT_XYT;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+
+ EvitEtat=3;
+ break;
+
+ case 3: //on charge l'instruction suivante et sort de l'evitement
+ actual_instruction++;//on charge l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ EvitEtat = 0;
+ ingnorBalise=0;
+ Fevitement=0;
+ break;
}
-
-
+
break;
-
-
-
- case ETAT_END:
- if (ModeDemo){
+
+ case ETAT_END:
+ if (ModeDemo) {
gameEtat = ETAT_CHECK_CARTE_SCREEN;
ModeDemo = 1;
} else {
gameEtat = ETAT_END_LOOP;
}
- break;
+ break;
case ETAT_END_LOOP:
//Rien, on tourne en rond
-
- break;
+
+ break;
default:
-
- break;
- }
-}
+
+ break;
+ }
+}
@@ -1863,27 +1990,27 @@
if(FIFO_occupation!=0) {
int identifiant=msgRxBuffer[FIFO_lecture].id;
switch(identifiant) {
-
+
case ALIVE_MOTEUR:
- if (etat == ATT) {
-
- lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
- lcd.FillRect(0,400,400,150);
- lcd.SetTextColor(LCD_COLOR_BLACK);
- lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
- lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
+ if (etat == ATT) {
+
+ lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
+ lcd.FillRect(0,400,400,150);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
+ lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
}
break;
-
+
case ALIVE_BALISE:
- if (etat == ATT) {
-
- lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
- lcd.FillRect(0,600,400,150); //carte AX12
- lcd.SetTextColor(LCD_COLOR_BLACK);
- lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
- lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
- }
+ if (etat == ATT) {
+
+ lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
+ lcd.FillRect(0,600,400,150); //carte AX12
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
+ lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
+ }
break;
case RESET_IHM:
@@ -1896,8 +2023,8 @@
gameEtat = ETAT_GAME_START;
SendRawId(ACKNOWLEDGE_JACK);
}
- break;
-
+ break;
+
case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
case ALIVE_ACTIONNEURS_ARRIERE:
case ALIVE_HERKULEX:
@@ -1906,31 +2033,59 @@
waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
}
flag=1;
- break;
-
-
-
-
- /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
- case ACKNOWLEDGE_HERKULEX:
+ break;
+
+
+
+
+ /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
+ case ACKNOWLEDGE_HERKULEX:
case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
-
- case ACKNOWLEDGE_TELEMETRE:
- /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
+
+ case ACKNOWLEDGE_TELEMETRE:
+ /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
case ACKNOWLEDGE_MOTEUR:
case INSTRUCTION_END_BALISE:
+ case ACK_FIN_ACTION:
case INSTRUCTION_END_MOTEUR:
- case ACK_FIN_ACTION:
- if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
+ unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
+ memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
+ Send2Short(0x5D7,waitingAckFrom_FIN, waitingAckID_FIN);
+ Send2Short(0x5D8,msgRxBuffer[FIFO_lecture].id, recieveAckID);
+
+ if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
waitingAckFrom = 0;
- waitingAckID = 0;
+ waitingAckID = 0;
+ }
+ if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
+ //SendRawId(0x5D9);
+ waitingAckFrom_FIN = 0;
+ waitingAckID_FIN = 0;
}
- if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN)) {
- waitingAckFrom_FIN = 0;
- waitingAckID_FIN = 0;
- }
-
- break;
+
+ /*
+ if((waitingAckFrom == msgRxBuffer[FIFO_lecture].id) &&
+ ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] | (((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID) )
+ {
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ }
+ if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
+ |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
+ {
+ waitingAckFrom_FIN = 0;
+ waitingAckID_FIN = 0;
+ }
+ */
+ break;
+
+
+
+
+
+
+
+
#ifdef ROBOT_BIG
case ODOMETRIE_BIG_POSITION:
#else
@@ -1939,72 +2094,78 @@
x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
- break;
-
+ break;
+
case ACK_ACTION:
if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
waitingAckFrom = 0;
waitingAckID = 0;
}
break;
-
+
case BALISE_DANGER :
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
- break;
-
+ break;
+
case BALISE_STOP:
SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-
+
signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
- signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
-
- if(debut_angle_detection > fin_angle_detection)
- {
+ signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
+
+
+
+
+ if(debut_angle_detection > fin_angle_detection) {
angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
if(angle_moyen_balise_IR > 15.0f)
angle_moyen_balise_IR-=15.0f;
- }
- else
+ } else
angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
-
- #ifdef ROBOT_BIG
+
+#ifdef ROBOT_BIG
float seuil_bas_avant = 12.0; // >=
float seuil_haut_avant = 0.0; // <=
float seuil_bas_arriere = 4.0;
float seuil_haut_arriere = 8.0;
- #else
+#else
float seuil_bas_avant = 13.0;
float seuil_haut_avant = 15.0;
float seuil_bas_arriere = 5.0;
float seuil_haut_arriere = 7.0;
- #endif
-
- if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= 7
- || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
- || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere ){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
- if(needToStop() != 0 && ingnorBaliseOnce ==0) {
- if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
- {
+#endif
+
+
+
+ if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
+ || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
+ || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
+
+
+ if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
+ if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
+ SendRawId(0x0D9);//balise_stop
SendRawId(ASSERVISSEMENT_STOP);
//while(1); // ligne à décommenter si on est en homologation
if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
timeoutWarning.reset();
timeoutWarning.start();//Reset du timer utiliser par le timeout
}
+ //stop_evitement=1;
gameEtat = ETAT_WARNING_TIMEOUT;
}
}
-
+
+
+
+
}
-
-
-
ingnorBaliseOnce = 0;
- break;
-
+ break;
+
case BALISE_END_DANGER:
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
if(gameEtat == ETAT_WARNING_TIMEOUT) {
@@ -2012,79 +2173,78 @@
timeoutWarningWaitEnd.start();
gameEtat = ETAT_WARING_END_BALISE_WAIT;
}
- break;
-
+ break;
+
/*case OBJET_SUR_TABLE:
if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
-
+
gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
}
else{
-
+
waitingAckFrom = 0;
- waitingAckID = 0;
-
+ waitingAckID = 0;
+
strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
}
modeTelemetre = 0;
break;*/
-
+
case RECEPTION_DATA:
telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
waitingAckFrom = 0;
waitingAckID = 0;
break;
-
+
case RECEPTION_RECALAGE:
- telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //
+ telemetreDistance_arriere_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
telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
-
-
-
- if(ModeDemo==1)
- {
+
+
+
+ if(ModeDemo==1) {
sprintf(message,"%04d mm",telemetreDistance_arriere_droite);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER ARD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message, LEFT_MODE);
-
+
sprintf(message1,"%04d mm",telemetreDistance_avant_droite);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"LASER AVD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
-
+
sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(14),(unsigned char *)"LASER ARG : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(14),(unsigned char *)message2, LEFT_MODE);
-
+
sprintf(message3,"%04d mm",telemetreDistance_avant_gauche);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER AVG : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
}
break;
-
+
case RECEPTION_COULEUR:
- if (blocage_balise==0){
- couleur1=msgRxBuffer[FIFO_lecture].data[0];
- couleur2=msgRxBuffer[FIFO_lecture].data[1];
- couleur3=msgRxBuffer[FIFO_lecture].data[2];
-
- /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
- lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
- lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
+ if (blocage_balise==0) {
+ couleur1=msgRxBuffer[FIFO_lecture].data[0];
+ couleur2=msgRxBuffer[FIFO_lecture].data[1];
+ couleur3=msgRxBuffer[FIFO_lecture].data[2];
+
+ /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
+ lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
+ lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
}
-
- break;
-
+
+ break;
+
case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
actual_instruction = instruction.nextLineError;
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
/*waitingAckID_FIN=0;
waitingAckFrom_FIN=0;*/
SendRawId(0x40);
@@ -2103,7 +2263,7 @@
/* DESCRIPTION : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!) */
/****************************************************************************************/
signed char Bouton_Strat (void)
-{
+{
Button STRAT_1 (0, 30, 190, 110, strat_sd[0]);
Button STRAT_2 (210, 30, 190, 110, strat_sd[1]);
Button STRAT_3 (0, 150, 190, 110, strat_sd[2]);
@@ -2116,7 +2276,7 @@
Button STRAT_10 (210, 510, 190, 110, strat_sd[9]);
Button RETOUR (0, 680, 400, 110, "--Precedent--");
//Definition des boutons
-
+
Ack_strat = 0;
Strat = 0;
STRAT_1.Draw(0xFFF0F0F0, 0);
@@ -2127,102 +2287,101 @@
STRAT_6.Draw(0xFFF0F0F0, 0);
STRAT_7.Draw(0xFFF0F0F0, 0);
STRAT_8.Draw(0xFFF0F0F0, 0);
- STRAT_9.Draw(0xFFF0F0F0, 0);
- STRAT_10.Draw(0xFFF0F0F0, 0);
+ STRAT_9.Draw(0xFFF0F0F0, 0);
+ STRAT_10.Draw(0xFFF0F0F0, 0);
RETOUR.Draw(0xFFFF0000, 0);
-
- while(Ack_strat == 0)
- {
+
+ while(Ack_strat == 0) {
canProcessRx();
CANMessage msgTx=CANMessage();
//msgTx.id=ECRAN_CHOICE_STRAT;
if (RETOUR.Touched())
return -1;
while(RETOUR.Touched());
- //////////////////////////////STRATEGIE N°1
- if (STRAT_1.Touched()){
- Strat = 0;
- //msgTx.data[0] = 0x1;
- //can2.write(msgTx);
- while(STRAT_1.Touched());
- Ack_strat =1;
- }
- /////////////////////////////STRATEGIE N°2
- if (STRAT_2.Touched()){
- Strat = 1;
- //msgTx.data[0] = 0x2;
- //can2.write(msgTx);
- while(STRAT_2.Touched());
- Ack_strat =1;
- }
- //////////////////////////////STRATEGIE N°3
- if (STRAT_3.Touched()){
- Strat = 2;
- //msgTx.data[0] = 0x3;
- //can2.write(msgTx);
- while(STRAT_3.Touched());
- Ack_strat =1;
- }
- /////////////////////////////STRATEGIE N°4
- if (STRAT_4.Touched()){
- Strat = 3;
- //msgTx.data[0] = 0x4;
- //can2.write(msgTx);
- while(STRAT_4.Touched());
- Ack_strat =1;
- }
- ///////////////////////////////STRATEGIE N°5
- if (STRAT_5.Touched()){
- Strat = 4;
- //msgTx.data[0] = 0x5;
- //can2.write(msgTx);
- while(STRAT_5.Touched());
- Ack_strat =1;
- }
- ////////////////////////////////STRATEGIE N°6
- if (STRAT_6.Touched()){
- Strat = 5;
- //msgTx.data[0] = 0x6;
- //can2.write(msgTx);
- while(STRAT_6.Touched());
- Ack_strat =1;
- }
- /////////////////////////////////STRATEGIE N°7
- if (STRAT_7.Touched()){
- Strat = 6;
- //msgTx.data[0] = 0x7;
- //can2.write(msgTx);
- while(STRAT_7.Touched());
- Ack_strat =1;
- }
- /////////////////////////////////STRATEGIE N°8
- if (STRAT_8.Touched()){
- Strat = 7;
- //msgTx.data[0] = 0x8;
- //can2.write(msgTx);
- while(STRAT_8.Touched());
- Ack_strat =1;
- }
- /////////////////////////////////STRATEGIE N°9
- if (STRAT_9.Touched()){
- Strat = 8;
- //msgTx.data[0] = 0x9;
- //can2.write(msgTx);
- while(STRAT_9.Touched());
- Ack_strat =1;
- }
- ///////////////////////////////////STRATEGIE N°10
- if (STRAT_10.Touched()){
- Strat = 9;
- //msgTx.data[0] = 0xA;
- //can2.write(msgTx);
- while(STRAT_10.Touched());
- Ack_strat =1;
- }
-
+ //////////////////////////////STRATEGIE N°1
+ if (STRAT_1.Touched()) {
+ Strat = 0;
+ //msgTx.data[0] = 0x1;
+ //can2.write(msgTx);
+ while(STRAT_1.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////STRATEGIE N°2
+ if (STRAT_2.Touched()) {
+ Strat = 1;
+ //msgTx.data[0] = 0x2;
+ //can2.write(msgTx);
+ while(STRAT_2.Touched());
+ Ack_strat =1;
+ }
+ //////////////////////////////STRATEGIE N°3
+ if (STRAT_3.Touched()) {
+ Strat = 2;
+ //msgTx.data[0] = 0x3;
+ //can2.write(msgTx);
+ while(STRAT_3.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////STRATEGIE N°4
+ if (STRAT_4.Touched()) {
+ Strat = 3;
+ //msgTx.data[0] = 0x4;
+ //can2.write(msgTx);
+ while(STRAT_4.Touched());
+ Ack_strat =1;
+ }
+ ///////////////////////////////STRATEGIE N°5
+ if (STRAT_5.Touched()) {
+ Strat = 4;
+ //msgTx.data[0] = 0x5;
+ //can2.write(msgTx);
+ while(STRAT_5.Touched());
+ Ack_strat =1;
+ }
+ ////////////////////////////////STRATEGIE N°6
+ if (STRAT_6.Touched()) {
+ Strat = 5;
+ //msgTx.data[0] = 0x6;
+ //can2.write(msgTx);
+ while(STRAT_6.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°7
+ if (STRAT_7.Touched()) {
+ Strat = 6;
+ //msgTx.data[0] = 0x7;
+ //can2.write(msgTx);
+ while(STRAT_7.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°8
+ if (STRAT_8.Touched()) {
+ Strat = 7;
+ //msgTx.data[0] = 0x8;
+ //can2.write(msgTx);
+ while(STRAT_8.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°9
+ if (STRAT_9.Touched()) {
+ Strat = 8;
+ //msgTx.data[0] = 0x9;
+ //can2.write(msgTx);
+ while(STRAT_9.Touched());
+ Ack_strat =1;
+ }
+ ///////////////////////////////////STRATEGIE N°10
+ if (STRAT_10.Touched()) {
+ Strat = 9;
+ //msgTx.data[0] = 0xA;
+ //can2.write(msgTx);
+ while(STRAT_10.Touched());
+ Ack_strat =1;
+ }
+
}
- return Strat;
-
+ return Strat;
+
}
void affichage_compteur (int nombre)
@@ -2233,16 +2392,16 @@
unite = nombre-(10*dizaine);
print_segment(unite,-50);
print_segment(dizaine,100);
- if(centaine!=0){
+ if(centaine!=0) {
print_segment(centaine,350);
}
-
+
}
//****print_segment***
//Dessine en 7 segment le nombre en parametre
-// A
+// A
// =====
// | |
// B | G | E
@@ -2254,7 +2413,7 @@
/*
position pour le chiffre des unites
lcd.FillRect(460,75,120,25);// A
-lcd.FillRect(435,100,25,120);// B
+lcd.FillRect(435,100,25,120);// B
lcd.FillRect(435,245,25,120);// C
lcd.FillRect(460,365,120,25);// D
lcd.FillRect(580,100,25,120);// E
@@ -2263,97 +2422,96 @@
position pour le chiffre des dizaines
lcd.FillRect(260,75,120,25);// A
-lcd.FillRect(235,100,25,120);// B
+lcd.FillRect(235,100,25,120);// B
lcd.FillRect(235,245,25,120);// C
lcd.FillRect(260,365,120,25);// D
lcd.FillRect(380,100,25,120);// E
lcd.FillRect(380,245,25,120);// F
lcd.FillRect(260,220,120,25);// G
*/
-
+
void print_segment(int nombre, int decalage)
-{
-
- switch(nombre)
- {
+{
+
+ switch(nombre) {
case 0:
- lcd.FillRect(240-decalage,75,120,25);
- lcd.FillRect(215-decalage,100,25,120);
- lcd.FillRect(215-decalage,245,25,120);
- lcd.FillRect(360-decalage,245,25,120);
- lcd.FillRect(360-decalage,100,25,120);
- lcd.FillRect(240-decalage,365,120,25);
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);
+ lcd.FillRect(215-decalage,100,25,120);
+ lcd.FillRect(215-decalage,245,25,120);
+ lcd.FillRect(360-decalage,245,25,120);
+ lcd.FillRect(360-decalage,100,25,120);
+ lcd.FillRect(240-decalage,365,120,25);
+ break;
+
case 1:
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(360-decalage,245,25,120);// F
- break;
-
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
case 2:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(215-decalage,245,25,120);// C
- lcd.FillRect(240-decalage,365,120,25);// D
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(240-decalage,220,120,25);// G
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,245,25,120);// C
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
case 3:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(240-decalage,220,120,25);// G
- lcd.FillRect(240-decalage,365,120,25);// D
- lcd.FillRect(360-decalage,245,25,120);// F
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(240-decalage,220,120,25);// G
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
case 4:
- lcd.FillRect(215-decalage,100,25,120);// B
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(360-decalage,245,25,120);// F
- lcd.FillRect(240-decalage,220,120,25);// G
- break;
-
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
case 5:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(215-decalage,100,25,120);// B
- lcd.FillRect(240-decalage,220,120,25);// G
- lcd.FillRect(240-decalage,365,120,25);// D
- lcd.FillRect(360-decalage,245,25,120);// F
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(240-decalage,220,120,25);// G
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
case 6:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(215-decalage,100,25,120);// B
- lcd.FillRect(215-decalage,245,25,120);// C
- lcd.FillRect(240-decalage,365,120,25);// D
- lcd.FillRect(360-decalage,245,25,120);// F
- lcd.FillRect(240-decalage,220,120,25);// G
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(215-decalage,245,25,120);// C
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
case 7:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(360-decalage,245,25,120);// F
- break;
-
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
case 8:
- lcd.FillRect(240-decalage,75,120,25); // A
- lcd.FillRect(215-decalage,100,25,120);
- lcd.FillRect(215-decalage,245,25,120);
- lcd.FillRect(360-decalage,245,25,120);//...
- lcd.FillRect(360-decalage,100,25,120);
- lcd.FillRect(240-decalage,365,120,25);
- lcd.FillRect(240-decalage,220,120,25);// G
- break;
-
+ lcd.FillRect(240-decalage,75,120,25); // A
+ lcd.FillRect(215-decalage,100,25,120);
+ lcd.FillRect(215-decalage,245,25,120);
+ lcd.FillRect(360-decalage,245,25,120);//...
+ lcd.FillRect(360-decalage,100,25,120);
+ lcd.FillRect(240-decalage,365,120,25);
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
case 9:
- lcd.FillRect(240-decalage,75,120,25);// A
- lcd.FillRect(215-decalage,100,25,120);// B
- lcd.FillRect(240-decalage,365,120,25);// D
- lcd.FillRect(360-decalage,100,25,120);// E
- lcd.FillRect(360-decalage,245,25,120);// F
- lcd.FillRect(240-decalage,220,120,25);// G
- break;
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
}
}
@@ -2367,7 +2525,7 @@
lcd.FillRect(360-200,100,25,120);
lcd.FillRect(240-200,365,120,25);
lcd.FillRect(240-200,220,120,25);// G
-
+
lcd.FillRect(240,75,120,25); // A
lcd.FillRect(215,100,25,120);
lcd.FillRect(215,245,25,120);
@@ -2384,134 +2542,109 @@
unsigned short angleAvant = 0;
unsigned short angleArriere = 0;
unsigned short orientationArrondie = 0;
-
+
unsigned short position_avant_gauche=0;
unsigned short position_avant_droite=0;
unsigned short position_arriere_gauche=0;
unsigned short position_arriere_droite=0;
-
+
if(theta_robot >= 450 && theta_robot <= 1350)
- orientationArrondie = 90;
+ orientationArrondie = 90;
else if(theta_robot <= -450 && theta_robot >= -1350)
orientationArrondie = 270;
else if(theta_robot <= 450 && theta_robot >= -450)
orientationArrondie = 0;
else if(theta_robot >= 1350 && theta_robot <= -1350)
orientationArrondie = 180;
-
+
// Calcul de position pour faire la vérification de cohérence
- if(orientationArrondie == 90 || orientationArrondie == 270)
- {
+ if(orientationArrondie == 90 || orientationArrondie == 270) {
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;
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;
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;
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;
-
- }
- else if(orientationArrondie == 0 || orientationArrondie == 180)
- {
+
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) {
position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
}
-
-
- if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
- {
- if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1)
- {
+
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
+ if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
- angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double)ESPACE_INTER_TELEMETRE ))/M_PI;
- else
+ angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
-
+
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += angleArriere;
}
}
- }
- else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
- {
- if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1)
- {
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
+ if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
- angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double) ESPACE_INTER_TELEMETRE ))/M_PI;
- else
+ angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
-
+
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += angleArriere;
}
}
}
-
- if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
- {
- if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1)
- {
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
+ if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
- else
- angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
-
+ angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
+ if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += angleAvant;
}
}
}
- else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
- {
- if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1)
- {
- if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
- else
- angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
-
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += angleAvant;
- }
- }
- }
-
+
angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
-
-
- if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
- {
- if(orientationArrondie == 0)
- {
+
+
+
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
+ if(orientationArrondie == 0) {
angleRecalage -= 900;
-
+
/*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- distanceRecalage = *);
- else
- distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
- }
- else if(orientationArrondie == 90)
- {
- angleRecalage += 0;
- }
- else if(orientationArrondie == 180)
- {
- angleRecalage += 900;
- }
- else if(orientationArrondie == 270)
- {
+ distanceRecalage = *);
+ else
+ distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
+ } else if(orientationArrondie == 90) {
+ angleRecalage += 0;
+ } else if(orientationArrondie == 180) {
+ angleRecalage += 900;
+ } else if(orientationArrondie == 270) {
angleRecalage += 1800;
}
}
-
+
return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
}
@@ -2519,35 +2652,31 @@
{
unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
unsigned int moyennageTelemetre = 0;
-
+
telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
-
- if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
- {
+
+ if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
- if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
- {
+ if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_droite;
}
- if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
- {
+ if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
- if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
- {
+ if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_droite;
}
-
+
moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
+
return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
}
@@ -2555,35 +2684,31 @@
{
unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
unsigned int moyennageTelemetre = 0;
-
+
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;
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;
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;
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;
-
- if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1)
- {
+
+ if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
- if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
- {
+ if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_droite;
}
- if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
- {
+ if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
- if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
- {
+ if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_droite;
}
-
+
moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
- return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
}