CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 89:d8f8f4e12d5c
- Parent:
- 88:37714217b4c4
- Child:
- 90:2a3e2dca09a0
--- a/Strategie/Strategie.cpp Sat Jun 01 00:31:42 2019 +0000
+++ b/Strategie/Strategie.cpp Sat Jun 01 00:55:03 2019 +0000
@@ -74,7 +74,7 @@
short SCORE_PR=0;
int flag = 0, flag_strat = 0, flag_timer;
-int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
+int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
char Ack_strat = 0;
signed char Strat = 0;
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
@@ -1149,13 +1149,13 @@
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);
#ifdef ROBOT_SMALL
if(localData2>0) {
@@ -1171,28 +1171,25 @@
}
#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;*/
-
+ /*
+ //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(InversStrat == 0 || ingnorInversionOnce == 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 );
- }
+ 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
+ else
{
if(instruction.direction == LEFT)
{
@@ -1208,13 +1205,13 @@
}
}
*/
- //target_theta_robot = theta_robot + localData1*localData2;
-
+ //target_theta_robot = theta_robot + localData1*localData2;
+
break;
-
-
-
+
+
+
case MV_LINE://Ligne droite
waitingAckID = ASSERVISSEMENT_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -1555,15 +1552,15 @@
case ETAT_TELEMETRE_BALANCE:
- /* SendRawId(ASSERVISSEMENT_STOP);
- waitingAckID_FIN = ASSERVISSEMENT_STOP;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
- canProcessRx();*/
+ /* 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();
+ 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)) {
@@ -1656,21 +1653,18 @@
ingnorBaliseOnce = 0;
ingnorBalise = 0;
short new_theta_courbe;
-
+
if(instruction.direction == LEFT) {
new_theta_courbe = target_theta_robot - theta_robot;
} else {
new_theta_courbe = theta_robot - target_theta_robot;
}
-
-
- if(instruction.arg3 >= 0)
- {
+
+
+ if(instruction.arg3 >= 0) {
if(new_theta_courbe<0)new_theta_courbe+=3600;
else if(new_theta_courbe>instruction.arg3)new_theta_courbe-=3600;
- }
- else if(instruction.arg3 < 0)
- {
+ } else if(instruction.arg3 < 0) {
if(new_theta_courbe>0)new_theta_courbe-=3600;
else if(new_theta_courbe<instruction.arg3)new_theta_courbe+=3600;
}
@@ -1693,20 +1687,20 @@
if(instruction.direction == LEFT) instruction.direction = RIGHT;
else instruction.direction = LEFT;
}
-/*
-
- 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);
-*/
+ /*
+
+ 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;
@@ -2118,18 +2112,15 @@
}
flag=1;
break;
-
+
case ASSERVISSEMENT_ERROR_MOTEUR://erreur asservissement
- if(flagNonRepriseErrorMot)
- {
+ if(flagNonRepriseErrorMot) {
actual_instruction = instruction.nextLineError;
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
flagNonRepriseErrorMot = 0;
- }
- else
- {
+ } else {
flagNonRepriseErrorMot = 1;
- gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+ gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
}
break;
@@ -2154,7 +2145,7 @@
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 ) {
@@ -2221,9 +2212,20 @@
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;
+ */
+ signed char fin_angle_detection;
+ signed char debut_angle_detection;
+
+ if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise PR
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
+ } else { //data balise GR
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
+ }
if(debut_angle_detection > fin_angle_detection) {
angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
@@ -2254,20 +2256,19 @@
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,
+ || 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) ) {
+ } else if( (gameEtat > ETAT_GAME_START) && (gameEtat != ETAT_WARNING_TIMEOUT) ) {
SendRawId(ASSERVISSEMENT_STOP);
timeoutWarning.reset();
timeoutWarning.start();//Reset du timer utiliser par le timeout
@@ -2276,17 +2277,17 @@
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
- || 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;
- // }
+ /* 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;
@@ -2309,7 +2310,8 @@
break;
case RECEPTION_RECALAGE:
- wait_us(150);flagReceptionTelemetres = 1;
+ 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]);