CRAC Team
code avec la sortie d'évitement en plus géré par un ticker, ce code et le code "avec_modifs" buggent en match avec seulement la stratégie agressive.
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Evitement/Evitement.cpp
- Revision:
- 37:9d6a3ccc0582
- Parent:
- 21:d137ec53c3a9
--- a/Evitement/Evitement.cpp Mon May 31 13:36:03 2021 +0000
+++ b/Evitement/Evitement.cpp Sat Jul 17 11:07:17 2021 +0000
@@ -25,10 +25,56 @@
/* FUNCTION NAME: Balise Danger */
/* DESCRIPTION : FIFO -> BALISE_DANGER */
/****************************************************************************************/
-unsigned short balise_danger(void){
- SendSpeed(150);
- return(0);
+
+int Flag_stoped_danger = 0;
+
+Ticker ticker_timeout_evitement;
+
+unsigned short balise_danger(signed char FIFO_lecture)
+{
+ /*signed char fin_angle_detection;
+ signed char debut_angle_detection;
+ float angle_moyen_balise_IR = 0.0;
+// Debug_Audio(3,2);
+ //on recupere l'info d'angle de detection--------------------------------------
+ if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
+ } else { //data balise Gros Robot Detecte
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
+ }
+ //on moyenne l'angle------------------------------------------------------------
+ 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;
+ if(angle_moyen_balise_IR > 15.0f)
+ angle_moyen_balise_IR-=15.0f;
+ } else
+ angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
+ char seuil_bas_arriere = 5;
+ char seuil_haut_arriere = 7;
+ char seuil_bas_avant = 0xF;
+ char seuil_haut_avant = 0xD;
+ // #endif
+ // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
+ if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ }
+ return(0);*/
}
+
/****************************************************************************************/
/* FUNCTION NAME: Balise Stop */
/* DESCRIPTION : FIFO -> BALISE_STOP */
@@ -38,9 +84,9 @@
signed char fin_angle_detection;
signed char debut_angle_detection;
float angle_moyen_balise_IR = 0.0;
- Debug_Audio(3,2);
+// Debug_Audio(3,2);
//on recupere l'info d'angle de detection--------------------------------------
- if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+ if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
} else { //data balise Gros Robot Detecte
@@ -54,84 +100,155 @@
angle_moyen_balise_IR-=15.0f;
} else
angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
- #ifdef ROBOT_BIG
- float seuil_bas_avant = 12.0;
- float seuil_haut_avant = 15.0;
- float seuil_bas_arriere = 5.0;
- float seuil_haut_arriere = 7.0;
- #else
- float seuil_bas_arriere = 12.0;
- float seuil_haut_arriere = 15.0;
- float seuil_bas_avant = 4.0;
- float seuil_haut_avant = 7.0;
- #endif
- if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
- SendRawId(ASSERVISSEMENT_STOP);
+ char seuil_bas_arriere = 5;
+ char seuil_haut_arriere = 7;
+ char seuil_bas_avant = 0xF;
+ char seuil_haut_avant = 0xD;
+ // #endif
+ // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
+ if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+ {
+ if(Cote == 1)//Jaune
+ {
+ if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {}
+ else
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ Flag_stoped_danger = 1;
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ Flag_stoped_danger = 1;
+ }
+ }
+ }
+
+ else if(Cote == 0)//Bleu
+ {
+ if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {}
+ else
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ Flag_stoped_danger = 1;
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ Flag_stoped_danger = 1;
+ }
+ }
+ }
+ if(Flag_stoped_danger)
+ {
+ ticker_timeout_evitement.detach();
+ ticker_timeout_evitement.attach(&isr_end_danger, 2);
+ }
+ }
return(0);
}
/****************************************************************************************/
/* FUNCTION NAME: Balise end Danger */
/* DESCRIPTION : FIFO -> BALISE_END_DANGER */
/****************************************************************************************/
-unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot){
- Debug_Audio(3,1);
- switch(instruction->order){
- case MV_LINE:
- gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
- instruction->order = MV_XYT;
- instruction->arg1 = target_x_robot;// X
- instruction->arg2 = target_y_robot;// Y
- instruction->arg3 = target_theta_robot;// T
- break;
- case MV_TURN:
- gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
- instruction->order = MV_XYT;
- instruction->arg1 = target_x_robot;// X
- instruction->arg2 = target_y_robot;// Y
- instruction->arg3 = target_theta_robot;// T
- break;
- case MV_XYT:
- gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
- break;
- case MV_COURBURE:
- unsigned short alpha;
- gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
- instruction->order=MV_XYT;
- if(instruction->direction==LEFT) alpha=(dodgeq->inst[0].arg3-theta_robot);
- else alpha=(theta_robot-dodgeq->inst[0].arg3);
- if(alpha<450){
- dodgeq->nb=0;
- instruction->arg1=dodgeq->inst[0].arg1;//x
- instruction->arg2=dodgeq->inst[0].arg2;//y
- instruction->arg3=dodgeq->inst[0].arg3;//t
- } else if(alpha<900){
- dodgeq->nb=1;
- instruction->arg1=dodgeq->inst[1].arg1;//x
- instruction->arg2=dodgeq->inst[1].arg2;//y
- instruction->arg3=dodgeq->inst[1].arg3;//t
- } else if(alpha<1350){
- dodgeq->nb=2;
- instruction->arg1=dodgeq->inst[2].arg1;//x
- instruction->arg2=dodgeq->inst[2].arg2;//y
- instruction->arg3=dodgeq->inst[2].arg3;//t
- } else if(alpha<1800){
- dodgeq->nb=3;
- instruction->arg1=dodgeq->inst[3].arg1;//x
- instruction->arg2=dodgeq->inst[3].arg2;//y
- instruction->arg3=dodgeq->inst[3].arg3;//t
- } else if(alpha<2250){
- dodgeq->nb=4;
- instruction->arg1=dodgeq->inst[4].arg1;//x
- instruction->arg2=dodgeq->inst[4].arg2;//y
- instruction->arg3=dodgeq->inst[4].arg3;//t
- } else {
- dodgeq->nb=5;
- instruction->arg1=dodgeq->inst[5].arg1;//x
- instruction->arg2=dodgeq->inst[5].arg2;//y
- instruction->arg3=dodgeq->inst[5].arg3;//t
- }
- break;
+unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short local_target_x_robot, signed short local_target_y_robot, signed short local_target_theta_robot, signed short local_theta_robot,signed short x_robot,signed short y_robot){
+
+ ticker_timeout_evitement.detach();
+ if(Flag_stoped_danger)
+ {
+
+ ingnorInversionOnce=1;
+ wait_ms(1000);
+ switch(instruction->order){
+ case MV_RECALAGE:
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ Flag_stoped_danger = 0;
+ break;
+
+ case MV_LINE:
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order = MV_XYT;
+ instruction->arg1 = local_target_x_robot;// X
+ instruction->arg2 = local_target_y_robot;// Y
+ instruction->arg3 = local_target_theta_robot;// T
+ Flag_stoped_danger = 0;
+ break;
+ case MV_TURN:
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order = MV_XYT;
+ instruction->arg1 = local_target_x_robot;// X
+ instruction->arg2 = local_target_y_robot;// Y
+ instruction->arg3 = local_target_theta_robot;// T
+ Flag_stoped_danger = 0;
+ break;
+ case MV_XYT:
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ Flag_stoped_danger = 0;
+ break;
+ case MV_COURBURE:
+ short alpha;
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order=MV_XYT;
+ if(instruction->direction==LEFT)
+ alpha=(dodgeq->inst[0].arg3-theta_robot);
+ else
+ alpha=(theta_robot-dodgeq->inst[0].arg3);
+ if(alpha>3600)
+ alpha=alpha-3600;
+ if(alpha<-3600)
+ alpha=alpha+3600;
+ if(alpha<0)
+ alpha=-alpha;
+ if(alpha<450)
+ {
+ dodgeq->nb=0;
+ instruction->arg1=dodgeq->inst[0].arg1;//x
+ instruction->arg2=dodgeq->inst[0].arg2;//y
+ instruction->arg3=dodgeq->inst[0].arg3;//t
+ }
+ else if(alpha<900)
+ {
+ dodgeq->nb=1;
+ instruction->arg1=dodgeq->inst[1].arg1;//x
+ instruction->arg2=dodgeq->inst[1].arg2;//y
+ instruction->arg3=dodgeq->inst[1].arg3;//t
+ } else if(alpha<1350){
+ dodgeq->nb=2;
+ instruction->arg1=dodgeq->inst[2].arg1;//x
+ instruction->arg2=dodgeq->inst[2].arg2;//y
+ instruction->arg3=dodgeq->inst[2].arg3;//t
+ } else if(alpha<1800){
+ dodgeq->nb=3;
+ instruction->arg1=dodgeq->inst[3].arg1;//x
+ instruction->arg2=dodgeq->inst[3].arg2;//y
+ instruction->arg3=dodgeq->inst[3].arg3;//t
+ } else if(alpha<2250){
+ dodgeq->nb=4;
+ instruction->arg1=dodgeq->inst[4].arg1;//x
+ instruction->arg2=dodgeq->inst[4].arg2;//y
+ instruction->arg3=dodgeq->inst[4].arg3;//t
+ } else {
+ dodgeq->nb=5;
+ instruction->arg1=dodgeq->inst[5].arg1;//x
+ instruction->arg2=dodgeq->inst[5].arg2;//y
+ instruction->arg3=dodgeq->inst[5].arg3;//t
+ }
+ Flag_stoped_danger = 0;
+ break;
}
- SendSpeed(300);
+// SendSpeed(300);
+ }
return(0);
}