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
Evitement/Evitement.cpp
- Committer:
- goldmas
- Date:
- 2021-07-17
- Revision:
- 37:9d6a3ccc0582
- Parent:
- 21:d137ec53c3a9
File content as of revision 37:9d6a3ccc0582:
/*********************************************\
| Dodge algorithm |
| for CRAC Team 2020 |
| by Gabriel Tetar |
\*********************************************/
// TABLE
/*******************************************************\----> Y 0;3000
| | | |
| JAUNE | | VIOLET |
| | | |
| | | |
| |-------------------------------------| | X 0;2000
| | | |
| | | |
| | | |
| | | |
O*******************************************************/
#include "global.h"
#include <math.h>
#define M_PI 3.14159265358979323846f
/****************************************************************************************/
/* FUNCTION NAME: Balise Danger */
/* DESCRIPTION : FIFO -> BALISE_DANGER */
/****************************************************************************************/
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 */
/****************************************************************************************/
unsigned short balise_stop(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(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 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);
}
return(0);
}