CRAC Team
code petit robot pour homologation
Fork of
CRAC-Strat_2017_V2
by 
CRAC Team
Diff: Strategie/Strategie.cpp
- Revision:
- 12:14729d584500
- Parent:
- 11:ed13a480ddca
- Child:
- 14:c8fc06c4887f
diff -r ed13a480ddca -r 14729d584500 Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp Mon May 02 19:40:59 2016 +0000
+++ b/Strategie/Strategie.cpp Mon May 09 09:10:17 2016 +0000
@@ -18,10 +18,12 @@
signed short x_robot,y_robot,theta_robot;//La position du robot
+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
+
#ifdef ROBOT_BIG
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
-InterruptIn jack(p24); // entrée analogique en interruption pour le jack
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
+InterruptIn jack(p25); // entrée analogique en interruption pour le jack
#else
unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
@@ -34,6 +36,8 @@
unsigned char countRobotNear = 0;//Le nombre de robot à proximité
+unsigned char ingnorBaliseOnce = 0;
+
/****************************************************************************************/
/* FUNCTION NAME: chronometre_ISR */
/* DESCRIPTION : Interruption à la fin des 90s du match */
@@ -45,6 +49,7 @@
gameTimer.stop();//Arret du timer
#ifdef ROBOT_BIG
+ wait_ms(2000);
doFunnyAction();
#endif
@@ -78,7 +83,7 @@
signed short localData4 = 0;
unsigned char localData5 = 0;
- if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments
+ if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
gameTimer.stop();
gameTimer.reset();
gameEtat = ETAT_END;//Fin du temps
@@ -149,6 +154,7 @@
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);//On dit à l'IHM que toutes les cartes sont en ligne
@@ -163,8 +169,10 @@
} 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;
@@ -198,29 +206,37 @@
break;
case ETAT_GAME_INIT:
//On charge la liste des instructions
- //strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
loadAllInstruction();//Mise en cache de toute les instructions
gameEtat = ETAT_GAME_WAIT_FOR_JACK;
SendRawId(ECRAN_ACK_START_MATCH);
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
- SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
-#else
+
+#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 = -localData2;//Inversion theta
+ localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
+ }
+ SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
+#endif /*
SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
#endif*/
break;
case ETAT_GAME_WAIT_FOR_JACK:
- //TODO Attendre le jack
+ //On attend le jack
break;
case ETAT_GAME_START:
- chronoEnd.attach(&chronometre_ISR,90);
+ chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gameTimer.reset();
gameTimer.start();
- jack.fall(NULL);
+ jack.fall(NULL);//On désactive l'interruption du jack
SendRawId(GLOBAL_START);
+ tactile_printf("Start");//Pas vraiment util mais bon
break;
case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
/*
@@ -234,10 +250,8 @@
instruction = strat_instructions[actual_instruction];
//On effectue le traitement de l'instruction
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
- //actual_instruction++;
}
screenChecktry = 0;
- wait_ms(100);
break;
case ETAT_GAME_PROCESS_INSTRUCTION:
/*
@@ -246,7 +260,7 @@
//debug_Instruction(instruction);
switch(instruction.order)
{
- case MV_COURBURE:
+ case MV_COURBURE://C'est un rayon de courbure
waitingAckID = ASSERVISSEMENT_COURBURE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(instruction.nextActionType == ENCHAINEMENT) {
@@ -261,16 +275,20 @@
}
}
localData1 = ((instruction.direction == LEFT)?1:-1);
+ if(InversStrat == 1)
+ {
+ localData1 = -localData1;//Inversion de la direction
+ }
BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
break;
- case MV_LINE:
+ case MV_LINE://Ligne droite
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
if(instruction.nextActionType == ENCHAINEMENT) {
MV_enchainement++;
localData5 = 1;
} else {
- if(MV_enchainement > 0) {
+ if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
localData5 = 2;
MV_enchainement = 0;
} else {
@@ -280,19 +298,23 @@
localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
GoStraight(localData2, 0, 0, localData5);
break;
- case MV_TURN:
+ case MV_TURN: //Rotation sur place
if(instruction.direction == RELATIVE) {
localData2 = instruction.arg3;
- } else {
- if(abs(instruction.arg3 - theta_robot) > 180) {
- localData2 = 360 - instruction.arg3 - theta_robot;
- } else {
- localData2 = instruction.arg3 - theta_robot;
+ } else {//C'est un rotation absolu, il faut la convertir en relative
+ localData2 = instruction.arg3;
+
+ if(InversStrat == 1) {
+ localData2 = -localData2;
}
+
+ localData2 = (localData2 - theta_robot)%3600;
+ if(localData2 > 1800) {
+ localData2 = localData2-3600;
+ }
+
}
- if(InversStrat == 1) {
- localData2 = -localData2;
- }
+
Rotate(localData2);
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -305,12 +327,13 @@
}
if(InversStrat == 1) {
- //localData2 = 360 - instruction.arg3
+ localData2 = -instruction.arg3;
localData3 = 3000 - instruction.arg2;//Inversion du Y
} else {
localData3 = instruction.arg2;
+ localData2 = instruction.arg3;
}
- GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1);
+ GoToPosition(instruction.arg1,localData3,localData2,localData1);
waitingAckID = ASSERVISSEMENT_XYT;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
break;
@@ -318,7 +341,7 @@
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
instruction.nextActionType = WAIT;
- localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);
+ 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;
@@ -454,20 +477,30 @@
case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
- if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie
+ if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
{
gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
}
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 = 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
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;
@@ -512,6 +545,7 @@
case ALIVE_IHM:
case ALIVE_ACTIONNEURS:
case ALIVE_POMPES:
+ case ALIVE_AX12:
case ECRAN_ALL_CHECK:
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
@@ -590,19 +624,26 @@
}
can1.write(msgTx);
wait_ms(10);
- tactile_printf("Strat %d selectionne",msgTx.data[0]);
+ setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
+ tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
}
break;
case BALISE_STOP:
SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
- if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
- {
- wait_ms(10);
- SendRawId(ASSERVISSEMENT_STOP);
- gameEtat = ETAT_WARNING_TIMEOUT;
- timeoutWarning.reset();
- timeoutWarning.start();//Reset du timer utiliser par le timeout
+
+ if(needToStop() != 0 && ingnorBaliseOnce ==0) {
+ if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
+ {
+ SendRawId(ASSERVISSEMENT_STOP);
+ //while(1);
+ gameEtat = ETAT_WARNING_TIMEOUT;
+ if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
+ }
+ }
}
+ ingnorBaliseOnce = 0;
break;
case BALISE_END_DANGER:
if(gameEtat == ETAT_WARNING_TIMEOUT) {
@@ -611,6 +652,18 @@
gameEtat = ETAT_WARING_END_BALISE_WAIT;
}
break;
+
+ case ECRAN_CHOICE_START_ACTION:
+ if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
+ if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
+ runRobotTest();
+ } else {
+ initRobotActionneur();
+ }
+ wait_ms(500);
+ SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
+ }
+ break;
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;