![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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:
- gabrieltetar
- Date:
- 2020-03-03
- Revision:
- 10:1964bb91b925
- Parent:
- 1:7e925468f9d9
- Child:
- 21:d137ec53c3a9
File content as of revision 10:1964bb91b925:
/*********************************************\ | 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 */ /****************************************************************************************/ unsigned short balise_danger(void){ SendSpeed(150); 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; #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); return(0); } /****************************************************************************************/ /* FUNCTION NAME: Balise end Danger */ /* DESCRIPTION : FIFO -> BALISE_END_DANGER */ /****************************************************************************************/ unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, E_stratGameEtat* gameEtat, 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; } SendSpeed(300); return(0); }