CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Strategie/Strategie.cpp
- Committer:
- Sitkah
- Date:
- 2018-05-03
- Revision:
- 37:fca332b64b42
- Parent:
- 36:6dd30780bd8e
- Child:
- 38:76f886a1c8e6
File content as of revision 37:fca332b64b42:
#include "global.h"
#include <string.h>
#include <sstream>
//#include "StrategieManager.h"
#define M_PI 3.14159265358979323846
#define VERT 0xFF00FF00
#define ROUGE 0xFFFF0000
#define BLEU 0xFF0000FF
#define JAUNE 0xFFFEFE00
#define BLANC 0xFF000000
#define ORANGE 0xFFFFA500
#define NOIR 0xFF000000
#define DIY_GREY 0xFFDFDFDF
char tableau_aff[10][50];
char tableau_etat[22][50]=
{
"Check_carte_screen",
"Check_carte_screen_wait_ack",
"Check_cartes",
"Check_cartes_wait_ack",
"Wait_force",
"Config",
"Game_init",
"Game_wait_for_jack",
"Game_start",
"Game_next_instruction",
"Game_instruction",
"Game_wait_ack",
"Game_jump_time",
"Game_jump_config",
"Game_jump_position",
"Game_wait_end_instruction",
"Warning_timeout",
"Waring_end_balise_wait",
"Warning_end_last_instruction",
"Warning_switch_strategie",
"End",
"End_loop",
};
int waitingAckID_FIN;
int waitingAckFrom_FIN;
Ticker ticker;
TS_DISCO_F469NI ts;
LCD_DISCO_F469NI lcd;
TS_StateTypeDef TS_State;
Ticker chrono;
Timeout AffTime;
Timer timer;
Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
Timer gameTimer;
Timer debugetatTimer;
Timer timeoutWarning;
Timer timeoutWarningWaitEnd;
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};
char counter = 0;
char check;
char Jack = 1;
int flag = 0, flag_strat = 0, flag_timer;
char Ack_strat = 0;
signed char Strat = 0;
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
signed short x_robot,y_robot,theta_robot;//La position du robot
signed short target_x_robot, target_y_robot, target_theta_robot;
E_InstructionType actionPrecedente;
signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
int flagSendCan=1;
unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
unsigned char ligne=0;
float angle_moyen_balise_IR = 0.0;
signed char Strategie = 0; //N° de la strategie (1-10)
unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
unsigned char countRobotNear = 0;//Le nombre de robot à proximité
unsigned char ingnorBaliseOnce = 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;
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;
E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
/////////////////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,"");
////////////////////////////////////////////////////////////
void SendRawId (unsigned short id);
void SelectionStrat (unsigned char numeroStrat);
void Setflag(void);
void can2Rx_ISR(void);
signed char Bouton_Strat (void);
void print_segment(int nombre, int decalage);
void affichage_compteur (int nombre);
void effacer_segment(long couleur);
unsigned short telemetreDistance=0;
unsigned short telemetreDistance_avant_gauche=0;
unsigned short telemetreDistance_avant_droite=0;
unsigned short telemetreDistance_arriere_gauche=0;
unsigned short telemetreDistance_arriere_droite=0;
#ifdef ROBOT_BIG
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE,CHECK_ACTIONNEURS_AVANT,CHECK_ACTIONNEURS_ARRIERE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE,ALIVE_ACTIONNEURS_AVANT,ALIVE_ACTIONNEURS_ARRIERE};
InterruptIn jack(PG_11); // entrée analogique en interruption pour le jack
#else
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
InterruptIn jack(PG_11); // entrée analogique en interruption pour le jack
#endif
/****************************************************************************************/
/* FUNCTION NAME: chronometre_ISR */
/* DESCRIPTION : Interruption à la fin des 90s du match */
/****************************************************************************************/
void chronometre_ISR (void)
{
SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
SendRawId(GLOBAL_GAME_END);//Indication fin de match
etat=FIN;
gameTimer.stop();//Arret du timer
#ifdef ROBOT_BIG
wait_ms(2000);
doFunnyAction();
#endif
while(1);//On bloque la programme dans l'interruption
}
/****************************************************************************************/
/* FUNCTION NAME: jack_ISR */
/* DESCRIPTION : Interruption en changement d'état sur le Jack */
/****************************************************************************************/
void jack_ISR (void)
{
if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
gameEtat = ETAT_GAME_START;//On débute le match
etat=COMPTEUR;
}
}
/****************************************************************************************/
/* FUNCTION NAME: SelectionStrat */
/* DESCRIPTION : Affiche la Stratégie sélectionnée sur l'ihm */
/****************************************************************************************/
void SelectionStrat (unsigned char Strategie)
{
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
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)
{
flagSendCan = 1;
}
void affichage_var(double Var){
if(ligne==7)
ligne=0;
char aff[10]="toto";
sprintf(aff,"%lf ",Var);
lcd.DisplayStringAt(120, LINE(5+(ligne)), (uint8_t *)aff, LEFT_MODE);
ligne++;
}
void affichage_debug(int Var){
int i;
int conv=(int)Var;
SUIVANT.Draw(ROUGE, 0);
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++){
lcd.SetBackColor(VERT);
lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
}
/*while(!ack_bluetooth){
//liaison_bluetooth();
}
ack_bluetooth=0;*/
/*while(SUIVANT.Touched()==0);
while(SUIVANT.Touched());*/
}
void automate_etat_ihm(void)
{
int j;
if (j==0){
ts.Init(lcd.GetXSize(), lcd.GetYSize());
j++;
}
ts.GetState(&TS_State);
switch (etat)
{
case INIT :
ts.GetState(&TS_State);
canProcessRx();
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
wait(0.15);
lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE);
//cartes non verifiées////////////////
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);
////////////////////////////////////////
FORCE_LAUNCH.Draw(0xFFFF0000, 0);
/*while(flag == 0)
{
ts.GetState(&TS_State);
canProcessRx();
if (FORCE_LAUNCH.Touched())
{
etat = CHOIX;
gameEtat = ETAT_CONFIG;
flag = 1;
while(FORCE_LAUNCH.Touched());
}
}*/
etat=ATT;
break;
case ATT :
if (flag==1){
etat = CHOIX;
gameEtat = ETAT_CONFIG;
}
else if (FORCE_LAUNCH.Touched()){
etat = CHOIX;
gameEtat = ETAT_CONFIG;
while(FORCE_LAUNCH.Touched());
}
break;
case CHOIX :
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
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)
{
canProcessRx();
if(DEMONSTRATION.Touched())
{
etat = DEMO;
while(DEMONSTRATION.Touched());
}
if(MATCH.Touched())
{
etat = SELECT_SIDE;
while(MATCH.Touched());
}
}
break;
case DEMO :
lcd.Clear(LCD_COLOR_WHITE);
RETOUR.Draw(0xFFFF0000, 0);
TEST_HERKULEX.Draw(VERT, 0);
TEST_LASER.Draw(VERT, 0);
TEST_COULEURS.Draw(VERT, 0);
TEST_TIR_BALLE.Draw(VERT, 0);
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 // A changer , discussion avec l'ihm
}
while (etat == DEMO)
{
canProcessRx();
if(TEST_HERKULEX.Touched())
{
//Strat = 0x10;
while(TEST_HERKULEX.Touched());
CANMessage trame_Tx = CANMessage();
trame_Tx.len = 1;
trame_Tx.format = CANStandard;
trame_Tx.type = CANData;
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=0x2;
can2.write(trame_Tx);
TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
etat = TEST_SERVO;
lcd.Clear(LCD_COLOR_WHITE);
ModeDemo=1;
}
else if(TEST_LASER.Touched())
{
//Strat = 0x11;
while(TEST_LASER.Touched());
TEST_LASER.Draw(0xFFF0F0F0, 0);
etat = TEST_TELEMETRE;
}
else if (TEST_COULEURS.Touched()){
while(TEST_COULEURS.Touched());
TEST_LASER.Draw(0xFFF0F0F0, 0);
etat =TEST_CAPTEURS ;
}
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);
CANMessage trame_Tx = CANMessage();
trame_Tx.len = 1;
trame_Tx.format = CANStandard;
trame_Tx.type = CANData;
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()){
while(TEST_TRIEUR.Touched());
etat=DEMO_TRIEUR;
lcd.Clear(LCD_COLOR_WHITE);
}
if(RETOUR.Touched())
{
etat = CHOIX;
while(RETOUR.Touched());
}
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
/*if (Strat< 0x10){ // Si la strat est une strat de match, on desactive le mode demo
ModeDemo = 0;
}
else { // sinon, on active le mode demo, utile pour la fin de la demo
ModeDemo = 1;
}*/
Ack_strat = 1;
wait_ms(10);
//tactile_printf("Strat %d, Asser desactive",Strat);
}
//SelectionStrat(Strategie);
}
break;
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 (RETOUR.Touched());
etat=DEMO;
}
else if(TRI.Touched()){
while (TRI.Touched());
SendRawId(AIGUILLEUR_CENTRE);
wait(0.5);
SendRawId(AIGUILLEUR_DROITE);
wait(0.5);
SendRawId(AIGUILLEUR_GAUCHE);
wait(0.5);
SendRawId(AIGUILLEUR_CENTRE);
break;
}
else if(AIGUILLEUR_D.Touched()){
while (AIGUILLEUR_D.Touched());
SendRawId(AIGUILLEUR_DROITE);
break;
}
else if(AIGUILLEUR_G.Touched()){
while (AIGUILLEUR_G.Touched());
SendRawId(AIGUILLEUR_GAUCHE);
break;
}
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;
case DEMO_IMMEUBLE:
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){
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(VERT,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_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 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++;
}
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++;
}
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()){
while(RETOUR.Touched());
etat=DEMO;
}
break;
}
if(RETOUR.Touched()){
while(RETOUR.Touched());
etat=DEMO;
}
}
break;
case TEST_SERVO:
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
ABAISSE_BLOC.Draw(VERT, 0);
RELEVE_BLOC.Draw(VERT, 0);
BRAS_ABEILLE_ON.Draw(VERT, 0);
BRAS_ABEILLE_OFF.Draw(VERT, 0);
INTERRUPTEUR_ON.Draw(VERT, 0);
INTERRUPTEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(0xFFFF0000,0);
while(etat==TEST_SERVO){
if(RETOUR.Touched()){
while (RETOUR.Touched());
etat=DEMO;
}
else if(ABAISSE_BLOC.Touched()){
while (ABAISSE_BLOC.Touched());
SendRawId(BAISSER_ATTRAPE_BLOC);
break;
}
else if(RELEVE_BLOC.Touched()){
while (RELEVE_BLOC.Touched());
SendRawId(RELEVER_ATTRAPE_BLOC);
break;
}
else if(BRAS_ABEILLE_ON.Touched()){
while (BRAS_ABEILLE_ON.Touched());
SendRawId(BRAS_ABEILLE_UP);
break;
}
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;
}
}
break;
case TEST_TIR:
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
TIR_CHATEAU.Draw(VERT, 0);
EPURATION.Draw(VERT, 0);
LANCEUR_ON.Draw(VERT, 0);
LANCEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(ROUGE, 0);
while(etat==TEST_TIR){
if(TIR_CHATEAU.Touched()){
while (TIR_CHATEAU.Touched());
SendRawId(INCLINAISON_CHATEAU);
break;
}
else if (EPURATION.Touched()){
while (EPURATION.Touched());
SendRawId(INCLINAISON_EPURATION);
break;
}
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()){
while (LANCEUR_OFF.Touched());
SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
break;
}
else if (RETOUR.Touched()){
while (RETOUR.Touched());
etat=DEMO;
}
}
break;
case TEST_TELEMETRE:
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){
SendRawId(DATA_RECALAGE);
wait(0.1);
canProcessRx();
if(RETOUR.Touched()){
while( RETOUR.Touched());
etat=DEMO;
lcd.Clear(LCD_COLOR_WHITE);
}
}
break;
case SELECT_SIDE :
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)
{
canProcessRx();
if(COTE_VERT.Touched())
{
liaison_Tx.envoyer(0x30,3,"123");
Cote = 0x0;
InversStrat = Cote;
etat = TACTIQUE;
CANMessage trame_Tx = CANMessage();
trame_Tx.len = 1;
trame_Tx.format = CANStandard;
trame_Tx.type = CANData;
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=Cote;
can2.write(trame_Tx);
while(COTE_VERT.Touched());
}
if(COTE_ORANGE.Touched())
{
Cote = 0x1;
InversStrat= Cote;
etat = TACTIQUE;
CANMessage trame_Tx = CANMessage();
trame_Tx.len = 1;
trame_Tx.format = CANStandard;
trame_Tx.type = CANData;
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=Cote;
can2.write(trame_Tx);
while(COTE_ORANGE.Touched());
}
if(RETOUR.Touched())
{
etat = CHOIX;
while(RETOUR.Touched());
}
}
break;
case TACTIQUE :
if (Cote == 0){
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
}
else if (Cote == 1){
lcd.Clear(JAUNE);
lcd.SetBackColor(JAUNE);
}
else {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
}
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)
{
etat = SELECT_SIDE;
}
else
{
etat = DETAILS;
}
wait(0.1);
break;
case DETAILS :
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);
while (etat == DETAILS)
{
canProcessRx();
if (CHECK.Touched())
{
if(gameEtat == ETAT_CONFIG) {
gameEtat = ETAT_GAME_INIT;
etat=LECTURE;
}
while(CHECK.Touched());
}
if(RETOUR.Touched())
{
etat = TACTIQUE;
while(RETOUR.Touched());
}
}
break;
case LECTURE :
break;
case AFF_WAIT_JACK :
lcd.Clear(BLANC);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
if (Cote == 0){
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
}
else if (Cote == 1){
lcd.Clear(JAUNE);
lcd.SetBackColor(JAUNE);
}
else {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
}
canProcessRx();
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
etat=WAIT_JACK;
break;
case WAIT_JACK:
break;
case COMPTEUR:
/*cptf=gameTimer.read();
lcd.SetTextColor(LCD_COLOR_BLACK);
cpt=int(cptf);
if(cpt != cpt1){
lcd.Clear(VERT);
affichage_compteur(100-cpt);
}
cpt1=cpt;
flag_timer=0;
affichage_debug(gameEtat);
lcd.SetBackColor(LCD_COLOR_WHITE);
*/
break;
case FIN :
lcd.Clear (LCD_COLOR_WHITE);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(120, 190, (uint8_t *)"LANCEMENT", LEFT_MODE);
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.DisplayStringAt(120, 210, (uint8_t *)"REDEMARAGE", LEFT_MODE);
lcd.DisplayStringAt(120, 230, (uint8_t *)"NECESSAIRE", LEFT_MODE);
while(1); // force le redemarage du robot
//break;
}
}
/****************************************************************************************/
/* FUNCTION NAME: automate_process */
/* DESCRIPTION : Automate de gestion de la stratégie du robot */
/****************************************************************************************/
void automate_process(void){
static unsigned char AX12_enchainement = 0;
static unsigned char MV_enchainement = 0;
signed char localData1 = 0;
signed short localData2 = 0;
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:
/*
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
*/
//printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
cartesCheker.stop();
screenChecktry = 0;
countAliveCard++;
checkCurrent++;
if(checkCurrent >= NOMBRE_CARTES) {
printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
if(countAliveCard >= NOMBRE_CARTES) {
gameEtat = ETAT_CONFIG;
flag=1;
//tactile_printf("Selection couleur et strategie");
}
else {
gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
waitingAckFrom = ECRAN_ALL_CHECK;
}
}
else
gameEtat = ETAT_CHECK_CARTES;
}
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){
gameEtat = ETAT_CONFIG;
flag=1;
}
else{
gameEtat = ETAT_WAIT_FORCE;
waitingAckFrom = ECRAN_ALL_CHECK;
}
}
else
gameEtat = ETAT_CHECK_CARTES;
}
else
gameEtat = ETAT_CHECK_CARTES;
}
break;
case ETAT_WAIT_FORCE:
/*
Attente du forçage de la part de la carte IHM
*/
if(waitingAckFrom == 0) {
gameEtat = ETAT_CONFIG;
}
break;
case ETAT_CONFIG:
/*
Attente de l'odre de choix de mode,
Il est possible de modifier la couleur et l'id de la stratégie
Il est aussi possible d'envoyer les ordres de debug
*/
modeTelemetre = 0;
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)
{
SendRawId(DEBUG_FAKE_JAKE);
}
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
#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
localData2 = POSITION_DEBUT_T;
localData3 = POSITION_DEBUT_Y;
if(InversStrat == 1) {
localData2 = 1800-localData2;//Inversion theta
if(localData2 > 1800) localData2 -= 3600;
else if(localData2 <= -1800) localData2 += 3600;
localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
}
SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
#endif
#ifdef ROBOT_SMALL
localData2 = POSITION_DEBUT_T;
localData3 = POSITION_DEBUT_Y;
if(InversStrat == 1) {
localData2 = -localData2;//Inversion theta
localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
}
SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,localData3,localData2);
#endif
break;
case ETAT_GAME_WAIT_FOR_JACK:
//On attend le jack
break;
case ETAT_GAME_START:
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
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);
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;
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
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
}
screenChecktry = 0;
ingnorInversionOnce = 0;
break;
case ETAT_GAME_PROCESS_INSTRUCTION:
/*
Traitement de l'instruction, envoie de la trame CAN
*/
//debug_Instruction(instruction);
affichage_debug(gameEtat);
actionPrecedente = 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){
MV_enchainement++;
localData5 = 1;
} else {
if(MV_enchainement > 0) {
localData5 = 2;
MV_enchainement = 0;
} else {
localData5 = 0;
}
}
localData1 = ((instruction.direction == LEFT)?1:-1);
localData2 = instruction.arg3;
if(InversStrat == 1 && ingnorInversionOnce == 0)
{
localData1 = -localData1;//Inversion de la direction
#ifdef ROBOT_BIG
localData2 = -localData2;
localData1 = -localData1;//Inversion de la direction
#endif
}
BendRadius(instruction.arg1, localData2, localData1, localData5);
target_theta_robot = localData2 - theta_robot;
/*
if(instruction.direction == LEFT){
}else{
target_theta_robot = theta_robot + localData2;
}*/
break;
case MV_LINE://Ligne droite
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(instruction.nextActionType == ENCHAINEMENT) {
MV_enchainement++;
localData5 = 1;
} else {
if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
localData5 = 2;
MV_enchainement = 0;
} else {
localData5 = 0;
}
}
#ifdef ROBOT_BIG
if(InversStrat == 1 && ingnorInversionOnce == 0) {
/*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
else instruction.direction = FORWARD;*/
instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
}
#endif
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_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
target_theta_robot = theta_robot;
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;
}
}
#ifdef ROBOT_SMALL
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -localData2;
}
#endif
#ifdef ROBOT_BIG
if(InversStrat == 1 && ingnorInversionOnce == 0) {
if(instruction.direction == RELATIVE) {
localData2 = instruction.arg3;
} else {//C'est un rotation absolu, il faut la convertir en relative
localData2 = 1800-instruction.arg3;//Inversion theta
if(localData2 > 1800) localData2 -= 3600;
else if(localData2 <= -1800) localData2 += 3600;
localData2 = (localData2 - theta_robot)%3600;
if(localData2 > 1800) {
localData2 = localData2-3600;
}
}
}
#endif
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Rotate(localData2);
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
} else {
localData3 = instruction.arg2;
localData2 = instruction.arg3;
}
#ifdef ROBOT_BIG
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData1 = -localData1;
localData2 = 1800-instruction.arg3;//Inversion theta
if(localData2 > 1800) localData2 -= 3600;
else if(localData2 <= -1800) localData2 += 3600;
}
#endif
GoToPosition(instruction.arg1,localData3,localData2,localData1);
waitingAckID = ASSERVISSEMENT_XYT;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
target_x_robot = instruction.arg1;
target_y_robot = localData3;
target_theta_robot = localData2;
break;
case MV_RECALAGE:
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) {
localData3 = 3000 - instruction.arg1;//Inversion du Y
} else {
localData3 = instruction.arg1;
}
} else {
localData5 = 1;
localData3 = instruction.arg1;
}
GoStraight(localData2, localData5, localData3, 0);
}
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)
{
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() );
}
}
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){
//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 {
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
return;
#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{
//C'est un AX12 qu'il faut bouger
//AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
//AX12_enchainement++;
}
break;
default:
//Instruction inconnue, on l'ignore
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
cartesCheker.reset();//On reset le timeOut
cartesCheker.start();
if(AX12_enchainement > 0) {
//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{
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;
case ETAT_GAME_WAIT_ACK:
canProcessRx();
if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
//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
gameEtat = ETAT_GAME_JUMP_TIME;
cartesCheker.reset();//On reset le timeOut
cartesCheker.start();
}
}
else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
switch(instruction.order)
{
case MV_COURBURE:
waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_LINE:
waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_TURN:
waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_XYT:
waitingAckID_FIN = ASSERVISSEMENT_XYT;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_RECALAGE:
waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case 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){
// 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;
waitingAckID_FIN = OBJET_SUR_TABLE;
waitingAckFrom_FIN = 0;
}
break;
default:
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){
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 {
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
}
}
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;
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;
}
if(abs(y_robot-instruction.JumpY)<10){
depasY = 0;
}else if(y_robot > instruction.JumpY){
depasY = -1;
}
gameEtat = ETAT_GAME_JUMP_POSITION;
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;
case ETAT_GAME_WAIT_END_INSTRUCTION:
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;
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
{
gameEtat = ETAT_EVITEMENT;
}
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;
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;*/
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;
instruction.order = MV_XYT;
instruction.arg1 = target_x_robot;
instruction.arg2 = target_y_robot;
instruction.arg3 = target_theta_robot;
instruction.direction = (localData1)?FORWARD:BACKWARD;
ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
return;
case MV_XYT:
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){
target_theta_robot = target_theta_robot - theta_robot;
}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) {
target_theta_robot = -target_theta_robot;
}
instruction.arg3 = target_theta_robot;
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;
}
//actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
//gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
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;
case ETAT_EVITEMENT :
E_Evitement EvitEtat= ETAT_INIT_EVITEMENT;
while(1)
{
canProcessRx();
switch(EvitEtat)
{
case ETAT_INIT_EVITEMENT:
EvitEtat = ETAT_ESTIMATION_POSITION;
break;
case ETAT_ESTIMATION_POSITION :
wait_ms(100);
Rotate(3600);
EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK;
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
break;
case ETAT_ESTIMATION_POSITION_ROTATION_ACK:
if(waitingAckID == 0 && waitingAckFrom == 0)
EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK_END;
break;
case ETAT_ESTIMATION_POSITION_ROTATION_ACK_END:
if(angle_moyen_balise_IR >= 13.5 && angle_moyen_balise_IR <= 14.5)
{
wait_ms(100);
SendRawId(ASSERVISSEMENT_STOP);
EvitEtat = ETAT_FIN_EVITEMENT;
}
break;
case ETAT_FIN_EVITEMENT:
break;
}
}
break;
case ETAT_END:
if (ModeDemo){
gameEtat = ETAT_CHECK_CARTE_SCREEN;
ModeDemo = 1;
} else {
gameEtat = ETAT_END_LOOP;
}
break;
case ETAT_END_LOOP:
//Rien, on tourne en rond
break;
default:
break;
}
}
/****************************************************************************************/
/* FUNCTION NAME: canProcessRx */
/* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
/****************************************************************************************/
void canProcessRx(void)
{
static signed char FIFO_occupation=0,FIFO_max_occupation=0;
char useless1 = 0;
char message[10]="toto";
char message1[10]="toto";
char message2[10]="toto";
char message3[10]="toto";
FIFO_occupation=FIFO_ecriture-FIFO_lecture;
if(FIFO_occupation<0)
FIFO_occupation=FIFO_occupation+SIZE_FIFO;
if(FIFO_max_occupation<FIFO_occupation)
FIFO_max_occupation=FIFO_occupation;
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);
}
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);
}
break;
case RESET_IHM:
etat = CHOIX;
break;
case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
case GLOBAL_JACK:
if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
gameEtat = ETAT_GAME_START;
SendRawId(ACKNOWLEDGE_JACK);
}
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:
case ECRAN_ALL_CHECK:
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
}
break;
/////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
case ACKNOWLEDGE_HERKULEX:
case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans INSTRUCTION_END_AX12 mais conserve l'ident initial
case ACKNOWLEDGE_TELEMETRE:
/////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
case ACKNOWLEDGE_MOTEUR:
case INSTRUCTION_END_BALISE:
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)) {
lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"ACK OK",LEFT_MODE);
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)) {
//SendRawId(waitingAckID);
waitingAckFrom_FIN = 0;
waitingAckID_FIN = 0;
}
break;
#ifdef ROBOT_BIG
case ODOMETRIE_BIG_POSITION:
#else
case ODOMETRIE_SMALL_POSITION:
#endif
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;
//case SERVO_AX12_SETGOAL:
//SendAck(0x114, SERVO_AX12_SETGOAL);
//if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
//AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8));
//break;
/*case SERVO_AX12_PROCESS:
SendAck(0x114, SERVO_AX12_PROCESS);
//AX12_processChange(1);
break;
case SERVO_AX12_DONE:
SendRawId(POMPE_PWM);
//SendAck(0x114, SERVO_AX12_DONE);
AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
waitingAckFrom = 0;
waitingAckID = 0;*/
//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;
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)
{
angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0;
if(angle_moyen_balise_IR > 15.0)
angle_moyen_balise_IR-=15.0;
}
else
angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
if (instruction.order != MV_TURN && instruction.order != ACTION ){ //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)
{
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
}
gameEtat = ETAT_WARNING_TIMEOUT;
}
}
}
ingnorBaliseOnce = 0;
break;
case BALISE_END_DANGER:
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
if(gameEtat == ETAT_WARNING_TIMEOUT) {
timeoutWarningWaitEnd.reset();
timeoutWarningWaitEnd.start();
gameEtat = ETAT_WARING_END_BALISE_WAIT;
}
break;
case OBJET_SUR_TABLE:
if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
}
else{
waitingAckFrom = 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*35.5+50;
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_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)
{
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);
}
break;
case RECEPTION_COULEUR:
couleur1=msgRxBuffer[FIFO_lecture].data[0];
couleur2=msgRxBuffer[FIFO_lecture].data[1];
couleur3=msgRxBuffer[FIFO_lecture].data[2];
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;
/*waitingAckID_FIN=0;
waitingAckFrom_FIN=0;*/
SendRawId(0x40);
break;
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
}
}
/****************************************************************************************/
/* FUNCTION NAME: Bouton_Strat */
/* 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]);
Button STRAT_4 (210, 150, 190, 110, strat_sd[3]);
Button STRAT_5 (0, 270, 190, 110,strat_sd[4]);
Button STRAT_6 (210, 270, 190, 110, strat_sd[5]);
Button STRAT_7 (0, 390, 190, 110, strat_sd[6]);
Button STRAT_8 (210, 390, 190, 110, strat_sd[7]);
Button STRAT_9 (0, 510, 190, 110, strat_sd[8]);
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);
STRAT_2.Draw(0xFFF0F0F0, 0);
STRAT_3.Draw(0xFFF0F0F0, 0);
STRAT_4.Draw(0xFFF0F0F0, 0);
STRAT_5.Draw(0xFFF0F0F0, 0);
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);
RETOUR.Draw(0xFFFF0000, 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;
}
}
return Strat;
}
void affichage_compteur (int nombre)
{
int dizaine=0,unite=0;
dizaine = nombre/10;
unite = nombre-(10*dizaine);
print_segment(unite,0);
print_segment(dizaine,200);
}
//****print_segment***
//Dessine en 7 segment le nombre en parametre
// A
// =====
// | |
// B | G | E
// |=====|
// C | | F
// | |
// =====
// D
/*
position pour le chiffre des unites
lcd.FillRect(460,75,120,25);// A
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
lcd.FillRect(580,245,25,120);// F
lcd.FillRect(460,220,120,25);// G
position pour le chiffre des dizaines
lcd.FillRect(260,75,120,25);// A
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)
{
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;
case 1:
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;
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;
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;
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;
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;
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;
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;
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;
}
}
void effacer_segment(long couleur)
{
lcd.SetTextColor(couleur);
lcd.FillRect(240-200,75,120,25); // A
lcd.FillRect(215-200,100,25,120);
lcd.FillRect(215-200,245,25,120);
lcd.FillRect(360-200,245,25,120);//...
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);
lcd.FillRect(360,245,25,120);//...
lcd.FillRect(360,100,25,120);
lcd.FillRect(240,365,120,25);
lcd.FillRect(240,220,120,25);// G
}
short recalageAngulaireCapteur(void)
{
unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
unsigned int moyennageTelemetre = 0;
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;
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)
{
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)
{
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(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 =(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)
{
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 =(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(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)
{
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)
{
angleRecalage += 1800;
}
}
return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
}
short recalageDistanceX(void)
{
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)
{
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
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)
{
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
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);
}
short recalageDistanceY(void)
{
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)
{
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
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)
{
nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
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);
}