CRAC Team
code petit robot pour homologation
Fork of
CRAC-Strat_2017_V2
by 
CRAC Team
Diff: Strategie/Strategie.cpp
- Revision:
- 14:c8fc06c4887f
- Parent:
- 12:14729d584500
- Child:
- 15:c2fc239e85df
diff -r 93edbb03a8c6 -r c8fc06c4887f Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp Tue Jan 31 16:00:33 2017 +0000
+++ b/Strategie/Strategie.cpp Fri Mar 31 16:20:26 2017 +0000
@@ -1,4 +1,4 @@
-#include "Strategie.h"
+ #include "Strategie.h"
E_stratGameEtat gameEtat = ETAT_CHECK_CARTE_SCREEN;
E_stratGameEtat lastEtat = ETAT_CHECK_CARTE_SCREEN;
@@ -13,6 +13,7 @@
unsigned short waitingAckID = 0;//L'id du ack attendu
unsigned short waitingAckFrom = 0;//La provenance du ack attendu
+char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
@@ -21,8 +22,12 @@
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,CHECK_AX12,CHECK_POMPES};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
+//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};
+
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
+
InterruptIn jack(p25); // entrée analogique en interruption pour le jack
#else
unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
@@ -34,10 +39,13 @@
unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
+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;
+
/****************************************************************************************/
/* FUNCTION NAME: chronometre_ISR */
/* DESCRIPTION : Interruption à la fin des 90s du match */
@@ -80,7 +88,7 @@
signed char localData1 = 0;
signed short localData2 = 0;
unsigned short localData3 = 0;
- signed short localData4 = 0;
+ //signed short localData4 = 0;
unsigned char localData5 = 0;
if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
@@ -109,6 +117,8 @@
cartesCheker.start();//On lance le timer pour le timeout
gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK;
+ //gameEtat = ETAT_GAME_START;
+
break;
case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
/*
@@ -142,6 +152,10 @@
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:
/*
@@ -203,10 +217,12 @@
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();//Mise en cache de toute les instructions
+
gameEtat = ETAT_GAME_WAIT_FOR_JACK;
SendRawId(ECRAN_ACK_START_MATCH);
tactile_printf("Attente du JACK.");
@@ -230,10 +246,13 @@
//On attend le jack
break;
case ETAT_GAME_START:
- chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+
+ if (ModeDemo == 0){
+ chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
+ gameTimer.start();
+ }
gameTimer.reset();
- gameTimer.start();
jack.fall(NULL);//On désactive l'interruption du jack
SendRawId(GLOBAL_START);
tactile_printf("Start");//Pas vraiment util mais bon
@@ -243,6 +262,7 @@
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
@@ -297,6 +317,7 @@
}
localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
GoStraight(localData2, 0, 0, localData5);
+
break;
case MV_TURN: //Rotation sur place
if(instruction.direction == RELATIVE) {
@@ -358,11 +379,13 @@
GoStraight(localData2, localData5, localData3, 0);
break;
case ACTION:
+ SendRawId(0x300);
waitingAckFrom = 0;
waitingAckID = 0;
if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
//L'action est spécifique
if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
+ ///wait_us(200);
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
} else {
@@ -373,6 +396,7 @@
//C'est un AX12 qu'il faut bouger
AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
AX12_enchainement++;
+
}
break;
default:
@@ -401,7 +425,7 @@
/*
Attente de l'ack de l'instruction
*/
- if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu
+ if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu
//if(true) {
cartesCheker.stop();
if(instruction.nextActionType == JUMP) {
@@ -437,8 +461,16 @@
waitingAckFrom = INSTRUCTION_END_MOTEUR;
break;
case ACTION:
- waitingAckFrom = 0;//SERVO_AX12_DONE;
- waitingAckID = 0;//instruction.arg1;
+ SendRawId(0x302);
+ waitingAckFrom = 0; //SERVO_AX12_DONE;
+ waitingAckID = 0;// instruction.arg1;
+ /*if (modeTelemetre == 0){
+
+ }else{ // si on attend la reponse du telemetre
+ waitingAckFrom = 0;//TELEMETRE_RECHERCHE_OBJET;
+ waitingAckID = OBJET_SUR_TABLE;
+ //modeTelemetre = 0;
+ }*/
break;
default:
break;
@@ -505,9 +537,13 @@
- case ETAT_END:
-
- gameEtat = ETAT_END_LOOP;
+ 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
@@ -555,6 +591,7 @@
case ACKNOWLEDGE_BALISE:
case ACKNOWLEDGE_MOTEUR:
case ACKNOWLEDGE_IHM:
+ case ACKNOWLEDGE_TELEMETRE:
case INSTRUCTION_END_BALISE:
case INSTRUCTION_END_MOTEUR:
case INSTRUCTION_END_IHM:
@@ -580,17 +617,25 @@
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));
+ //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);
+ SendAck(0x114, SERVO_AX12_PROCESS);
+ //AX12_processChange(1);
break;
+
case SERVO_AX12_DONE:
- //SendAck(0x114, SERVO_AX12_DONE);
+ SendRawId(0x666);
+ /*//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 ECRAN_CHOICE_COLOR://Choix de la couleur
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
@@ -609,6 +654,11 @@
}
break;
+
+ case TELEMETRE_RECHERCHE_COIN:
+ SendRawId(RESET_TELEMETRE);
+ break;
+
case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur
@@ -617,6 +667,14 @@
msgTx.type=CANData;
if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
// id de la stratégie sur 1 octet
+ if (msgRxBuffer[FIFO_lecture].data[0] < 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;
+ }
+
msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
} else {
//erreur sur 1 octet
@@ -664,6 +722,13 @@
SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
}
break;
+
+ case OBJET_SUR_TABLE:
+ if ((gameEtat == ETAT_GAME_WAIT_ACK)&&(modeTelemetre)){
+ strat_instructions[actual_instruction+1].arg1 = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+ strat_instructions[actual_instruction+1].arg2 = msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
+ }
+ break;
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;