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@37:9d6a3ccc0582, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 11:07:17 2021 +0000
Revision:
37:9d6a3ccc0582
Parent:
21:d137ec53c3a9 
Dernier code de la coupe de fracne 2021

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 /****************************************************************************************/
goldmas 37:9d6a3ccc0582 28
goldmas 37:9d6a3ccc0582 29 int Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 30
goldmas 37:9d6a3ccc0582 31 Ticker ticker_timeout_evitement;
goldmas 37:9d6a3ccc0582 32
goldmas 37:9d6a3ccc0582 33 unsigned short balise_danger(signed char FIFO_lecture)
goldmas 37:9d6a3ccc0582 34 {
goldmas 37:9d6a3ccc0582 35 /*signed char fin_angle_detection;
goldmas 37:9d6a3ccc0582 36 signed char debut_angle_detection;
goldmas 37:9d6a3ccc0582 37 float angle_moyen_balise_IR = 0.0;
goldmas 37:9d6a3ccc0582 38 // Debug_Audio(3,2);
goldmas 37:9d6a3ccc0582 39 //on recupere l'info d'angle de detection--------------------------------------
goldmas 37:9d6a3ccc0582 40 if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
goldmas 37:9d6a3ccc0582 41 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
goldmas 37:9d6a3ccc0582 42 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
goldmas 37:9d6a3ccc0582 43 } else { //data balise Gros Robot Detecte
goldmas 37:9d6a3ccc0582 44 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
goldmas 37:9d6a3ccc0582 45 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
goldmas 37:9d6a3ccc0582 46 }
goldmas 37:9d6a3ccc0582 47 //on moyenne l'angle------------------------------------------------------------
goldmas 37:9d6a3ccc0582 48 if(debut_angle_detection > fin_angle_detection) {
goldmas 37:9d6a3ccc0582 49 angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
goldmas 37:9d6a3ccc0582 50 if(angle_moyen_balise_IR > 15.0f)
goldmas 37:9d6a3ccc0582 51 angle_moyen_balise_IR-=15.0f;
goldmas 37:9d6a3ccc0582 52 } else
goldmas 37:9d6a3ccc0582 53 angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
goldmas 37:9d6a3ccc0582 54 char seuil_bas_arriere = 5;
goldmas 37:9d6a3ccc0582 55 char seuil_haut_arriere = 7;
goldmas 37:9d6a3ccc0582 56 char seuil_bas_avant = 0xF;
goldmas 37:9d6a3ccc0582 57 char seuil_haut_avant = 0xD;
goldmas 37:9d6a3ccc0582 58 // #endif
goldmas 37:9d6a3ccc0582 59 // 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))
goldmas 37:9d6a3ccc0582 60 if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
goldmas 37:9d6a3ccc0582 61 {
goldmas 37:9d6a3ccc0582 62 if(asser_stop_direction==1)
goldmas 37:9d6a3ccc0582 63 {
goldmas 37:9d6a3ccc0582 64 if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
goldmas 37:9d6a3ccc0582 65 ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
goldmas 37:9d6a3ccc0582 66 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 67 }
goldmas 37:9d6a3ccc0582 68 else
goldmas 37:9d6a3ccc0582 69 {
goldmas 37:9d6a3ccc0582 70 if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
goldmas 37:9d6a3ccc0582 71 ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
goldmas 37:9d6a3ccc0582 72 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 73 }
goldmas 37:9d6a3ccc0582 74 }
goldmas 37:9d6a3ccc0582 75 return(0);*/
gabrieltetar 1:7e925468f9d9 76 }
goldmas 37:9d6a3ccc0582 77
gabrieltetar 1:7e925468f9d9 78 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 79 /* FUNCTION NAME: Balise Stop */
gabrieltetar 1:7e925468f9d9 80 /* DESCRIPTION : FIFO -> BALISE_STOP */
gabrieltetar 1:7e925468f9d9 81 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 82 unsigned short balise_stop(signed char FIFO_lecture){
gabrieltetar 1:7e925468f9d9 83
gabrieltetar 1:7e925468f9d9 84 signed char fin_angle_detection;
gabrieltetar 1:7e925468f9d9 85 signed char debut_angle_detection;
gabrieltetar 1:7e925468f9d9 86 float angle_moyen_balise_IR = 0.0;
goldmas 37:9d6a3ccc0582 87 // Debug_Audio(3,2);
gabrieltetar 1:7e925468f9d9 88 //on recupere l'info d'angle de detection--------------------------------------
goldmas 37:9d6a3ccc0582 89 if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
gabrieltetar 1:7e925468f9d9 90 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
gabrieltetar 1:7e925468f9d9 91 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
gabrieltetar 1:7e925468f9d9 92 } else { //data balise Gros Robot Detecte
gabrieltetar 1:7e925468f9d9 93 fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
gabrieltetar 1:7e925468f9d9 94 debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
gabrieltetar 1:7e925468f9d9 95 }
gabrieltetar 1:7e925468f9d9 96 //on moyenne l'angle------------------------------------------------------------
gabrieltetar 1:7e925468f9d9 97 if(debut_angle_detection > fin_angle_detection) {
gabrieltetar 1:7e925468f9d9 98 angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
gabrieltetar 1:7e925468f9d9 99 if(angle_moyen_balise_IR > 15.0f)
gabrieltetar 1:7e925468f9d9 100 angle_moyen_balise_IR-=15.0f;
gabrieltetar 1:7e925468f9d9 101 } else
gabrieltetar 1:7e925468f9d9 102 angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
goldmas 37:9d6a3ccc0582 103 char seuil_bas_arriere = 5;
goldmas 37:9d6a3ccc0582 104 char seuil_haut_arriere = 7;
goldmas 37:9d6a3ccc0582 105 char seuil_bas_avant = 0xF;
goldmas 37:9d6a3ccc0582 106 char seuil_haut_avant = 0xD;
goldmas 37:9d6a3ccc0582 107 // #endif
goldmas 37:9d6a3ccc0582 108 // 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))
goldmas 37:9d6a3ccc0582 109 if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
goldmas 37:9d6a3ccc0582 110 {
goldmas 37:9d6a3ccc0582 111 if(Cote == 1)//Jaune
goldmas 37:9d6a3ccc0582 112 {
goldmas 37:9d6a3ccc0582 113 if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {}
goldmas 37:9d6a3ccc0582 114 else
goldmas 37:9d6a3ccc0582 115 {
goldmas 37:9d6a3ccc0582 116 if(asser_stop_direction==1)
goldmas 37:9d6a3ccc0582 117 {
goldmas 37:9d6a3ccc0582 118 if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
goldmas 37:9d6a3ccc0582 119 ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
goldmas 37:9d6a3ccc0582 120 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 121 Flag_stoped_danger = 1;
goldmas 37:9d6a3ccc0582 122 }
goldmas 37:9d6a3ccc0582 123 else
goldmas 37:9d6a3ccc0582 124 {
goldmas 37:9d6a3ccc0582 125 if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
goldmas 37:9d6a3ccc0582 126 ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
goldmas 37:9d6a3ccc0582 127 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 128 Flag_stoped_danger = 1;
goldmas 37:9d6a3ccc0582 129 }
goldmas 37:9d6a3ccc0582 130 }
goldmas 37:9d6a3ccc0582 131 }
goldmas 37:9d6a3ccc0582 132
goldmas 37:9d6a3ccc0582 133 else if(Cote == 0)//Bleu
goldmas 37:9d6a3ccc0582 134 {
goldmas 37:9d6a3ccc0582 135 if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {}
goldmas 37:9d6a3ccc0582 136 else
goldmas 37:9d6a3ccc0582 137 {
goldmas 37:9d6a3ccc0582 138 if(asser_stop_direction==1)
goldmas 37:9d6a3ccc0582 139 {
goldmas 37:9d6a3ccc0582 140 if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
goldmas 37:9d6a3ccc0582 141 ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
goldmas 37:9d6a3ccc0582 142 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 143 Flag_stoped_danger = 1;
goldmas 37:9d6a3ccc0582 144 }
goldmas 37:9d6a3ccc0582 145 else
goldmas 37:9d6a3ccc0582 146 {
goldmas 37:9d6a3ccc0582 147 if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
goldmas 37:9d6a3ccc0582 148 ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
goldmas 37:9d6a3ccc0582 149 SendRawId(ASSERVISSEMENT_STOP);
goldmas 37:9d6a3ccc0582 150 Flag_stoped_danger = 1;
goldmas 37:9d6a3ccc0582 151 }
goldmas 37:9d6a3ccc0582 152 }
goldmas 37:9d6a3ccc0582 153 }
goldmas 37:9d6a3ccc0582 154 if(Flag_stoped_danger)
goldmas 37:9d6a3ccc0582 155 {
goldmas 37:9d6a3ccc0582 156 ticker_timeout_evitement.detach();
goldmas 37:9d6a3ccc0582 157 ticker_timeout_evitement.attach(&isr_end_danger, 2);
goldmas 37:9d6a3ccc0582 158 }
goldmas 37:9d6a3ccc0582 159 }
gabrieltetar 1:7e925468f9d9 160 return(0);
gabrieltetar 1:7e925468f9d9 161 }
gabrieltetar 1:7e925468f9d9 162 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 163 /* FUNCTION NAME: Balise end Danger */
gabrieltetar 1:7e925468f9d9 164 /* DESCRIPTION : FIFO -> BALISE_END_DANGER */
gabrieltetar 1:7e925468f9d9 165 /****************************************************************************************/
goldmas 37:9d6a3ccc0582 166 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){
goldmas 37:9d6a3ccc0582 167
goldmas 37:9d6a3ccc0582 168 ticker_timeout_evitement.detach();
goldmas 37:9d6a3ccc0582 169 if(Flag_stoped_danger)
goldmas 37:9d6a3ccc0582 170 {
goldmas 37:9d6a3ccc0582 171
goldmas 37:9d6a3ccc0582 172 ingnorInversionOnce=1;
goldmas 37:9d6a3ccc0582 173 wait_ms(1000);
goldmas 37:9d6a3ccc0582 174 switch(instruction->order){
goldmas 37:9d6a3ccc0582 175 case MV_RECALAGE:
goldmas 37:9d6a3ccc0582 176 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
goldmas 37:9d6a3ccc0582 177 Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 178 break;
goldmas 37:9d6a3ccc0582 179
goldmas 37:9d6a3ccc0582 180 case MV_LINE:
goldmas 37:9d6a3ccc0582 181 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
goldmas 37:9d6a3ccc0582 182 instruction->order = MV_XYT;
goldmas 37:9d6a3ccc0582 183 instruction->arg1 = local_target_x_robot;// X
goldmas 37:9d6a3ccc0582 184 instruction->arg2 = local_target_y_robot;// Y
goldmas 37:9d6a3ccc0582 185 instruction->arg3 = local_target_theta_robot;// T
goldmas 37:9d6a3ccc0582 186 Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 187 break;
goldmas 37:9d6a3ccc0582 188 case MV_TURN:
goldmas 37:9d6a3ccc0582 189 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
goldmas 37:9d6a3ccc0582 190 instruction->order = MV_XYT;
goldmas 37:9d6a3ccc0582 191 instruction->arg1 = local_target_x_robot;// X
goldmas 37:9d6a3ccc0582 192 instruction->arg2 = local_target_y_robot;// Y
goldmas 37:9d6a3ccc0582 193 instruction->arg3 = local_target_theta_robot;// T
goldmas 37:9d6a3ccc0582 194 Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 195 break;
goldmas 37:9d6a3ccc0582 196 case MV_XYT:
goldmas 37:9d6a3ccc0582 197 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
goldmas 37:9d6a3ccc0582 198 Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 199 break;
goldmas 37:9d6a3ccc0582 200 case MV_COURBURE:
goldmas 37:9d6a3ccc0582 201 short alpha;
goldmas 37:9d6a3ccc0582 202 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
goldmas 37:9d6a3ccc0582 203 instruction->order=MV_XYT;
goldmas 37:9d6a3ccc0582 204 if(instruction->direction==LEFT)
goldmas 37:9d6a3ccc0582 205 alpha=(dodgeq->inst[0].arg3-theta_robot);
goldmas 37:9d6a3ccc0582 206 else
goldmas 37:9d6a3ccc0582 207 alpha=(theta_robot-dodgeq->inst[0].arg3);
goldmas 37:9d6a3ccc0582 208 if(alpha>3600)
goldmas 37:9d6a3ccc0582 209 alpha=alpha-3600;
goldmas 37:9d6a3ccc0582 210 if(alpha<-3600)
goldmas 37:9d6a3ccc0582 211 alpha=alpha+3600;
goldmas 37:9d6a3ccc0582 212 if(alpha<0)
goldmas 37:9d6a3ccc0582 213 alpha=-alpha;
goldmas 37:9d6a3ccc0582 214 if(alpha<450)
goldmas 37:9d6a3ccc0582 215 {
goldmas 37:9d6a3ccc0582 216 dodgeq->nb=0;
goldmas 37:9d6a3ccc0582 217 instruction->arg1=dodgeq->inst[0].arg1;//x
goldmas 37:9d6a3ccc0582 218 instruction->arg2=dodgeq->inst[0].arg2;//y
goldmas 37:9d6a3ccc0582 219 instruction->arg3=dodgeq->inst[0].arg3;//t
goldmas 37:9d6a3ccc0582 220 }
goldmas 37:9d6a3ccc0582 221 else if(alpha<900)
goldmas 37:9d6a3ccc0582 222 {
goldmas 37:9d6a3ccc0582 223 dodgeq->nb=1;
goldmas 37:9d6a3ccc0582 224 instruction->arg1=dodgeq->inst[1].arg1;//x
goldmas 37:9d6a3ccc0582 225 instruction->arg2=dodgeq->inst[1].arg2;//y
goldmas 37:9d6a3ccc0582 226 instruction->arg3=dodgeq->inst[1].arg3;//t
goldmas 37:9d6a3ccc0582 227 } else if(alpha<1350){
goldmas 37:9d6a3ccc0582 228 dodgeq->nb=2;
goldmas 37:9d6a3ccc0582 229 instruction->arg1=dodgeq->inst[2].arg1;//x
goldmas 37:9d6a3ccc0582 230 instruction->arg2=dodgeq->inst[2].arg2;//y
goldmas 37:9d6a3ccc0582 231 instruction->arg3=dodgeq->inst[2].arg3;//t
goldmas 37:9d6a3ccc0582 232 } else if(alpha<1800){
goldmas 37:9d6a3ccc0582 233 dodgeq->nb=3;
goldmas 37:9d6a3ccc0582 234 instruction->arg1=dodgeq->inst[3].arg1;//x
goldmas 37:9d6a3ccc0582 235 instruction->arg2=dodgeq->inst[3].arg2;//y
goldmas 37:9d6a3ccc0582 236 instruction->arg3=dodgeq->inst[3].arg3;//t
goldmas 37:9d6a3ccc0582 237 } else if(alpha<2250){
goldmas 37:9d6a3ccc0582 238 dodgeq->nb=4;
goldmas 37:9d6a3ccc0582 239 instruction->arg1=dodgeq->inst[4].arg1;//x
goldmas 37:9d6a3ccc0582 240 instruction->arg2=dodgeq->inst[4].arg2;//y
goldmas 37:9d6a3ccc0582 241 instruction->arg3=dodgeq->inst[4].arg3;//t
goldmas 37:9d6a3ccc0582 242 } else {
goldmas 37:9d6a3ccc0582 243 dodgeq->nb=5;
goldmas 37:9d6a3ccc0582 244 instruction->arg1=dodgeq->inst[5].arg1;//x
goldmas 37:9d6a3ccc0582 245 instruction->arg2=dodgeq->inst[5].arg2;//y
goldmas 37:9d6a3ccc0582 246 instruction->arg3=dodgeq->inst[5].arg3;//t
goldmas 37:9d6a3ccc0582 247 }
goldmas 37:9d6a3ccc0582 248 Flag_stoped_danger = 0;
goldmas 37:9d6a3ccc0582 249 break;
gabrieltetar 1:7e925468f9d9 250 }
goldmas 37:9d6a3ccc0582 251 // SendSpeed(300);
goldmas 37:9d6a3ccc0582 252 }
gabrieltetar 1:7e925468f9d9 253 return(0);
gabrieltetar 1:7e925468f9d9 254 }