File content as of revision 37:ee2c72e76d9c:

/*********************************************\
|             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                                                 */
/****************************************************************************************/

int Flag_stoped_danger = 0;

unsigned short balise_danger(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;
    char seuil_bas_arriere = 5; 
    char seuil_haut_arriere = 7;
    char seuil_bas_avant = 0xF;
    char seuil_haut_avant = 0xD;
 //   #endif 
    // 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))
    if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
    {
        if(asser_stop_direction==1)
        {
            if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
                ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
                SendRawId(ASSERVISSEMENT_STOP);
        }
        else
        {
            if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
                ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
                SendRawId(ASSERVISSEMENT_STOP);
        }
    }
    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;
    char seuil_bas_arriere = 5; 
    char seuil_haut_arriere = 7;
    char seuil_bas_avant = 0xF;
    char seuil_haut_avant = 0xD;
 //   #endif 
    // 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))
    if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
    {
        if(Cote == 1)//Jaune
        {
            if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {}
            else
            {
                if(asser_stop_direction==1)
                {
                    if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
                        ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
                        SendRawId(ASSERVISSEMENT_STOP);
                            Flag_stoped_danger = 1;
                }
                else
                {
                    if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
                        ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
                        SendRawId(ASSERVISSEMENT_STOP);
                            Flag_stoped_danger = 1;
                }
            }
        }   
        
        else if(Cote == 0)//Bleu
        {
            if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {}
            else
            {
                if(asser_stop_direction==1)
                {
                    if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
                        ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
                        SendRawId(ASSERVISSEMENT_STOP);
                            Flag_stoped_danger = 1;
                }
                else
                {
                    if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
                        ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
                        SendRawId(ASSERVISSEMENT_STOP);
                            Flag_stoped_danger = 1;
                }
            }
        }
    }
    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 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){
    
    if(Flag_stoped_danger)
    {
        
        ingnorInversionOnce=1;
        wait_ms(1000);
        switch(instruction->order){
            case MV_RECALAGE:
                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;  
                    Flag_stoped_danger = 0;          
            break;
            
            case MV_LINE:
                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
                instruction->order = MV_XYT;
                instruction->arg1 = local_target_x_robot;// X
                instruction->arg2 = local_target_y_robot;// Y
                instruction->arg3 = local_target_theta_robot;// T
                    Flag_stoped_danger = 0;
            break;
            case MV_TURN:
                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
                instruction->order = MV_XYT;
                instruction->arg1 = local_target_x_robot;// X
                instruction->arg2 = local_target_y_robot;// Y
                instruction->arg3 = local_target_theta_robot;// T
                    Flag_stoped_danger = 0;
            break;
            case MV_XYT:
                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
                    Flag_stoped_danger = 0;
            break;
            case MV_COURBURE:
                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>3600)
                    alpha=alpha-3600;
                if(alpha<-3600)
                    alpha=alpha+3600;   
                if(alpha<0)
                    alpha=-alpha;
                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
                } 
                    Flag_stoped_danger = 0;
            break;
        }
//    SendSpeed(300);
    }
    return(0);
}