CRAC Team
/
code_carte_IHM_sans_bug
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Strategie/Strategie.cpp
- Committer:
- gabrieltetar
- Date:
- 2020-07-08
- Revision:
- 24:1a13c998c7ac
- Parent:
- 22:c7763a7ec6c7
- Child:
- 27:ff70537a7619
File content as of revision 24:1a13c998c7ac:
#include "global.h"
#define M_PI 3.14159265358979323846
E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
T_etat strat_etat_s = INIT;
int waitingAckID_FIN;
int waitingAckFrom_FIN;
Ticker ticker;
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;
short SCORE_GLOBAL=0;
short SCORE_GR=0;
short SCORE_PR=0;
unsigned short distance_recalage;
unsigned short distance_revenir;
unsigned short x;
unsigned short y;
unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
unsigned short flag_check_carte = 0, flag_strat = 0, flag_timer;
int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 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;
//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
int flagSendCan=1;
unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
unsigned char ligne=0;
int Fevitement=0;
int EvitEtat= 0;
int stop_evitement=0;
signed char nbStrat = 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;//une fois détecté réinitialise
unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
short direction;
unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
struct S_Instruction instruction;
struct S_Dodge_queue dodgeq;
char couleur1, couleur2, couleur3;
float cptf;
int cpt,cpt1;
E_stratGameEtat memGameEtat= gameEtat;
E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
E_Stratposdebut etat_pos=RECALAGE_1;
void SendRawId (unsigned short id);
void can2Rx_ISR(void);
signed char blocage_balise;
void print_segment(int nombre, int decalage);
void affichage_compteur (int nombre);
void effacer_segment(long couleur);
unsigned char doAction(unsigned char id, unsigned short arg1, short arg2);
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;
unsigned char DT_AVD_interrupt=0;
unsigned char DT_AVG_interrupt=0;
unsigned char DT_ARD_interrupt=0;
unsigned char DT_ARG_interrupt=0;
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
/****************************************************************************************/
/* 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
strat_etat_s=FIN;
gameTimer.stop();//Arret du timer
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
//strat_etat_s=COMPTEUR;
blocage_balise=1;
}
}
/****************************************************************************************/
/* FUNCTION NAME: Strategie */
/* DESCRIPTION : Automate de gestion de la stratégie du robot */
/****************************************************************************************/
void Strategie(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
strat_etat_s=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;
SendRawId(ECRAN_ALL_CHECK);
flag_check_carte=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_check_carte=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(nbStrat);//Mise en cache de toute les instructions
led3=1;
SendRawId(GLOBAL_START);
gameEtat = ETAT_GAME_WAIT_FOR_JACK;
Debug_Audio(3,7);
if (strat_etat_s == TEST_MOTEUR|| strat_etat_s ==TEST_VENTOUSE || strat_etat_s == TEST_COULEUR || strat_etat_s ==TEST_SERVO_BRAS) {
SendRawId(DEBUG_FAKE_JAKE);
} else {
strat_etat_s = 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) {
localData2 = -localData2;//Inversion theta
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;
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 !!!)
case RECALAGE_1:
SendRawId(RECALAGE_START);
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
GoStraight(3000, 1,MOITIEE_ROBOT, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
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;
GoStraight(-100, 0, 0, 0);//-450
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=TOURNER;
break;
case TOURNER:
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(Cote==0) {
localData2 = 900;
} else {
localData2=-900;
}
Rotate(localData2);
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=RECALAGE_2;
break;
case RECALAGE_2:
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(Cote==1) {
localData3=3000-(MOITIEE_ROBOT);
} else {
localData3=MOITIEE_ROBOT;
}
GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=RECULER_2;
break;
case RECULER_2:
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
GoStraight(-100, 0, 0, 0);
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
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
} else {
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;
while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
canProcessRx();
etat_pos=FIN_POS;
break;
case FIN_POS:
actual_instruction = instruction.nextLineOK;
break;
}
}
break;
case ETAT_GAME_START:
Debug_Audio(3,3);
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:
flagNonRepriseErrorMot = 0;
/*
Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
*/
//printf("load next instruction\n");
if(dodgeq.nb > 0){//dodge q
instruction.order=dodgeq.inst[dodgeq.nb-1].order;
instruction.arg1=dodgeq.inst[dodgeq.nb-1].arg1;
instruction.arg2=dodgeq.inst[dodgeq.nb-1].arg2;
instruction.arg3=dodgeq.inst[dodgeq.nb-1].arg3;
gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
dodgeq.nb--;
}//end dodge q
else{// no dodge q
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;
}
}//end no dodge q
screenChecktry = 0;
ingnorInversionOnce = 0;
break;
case ETAT_GAME_PROCESS_INSTRUCTION:
/*
Traitement de l'instruction, envoie de la trame CAN
*/
//debug_Instruction(instruction);
actionPrecedente = instruction.order;
switch(instruction.order) {
case MV_COURBURE://C'est un rayon de courbure
Debug_Audio(3,6);
float alpha=0, theta=0;
unsigned short alph=0;
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;
}
}
if(InversStrat == 1 && ingnorInversionOnce == 0) {
if(instruction.direction == LEFT) instruction.direction = RIGHT;
else instruction.direction = LEFT;
}
localData1 = ((instruction.direction == LEFT)?1:-1);
localData2 = instruction.arg3;
/*if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData1 = -localData1;//Inversion de la direction
}*/
BendRadius(instruction.arg1, localData2, localData1, localData5);
if(localData2>0) {
direction=1;
} else {
direction=-1;
}
if(localData2>0)alph=localData2;
else alph=-localData2;
alpha = localData2*M_PI/1800.0f;
theta = theta_robot*M_PI/1800.0f;
if(instruction.direction == LEFT) { //-------------LEFT
if(alph<450){ // 1 XYT
dodgeq.inst[0].order = MV_XYT;
dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[0].arg3 = theta_robot + alph;// T
}
else if(alph<900){
for(int c=0;c<2;c++){ // 2 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[c].arg3 = theta_robot + alph;// T
alpha-=alpha/2.0f;
alph-=alph/2;
}
}
else if(alph<1350){
for(int c=0;c<3;c++){ // 3 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[c].arg3 = theta_robot + alph;// T
alpha-=alpha/3.0f;
alph-=alph/3;
}
}
else if(alph<1800){
for(int c=0;c<4;c++){ // 4 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[c].arg3 = theta_robot + alph;// T
alpha-=alpha/4.0f;
alph-=alph/4;
}
}
else if(alph<2250){
for(int c=0;c<5;c++){ // 5 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[c].arg3 = theta_robot + alph;// T
alpha-=alpha/5.0f;
alph-=alph/5;
}
}
else {
for(int c=0;c<6;c++){ // 6 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
dodgeq.inst[c].arg3 = theta_robot + alph;// T
alpha-=alpha/6.0f;
alph-=alph/6;
}
}
} else { //-----------------------------------------RIGHT
if(alph<450){ // 1 XYT
dodgeq.inst[0].order = MV_XYT;
dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[0].arg3 = theta_robot - alph;// T
}
else if(alph<900){
for(int c=0;c<2;c++){ // 2 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[c].arg3 = theta_robot - alph;// T
alpha-=alpha/2.0f;
}
}
else if(alph<1350){
for(int c=0;c<3;c++){ // 3 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[c].arg3 = theta_robot - alph;// T
alpha-=alpha/3.0f;
}
}
else if(alph<1800){
for(int c=0;c<4;c++){ // 4 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[c].arg3 = theta_robot - alph;// T
alpha-=alpha/4.0f;
}
}
else if(alph<2250){
for(int c=0;c<5;c++){ // 5 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[c].arg3 = theta_robot - alph;// T
alpha-=alpha/5.0f;
}
}
else {
for(int c=0;c<6;c++){ // 6 points de passages
dodgeq.inst[c].order = MV_XYT;
dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
dodgeq.inst[c].arg3 = theta_robot - alph;// T
alpha-=alpha/6.0f;
}
}
}
break;
case MV_LINE://Ligne droite
Debug_Audio(3,8);
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;
}
}
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
Debug_Audio(3,9);
target_x_robot = x_robot;
target_y_robot = y_robot;
target_theta_robot = theta_robot + localData2;
localData2 = instruction.arg3;
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -localData2;
}
if(instruction.direction == ABSOLUTE) {
//C'est un rotation absolu, il faut la convertir en relative
localData2 = (localData2 - theta_robot)%3600;
if(localData2 > 1800) localData2 = localData2-3600;
else if(localData2 <-1800) localData2 = localData2+3600;
}
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Rotate(localData2);
break;
case MV_XYT:
Debug_Audio(3,10);
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;
}
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:
waitingAckID_FIN = 0;
waitingAckFrom_FIN = 0;
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);
// unsigned char test=(unsigned char) tempo;
// SendMsgCan(0x5BD, &test,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 {
gameEtat = ETAT_GAME_WAIT_ACK;
}
return;
} 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;
} 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();
//SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
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
/*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);*/
gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
switch(instruction.order) {
case MV_BEZIER:
waitingAckID_FIN = ASSERVISSEMENT_BEZIER;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
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'strat_etat_s 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_END:
Debug_Audio(3,4);
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 message[10]="toto";
char message1[10]="toto";
char message2[10]="toto";
char message3[10]="toto";
char message4[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;
//SendRawId(
}
if(FIFO_occupation!=0) {
int identifiant=msgRxBuffer[FIFO_lecture].id;
if (waitingId == identifiant) waitingId = 0;
switch(identifiant) {
case ALIVE_MOTEUR:
if (strat_etat_s == 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 (strat_etat_s == 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:
strat_etat_s = 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
}
flag_check_carte=1;
break;
case ASSERVISSEMENT_ERROR_MOTEUR://erreur asservissement
{
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);
if(recieveAckID == waitingAckID_FIN && waitingAckFrom_FIN == INSTRUCTION_END_MOTEUR)
{
if(flagNonRepriseErrorMot) {
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
flagNonRepriseErrorMot = 0;
} else {
flagNonRepriseErrorMot = 1;
timeoutWarningWaitEnd.reset();
timeoutWarningWaitEnd.start();
//gameEtat = ETAT_WARING_END_BALISE_WAIT;
}
}
/*
if(flagNonRepriseErrorMot) {
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
flagNonRepriseErrorMot = 0;
} else {
flagNonRepriseErrorMot = 1;
gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
}*/
} 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_MOTEUR:
case INSTRUCTION_END_BALISE:
case ACK_FIN_ACTION:
case INSTRUCTION_END_MOTEUR:
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);
/*
//on desactive la balise dans les rotations XYT
if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1;
if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0;
//on desactive la balise dans les rotations
if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=1;
if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=0;
*/
// SendMsgCan(0x666,&ingnorBalise,1);
if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
waitingAckFrom = 0;
waitingAckID = 0;
}
if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
waitingAckFrom_FIN = 0;
waitingAckID_FIN = 0;
}
/*
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;
case ODOMETRIE_BIG_POSITION:
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 ODOMETRIE_SMALL_POSITION:
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 ACK_ACTION:
if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
waitingAckFrom = 0;
waitingAckID = 0;
}
break;
case BALISE_DANGER :
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
balise_danger();
break;
case BALISE_STOP:
SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
balise_stop(FIFO_lecture);
break;
case BALISE_END_DANGER:
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot);
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:
wait_us(150);
flagReceptionTelemetres = 1;
// telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
telemetreDistance_avant_gauche = 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_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
if(ModeDemo==1) {
sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
sprintf(message4,"%04d",theta_robot);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
}
break;
case RECEPTION_TELEMETRE_LOGIQUE:
DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0];
DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1];
DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2];
DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3];
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);*/
}
break;
default:
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;
}
}
void affichage_compteur (int nombre)
{
int dizaine=0,unite=0,centaine=0;
centaine = nombre/100;
dizaine = nombre/10;
unite = nombre-(10*dizaine);
print_segment(unite,-50);
print_segment(dizaine,100);
if(centaine!=0) {
print_segment(centaine,350);
}
}
//****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;
unsigned short tempo= telemetreDistance_arriere_gauche;
telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
telemetreDistance_arriere_droite=tempo;
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;
unsigned short tempo= telemetreDistance_arriere_gauche;
telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
telemetreDistance_arriere_droite=tempo;
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;
unsigned short tempo= telemetreDistance_arriere_gauche;
telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
telemetreDistance_arriere_droite=tempo;
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);
}
/*************************************************************************************************/
/* FUNCTION NAME: doAction */
/* DESCRIPTION : Effectuer une action specifique correspondant au numéro dans le fichier strat */
/*************************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short arg1, short arg2)
{
int retour = 1;
CANMessage msgTx=CANMessage();
msgTx.format=CANStandard;
msgTx.type=CANData;
switch(id)
{
case 120:
//SendRawId(ACCELERATEUR_INSERTION_AVANT_GAUCHE);
break;
case 121:
//SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE);
break;
case 150:
SCORE_PR+=arg1;
waitingAckFrom = 0;
waitingAckID = 0;
break;
case 200 :
SendRawId(DATA_TELEMETRE);
/*telemetreDistance = dataTelemetre();
wait_ms(1);
telemetreDistance = dataTelemetre();
telemetreDistance = telemetreDistance - 170;*/
break;
case 201 :
SendRawId(0x99);//
retour = 2;
break;
case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
isStopEnable =(unsigned char) arg1;
// SendMsgCan(0x5BC, &isStopEnable,1);
waitingAckFrom = 0;
waitingAckID =0;
break;
case 19: // CHANGER LA VITESSE + DECELERATION
//SendSpeedDecel(arg1,(unsigned short) arg2);
wait_us(200);
waitingAckFrom = 0;
waitingAckID =0;
break;
case 20://Désactiver l'asservissement
setAsservissementEtat(0);
break;
case 21://Activer l'asservissement
setAsservissementEtat(1);
break;
case 22://Changer la vitesse du robot
SendSpeed(arg1);//,(unsigned short)arg2, (unsigned short)arg2);
wait_us(200);
waitingAckFrom = 0;
waitingAckID = 0;
break;
case 23:
SendAccel(arg1,(unsigned short)arg2);//,(unsigned short)arg2, (unsigned short)arg2);
wait_us(200);
waitingAckFrom = 0;
waitingAckID = 0;
break;
case 151:
unsigned char argu_at_1_bras = arg1;
SendMsgCan(BRAS_AT_1, &argu_at_1_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 152:
unsigned char argu_re_1_bras = arg1;
SendMsgCan(BRAS_RE_1, &argu_re_1_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 153:
unsigned char argu_at_2_bras = arg1;
SendMsgCan(BRAS_AT_2, &argu_at_2_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 154:
unsigned char argu_re_2_bras = arg1;
SendMsgCan(BRAS_RE_2, &argu_re_2_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 155:
unsigned char argu_at_3_bras = arg1;
SendMsgCan(BRAS_AT_3, &argu_at_3_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 156:
unsigned char argu_re_3_bras = arg1;
SendMsgCan(BRAS_RE_3, &argu_re_3_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 157:
unsigned char argu_at_vent = arg1;
SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
case 158:
unsigned char argu_re_vent = arg1;
SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
break;
default:
retour = 0;//L'action n'existe pas, il faut utiliser le CAN
break;
}
return retour;//L'action est spécifique.
}