CRAC Team / Mbed 2 deprecated code_carte_IHM_avec_sortie_evitement

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@10:1964bb91b925, 2020-03-03 (annotated)

Committer:
gabrieltetar
Date:
Tue Mar 03 16:41:33 2020 +0000
Revision:
10:1964bb91b925
Parent:
1:7e925468f9d9
Child:
21:d137ec53c3a9
restructred demo menu - features added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabrieltetar 1:7e925468f9d9 1 /*********************************************\
gabrieltetar 1:7e925468f9d9 2 | Dodge algorithm |
gabrieltetar 1:7e925468f9d9 3 | for CRAC Team 2020 |
gabrieltetar 1:7e925468f9d9 4 | by Gabriel Tetar |
gabrieltetar 1:7e925468f9d9 5 \*********************************************/
gabrieltetar 1:7e925468f9d9 6
gabrieltetar 1:7e925468f9d9 7 // TABLE
gabrieltetar 1:7e925468f9d9 8 /*******************************************************\----> Y 0;3000
gabrieltetar 1:7e925468f9d9 9 | | | |
gabrieltetar 1:7e925468f9d9 10 | JAUNE | | VIOLET |
gabrieltetar 1:7e925468f9d9 11 | | | |
gabrieltetar 1:7e925468f9d9 12 | | | |
gabrieltetar 1:7e925468f9d9 13 | |-------------------------------------| | X 0;2000
gabrieltetar 1:7e925468f9d9 14 | | | |
gabrieltetar 1:7e925468f9d9 15 | | | |
gabrieltetar 1:7e925468f9d9 16 | | | |
gabrieltetar 1:7e925468f9d9 17 | | | |
gabrieltetar 1:7e925468f9d9 18 O*******************************************************/
gabrieltetar 1:7e925468f9d9 19
gabrieltetar 1:7e925468f9d9 20
gabrieltetar 1:7e925468f9d9 21 #include "global.h"
gabrieltetar 1:7e925468f9d9 22 #include <math.h>
gabrieltetar 1:7e925468f9d9 23 #define M_PI 3.14159265358979323846f
gabrieltetar 1:7e925468f9d9 24 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 25 /* FUNCTION NAME: Balise Danger */
gabrieltetar 1:7e925468f9d9 26 /* DESCRIPTION : FIFO -> BALISE_DANGER */
gabrieltetar 1:7e925468f9d9 27 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 28 unsigned short balise_danger(void){
gabrieltetar 1:7e925468f9d9 29 SendSpeed(150);
gabrieltetar 1:7e925468f9d9 30 return(0);
gabrieltetar 1:7e925468f9d9 31 }
gabrieltetar 1:7e925468f9d9 32 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 33 /* FUNCTION NAME: Balise Stop */
gabrieltetar 1:7e925468f9d9 34 /* DESCRIPTION : FIFO -> BALISE_STOP */
gabrieltetar 1:7e925468f9d9 35 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 36 unsigned short balise_stop(signed char FIFO_lecture){
gabrieltetar 1:7e925468f9d9 37
gabrieltetar 1:7e925468f9d9 38 signed char fin_angle_detection;
gabrieltetar 1:7e925468f9d9 39 signed char debut_angle_detection;
gabrieltetar 1:7e925468f9d9 40 float angle_moyen_balise_IR = 0.0;
gabrieltetar 10:1964bb91b925 41 Debug_Audio(3,2);
gabrieltetar 1:7e925468f9d9 42 //on recupere l'info d'angle de detection--------------------------------------
gabrieltetar 1:7e925468f9d9 43 if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
gabrieltetar 1:7e925468f9d9 44 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
gabrieltetar 1:7e925468f9d9 45 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
gabrieltetar 1:7e925468f9d9 46 } else { //data balise Gros Robot Detecte
gabrieltetar 1:7e925468f9d9 47 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
gabrieltetar 1:7e925468f9d9 48 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
gabrieltetar 1:7e925468f9d9 49 }
gabrieltetar 1:7e925468f9d9 50 //on moyenne l'angle------------------------------------------------------------
gabrieltetar 1:7e925468f9d9 51 if(debut_angle_detection > fin_angle_detection) {
gabrieltetar 1:7e925468f9d9 52 angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
gabrieltetar 1:7e925468f9d9 53 if(angle_moyen_balise_IR > 15.0f)
gabrieltetar 1:7e925468f9d9 54 angle_moyen_balise_IR-=15.0f;
gabrieltetar 1:7e925468f9d9 55 } else
gabrieltetar 1:7e925468f9d9 56 angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
gabrieltetar 1:7e925468f9d9 57 #ifdef ROBOT_BIG
gabrieltetar 1:7e925468f9d9 58 float seuil_bas_avant = 12.0;
gabrieltetar 1:7e925468f9d9 59 float seuil_haut_avant = 15.0;
gabrieltetar 1:7e925468f9d9 60 float seuil_bas_arriere = 5.0;
gabrieltetar 1:7e925468f9d9 61 float seuil_haut_arriere = 7.0;
gabrieltetar 1:7e925468f9d9 62 #else
gabrieltetar 1:7e925468f9d9 63 float seuil_bas_arriere = 12.0;
gabrieltetar 1:7e925468f9d9 64 float seuil_haut_arriere = 15.0;
gabrieltetar 1:7e925468f9d9 65 float seuil_bas_avant = 4.0;
gabrieltetar 1:7e925468f9d9 66 float seuil_haut_avant = 7.0;
gabrieltetar 1:7e925468f9d9 67 #endif
gabrieltetar 1:7e925468f9d9 68 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))
gabrieltetar 1:7e925468f9d9 69 SendRawId(ASSERVISSEMENT_STOP);
gabrieltetar 1:7e925468f9d9 70 return(0);
gabrieltetar 1:7e925468f9d9 71 }
gabrieltetar 1:7e925468f9d9 72 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 73 /* FUNCTION NAME: Balise end Danger */
gabrieltetar 1:7e925468f9d9 74 /* DESCRIPTION : FIFO -> BALISE_END_DANGER */
gabrieltetar 1:7e925468f9d9 75 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 76 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){
gabrieltetar 10:1964bb91b925 77 Debug_Audio(3,1);
gabrieltetar 1:7e925468f9d9 78 switch(instruction->order){
gabrieltetar 1:7e925468f9d9 79 case MV_LINE:
gabrieltetar 1:7e925468f9d9 80 *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 81 instruction->order = MV_XYT;
gabrieltetar 1:7e925468f9d9 82 instruction->arg1 = target_x_robot;// X
gabrieltetar 1:7e925468f9d9 83 instruction->arg2 = target_y_robot;// Y
gabrieltetar 1:7e925468f9d9 84 instruction->arg3 = target_theta_robot;// T
gabrieltetar 1:7e925468f9d9 85 break;
gabrieltetar 1:7e925468f9d9 86 case MV_TURN:
gabrieltetar 1:7e925468f9d9 87 *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 88 instruction->order = MV_XYT;
gabrieltetar 1:7e925468f9d9 89 instruction->arg1 = target_x_robot;// X
gabrieltetar 1:7e925468f9d9 90 instruction->arg2 = target_y_robot;// Y
gabrieltetar 1:7e925468f9d9 91 instruction->arg3 = target_theta_robot;// T
gabrieltetar 1:7e925468f9d9 92 break;
gabrieltetar 1:7e925468f9d9 93 case MV_XYT:
gabrieltetar 1:7e925468f9d9 94 *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 95 break;
gabrieltetar 1:7e925468f9d9 96 case MV_COURBURE:
gabrieltetar 1:7e925468f9d9 97 unsigned short alpha;
gabrieltetar 1:7e925468f9d9 98 *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 99 instruction->order=MV_XYT;
gabrieltetar 1:7e925468f9d9 100 if(instruction->direction==LEFT) alpha=(dodgeq->inst[0].arg3-theta_robot);
gabrieltetar 1:7e925468f9d9 101 else alpha=(theta_robot-dodgeq->inst[0].arg3);
gabrieltetar 1:7e925468f9d9 102 if(alpha<450){
gabrieltetar 1:7e925468f9d9 103 dodgeq->nb=0;
gabrieltetar 1:7e925468f9d9 104 instruction->arg1=dodgeq->inst[0].arg1;//x
gabrieltetar 1:7e925468f9d9 105 instruction->arg2=dodgeq->inst[0].arg2;//y
gabrieltetar 1:7e925468f9d9 106 instruction->arg3=dodgeq->inst[0].arg3;//t
gabrieltetar 1:7e925468f9d9 107 } else if(alpha<900){
gabrieltetar 1:7e925468f9d9 108 dodgeq->nb=1;
gabrieltetar 1:7e925468f9d9 109 instruction->arg1=dodgeq->inst[1].arg1;//x
gabrieltetar 1:7e925468f9d9 110 instruction->arg2=dodgeq->inst[1].arg2;//y
gabrieltetar 1:7e925468f9d9 111 instruction->arg3=dodgeq->inst[1].arg3;//t
gabrieltetar 1:7e925468f9d9 112 } else if(alpha<1350){
gabrieltetar 1:7e925468f9d9 113 dodgeq->nb=2;
gabrieltetar 1:7e925468f9d9 114 instruction->arg1=dodgeq->inst[2].arg1;//x
gabrieltetar 1:7e925468f9d9 115 instruction->arg2=dodgeq->inst[2].arg2;//y
gabrieltetar 1:7e925468f9d9 116 instruction->arg3=dodgeq->inst[2].arg3;//t
gabrieltetar 1:7e925468f9d9 117 } else if(alpha<1800){
gabrieltetar 1:7e925468f9d9 118 dodgeq->nb=3;
gabrieltetar 1:7e925468f9d9 119 instruction->arg1=dodgeq->inst[3].arg1;//x
gabrieltetar 1:7e925468f9d9 120 instruction->arg2=dodgeq->inst[3].arg2;//y
gabrieltetar 1:7e925468f9d9 121 instruction->arg3=dodgeq->inst[3].arg3;//t
gabrieltetar 1:7e925468f9d9 122 } else if(alpha<2250){
gabrieltetar 1:7e925468f9d9 123 dodgeq->nb=4;
gabrieltetar 1:7e925468f9d9 124 instruction->arg1=dodgeq->inst[4].arg1;//x
gabrieltetar 1:7e925468f9d9 125 instruction->arg2=dodgeq->inst[4].arg2;//y
gabrieltetar 1:7e925468f9d9 126 instruction->arg3=dodgeq->inst[4].arg3;//t
gabrieltetar 1:7e925468f9d9 127 } else {
gabrieltetar 1:7e925468f9d9 128 dodgeq->nb=5;
gabrieltetar 1:7e925468f9d9 129 instruction->arg1=dodgeq->inst[5].arg1;//x
gabrieltetar 1:7e925468f9d9 130 instruction->arg2=dodgeq->inst[5].arg2;//y
gabrieltetar 1:7e925468f9d9 131 instruction->arg3=dodgeq->inst[5].arg3;//t
gabrieltetar 1:7e925468f9d9 132 }
gabrieltetar 1:7e925468f9d9 133 break;
gabrieltetar 1:7e925468f9d9 134 }
gabrieltetar 1:7e925468f9d9 135 SendSpeed(300);
gabrieltetar 1:7e925468f9d9 136 return(0);
gabrieltetar 1:7e925468f9d9 137 }