CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 83:23e04b85ae06
- Parent:
- 82:759ceb2a8a70
- Child:
- 84:44d6cd2cab99
--- a/Strategie/Strategie.cpp Thu May 30 11:50:05 2019 +0000
+++ b/Strategie/Strategie.cpp Fri May 31 05:02:55 2019 +0000
@@ -74,6 +74,7 @@
short SCORE_PR=0;
int flag = 0, flag_strat = 0, flag_timer;
+int flagReceptionTelemetres = 0;
char Ack_strat = 0;
signed char Strat = 0;
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
@@ -84,7 +85,7 @@
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 char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
@@ -120,6 +121,7 @@
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 memGameEtat= gameEtat;
E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
E_Stratposdebut etat_pos=RECALAGE_1;
@@ -1142,6 +1144,11 @@
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) {
@@ -1161,19 +1168,51 @@
direction=1;
}
#endif
+
+/*
//target_theta_robot = localData2 - theta_robot;
target_theta_robot = (localData2 - theta_robot)%3600;
if(target_theta_robot > 1800) target_theta_robot = target_theta_robot-3600;
- else if(target_theta_robot <-1800) target_theta_robot = target_theta_robot+3600;
- /*
- if(instruction.direction == LEFT){
-
- }else{
- target_theta_robot = theta_robot + localData2;
- }*/
+ else if(target_theta_robot <-1800) target_theta_robot = target_theta_robot+3600;*/
+
+ if(InversStrat == 0 || ingnorInversionOnce == 1 || 1) {
+ if(instruction.direction == LEFT)
+ {
+ target_theta_robot = theta_robot + localData2;
+ target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
+ target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
+ }
+ else
+ {
+ target_theta_robot = theta_robot - localData2;
+ target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
+ target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
+ }
+ }
+ else
+ {
+ if(instruction.direction == LEFT)
+ {
+ target_theta_robot = -(theta_robot + localData2 - 1800);//-theta_robot - localData2;
+ target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
+ target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90-localData2/2) * M_PI / 180);
+ }
+ else
+ {
+ target_theta_robot = -(theta_robot - localData2 + 1800);//-theta_robot + localData2;
+ target_x_robot = x_robot + cos( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
+ target_y_robot = y_robot + sin( (double)(90-theta_robot) * M_PI / 180 ) * 2 * instruction.arg1 * cos( (double)(90+localData2/2) * M_PI / 180 );
+ }
+ }
+
+ //target_theta_robot = theta_robot + localData1*localData2;
+
break;
+
+
+
case MV_LINE://Ligne droite
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -1513,7 +1552,55 @@
break;
-
+ case ETAT_TELEMETRE_BALANCE:
+ /* SendRawId(ASSERVISSEMENT_STOP);
+ waitingAckID_FIN = ASSERVISSEMENT_STOP;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();*/
+ ingnorBaliseOnce=1;
+ SendRawId(DATA_RECALAGE);
+ wait_us(150);
+ canProcessRx();
+ if( ((instruction.order == MV_LINE) && (instruction.direction == FORWARD)) || ((instruction.order == MV_XYT) && (instruction.direction == FORWARD)) ) {
+ if(Cote==1) { //violet
+ if((telemetreDistance_avant_droite+100)>(y_robot-1500)) {
+ gameEtat=memGameEtat;
+ } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
+ gameEtat = ETAT_WARNING_TIMEOUT;
+ }
+ } else {
+ if((telemetreDistance_avant_gauche+100)>(1500-y_robot)) {
+ gameEtat=memGameEtat;
+ } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
+ gameEtat = ETAT_WARNING_TIMEOUT;
+ }
+ }
+ }
+ if( ((instruction.order == MV_LINE) && (instruction.direction == BACKWARD)) || ((instruction.order == MV_XYT) && (instruction.direction == BACKWARD)) ) {
+ if(Cote==1) {
+ if((telemetreDistance_arriere_droite-100)>(1500-y_robot)) {
+ gameEtat=memGameEtat;
+ } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
+ gameEtat = ETAT_WARNING_TIMEOUT;
+ }
+ } else {
+ if((telemetreDistance_arriere_gauche-100)>(1500-y_robot)) {
+ gameEtat=memGameEtat;
+ } else if((gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT)) {
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
+ gameEtat = ETAT_WARNING_TIMEOUT;
+ }
+ }
+ }
+ 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
@@ -1570,20 +1657,38 @@
if(instruction.direction == LEFT) {
target_theta_robot = target_theta_robot - theta_robot;
} else {
- target_theta_robot = theta_robot + target_theta_robot;
+ target_theta_robot = theta_robot - target_theta_robot;
}
-
- target_theta_robot = (target_theta_robot)%3600;
+
+ if(InversStrat == 1) {
+ 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;
+/*
+
+ 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
+ GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
+*/
+
break;
default:
@@ -2008,9 +2113,16 @@
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;
- SendMsgCan(0x666,&ingnorBalise,1);
+
+//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;
@@ -2108,43 +2220,39 @@
if (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)Send2Short(0x667,1,1);
#endif
if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
- || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
- || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
- || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
-
- if( (needToStop() != 0) && (ingnorBaliseOnce ==0) && (ingnorBalise==0) ) {
-
- if( (gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT) ) {
+ || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
+ || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
+ || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere )
+ { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
+
+ if( (needToStop() !=0) && (ingnorBaliseOnce ==0) && (ingnorBalise==0) ) {
+
+ if( needToStop() == 3) {//on regarde les telemetres
+ memGameEtat = gameEtat;
+ gameEtat = ETAT_TELEMETRE_BALANCE;
+ }
+ else if( (gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT) ) {
SendRawId(ASSERVISSEMENT_STOP);
- /*waitingAckID_FIN = ASSERVISSEMENT_STOP;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
- canProcessRx();*/
- //while(1); // ligne à décommenter si on est en homologation 2018
-
- if(gameEtat != ETAT_WARING_END_BALISE_WAIT || 1) {
- timeoutWarning.reset();
- timeoutWarning.start();//Reset du timer utiliser par le timeout
- }
+ timeoutWarning.reset();
+ timeoutWarning.start();//Reset du timer utiliser par le timeout
gameEtat = ETAT_WARNING_TIMEOUT;
}
+ ingnorBaliseOnce = 0;
}
- ingnorBaliseOnce = 0;
-
} else if(gameEtat==ETAT_WARNING_TIMEOUT) {
- if (!(instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+ /* if (!(instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
|| instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
|| instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
|| instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
|| instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
|| instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere) ) {
-
+*/
timeoutWarningWaitEnd.reset();
timeoutWarningWaitEnd.start();
gameEtat = ETAT_WARING_END_BALISE_WAIT;
- }
+ // }
}
break;
@@ -2159,7 +2267,6 @@
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;
@@ -2168,7 +2275,7 @@
break;
case RECEPTION_RECALAGE:
- wait_us(150);
+ 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]);