File content as of revision 21:d137ec53c3a9:

/*********************************************\
|             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, 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);
}