Diff: Evitement/Evitement.cpp

Revision:

1:7e925468f9d9

Child:

10:1964bb91b925

diff -r 41cc45429aba -r 7e925468f9d9 Evitement/Evitement.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Evitement/Evitement.cpp	Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,136 @@
+/*********************************************\
+|             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;
+    
+    //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){
+    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);
+}