![](/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@21:d137ec53c3a9, 2020-06-24 (annotated)
- Committer:
- gabrieltetar
- Date:
- Wed Jun 24 09:39:42 2020 +0000
- Revision:
- 21:d137ec53c3a9
- Parent:
- 10:1964bb91b925
- Child:
- 37:9d6a3ccc0582
SPLIT - A TEST
Who changed what in which revision?
User | Revision | Line number | New 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 | 21:d137ec53c3a9 | 76 | 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){ |
gabrieltetar | 10:1964bb91b925 | 77 | Debug_Audio(3,1); |
gabrieltetar | 1:7e925468f9d9 | 78 | switch(instruction->order){ |
gabrieltetar | 1:7e925468f9d9 | 79 | case MV_LINE: |
gabrieltetar | 21:d137ec53c3a9 | 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 | 21:d137ec53c3a9 | 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 | 21:d137ec53c3a9 | 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 | 21:d137ec53c3a9 | 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 | } |