File content as of revision 39:82c1de84bf40:

#include "mbed.h"
#include "CAN_asser.h"
#include "Robot.h"
#include "math.h"

bool automate_aveugle(Robot&, bool&);       //automate de début de partie
void automate_ejecte(Robot&, bool&);        //
void automate_testLasers(Robot&);           //
void automate_testABalle(Robot&, bool&);    //
void automate_deplacement(Robot&);          //
bool automate_fin_de_partie(Robot&);        //automate crève ballon de fin de partie
bool automate_arretUrgence(Robot&);         //automate d'arrêt d'urgence au filet
void automate_run(Robot&);                  //automate principale de partie
void securitePos(Robot&);

typedef enum{TURN_FAST, TURN_FAST_GO, STRAIGHT, STRAIGHT_GO, CORRECT_GAUCHE, CORRECT_GAUCHE_GO} type_etat_deplacement;
type_etat_deplacement etat_deplacement = TURN_FAST;

bool forceRAS = false;

int main(void)
{
    Robot robot;
    
    while(1)
        automate_run(robot);
}

void automate_run(Robot& robot)
{
    static bool ejecte = false;
    typedef enum{ATTENTE, RUN_AVEUGLE, RECHERCHE_BALLES, FIN_PARTIE} type_etat;
    static type_etat etat = ATTENTE;
    static Timer T_partie;
    
    if(T_partie.read() > 70.0f)
    {
        etat = FIN_PARTIE;
        robot.stop();
        robot.ejecte();
        T_partie.stop();
        T_partie.reset();
    }
    
    switch(etat)
    {
        case ATTENTE:
            if(!Robot::Jack)
            {
                T_partie.start();
                etat = RUN_AVEUGLE;
            }
            break;
            
        case RUN_AVEUGLE:
            if(automate_aveugle(robot, ejecte) && robot.tourne(1800))
                etat = RECHERCHE_BALLES;
            automate_ejecte(robot, ejecte);
            break;
            
        case RECHERCHE_BALLES:
            
            //le robot ne s'approche pas des murs
            securitePos(robot);
            
            if(!robot.aBalle() && !forceRAS)
                automate_deplacement(robot);
            
            // Recherche de balles
            automate_testLasers(robot);
            
            // Gestion du lancer de balle
            automate_testABalle(robot, ejecte);
            
            //Ejecte ou non en fonction des commandes des autres automates
            automate_ejecte(robot, ejecte);
            break;
            
        case FIN_PARTIE:
            automate_fin_de_partie(robot);
            break; 
    }
}

void automate_testLasers(Robot& robot)
{      
    typedef enum{RAS, SUSPICION, CONFIRMATION, VERIFICATION, CORRIGE, ABALLE, ATTENTE_VISION} type_etat;
    static type_etat etat = RAS;
    
    static const int distMurBalle = 20;
    static Timer T_suspi, T_arret, T_balle, T_corrige;
    static float droit[2] =  { robot.getDist(Laser::Droit),robot.getDist(Laser::Droit) }, 
                 gauche[2] = { robot.getDist(Laser::Gauche),robot.getDist(Laser::Gauche) },
                 distBalle;
    
    droit[0] = robot.getDist(Laser::Droit);
    gauche[0] = robot.getDist(Laser::Gauche);
    
    if(robot.aBalle())
        etat = ABALLE;
        
    if(forceRAS)
        etat = RAS;
    
    switch(etat)
    {
        case RAS:
            if(etat_deplacement != TURN_FAST_GO)
                etat_deplacement = TURN_FAST;
            if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 150)
            {
                T_suspi.start();
                etat = SUSPICION;
                dbug.printf("SUSPICION\n\r");
            }
            break;
            
        case SUSPICION:
            if(T_suspi.read_ms() < 500)
            {
                if( gauche[1] > (gauche[0] + distMurBalle) )
                {
                    etat = CONFIRMATION;
                    dbug.printf("CONFIRMATION\n\r");
                    T_suspi.stop();
                    T_suspi.reset();
                }
            }
            else
            {
                T_suspi.stop();
                T_suspi.reset();
                etat = RAS;
                dbug.printf("RAS\n\r");
            }
            break;
            
        case CONFIRMATION:
            distBalle = gauche[0];
            robot.stop();
            T_arret.start();
            etat = VERIFICATION;
            dbug.printf("VERIFICATION\n\r");
            break;
            
        case VERIFICATION:
            if(T_arret.read_ms() > 300)
            {
                T_arret.stop();
                T_arret.reset();
                
                if( fabs(gauche[0] - droit[0]) <= 5.5f && fabs(gauche[0]-distBalle) <= 6.0f)
                {
                    if(etat_deplacement != STRAIGHT_GO)
                    {
                        etat_deplacement = STRAIGHT;
                        dbug.printf("STRAIGHT\n\r");
                    }
                }
                else
                {
                    etat_deplacement = CORRECT_GAUCHE;
                    T_corrige.start();                  //Timer pour abandonner en cas de non retrouvage de balle
                    etat = CORRIGE;
                    dbug.printf("CORRIGE\n\r");
                }
            }
            break;
            
        case CORRIGE:
            if( droit[1] > (droit[0] + distMurBalle) )
            {
                if(etat_deplacement != STRAIGHT_GO)
                {
                    etat_deplacement = STRAIGHT;
                    dbug.printf("STRAIGHT\n\r");
                    T_corrige.stop();
                    T_corrige.reset();
                }
            }
            else if( T_corrige.read() > 1.0f )
            {
                etat = RAS;
                T_corrige.stop();
                T_corrige.reset();    
            }
            break;
            
        case ABALLE:
            if(!robot.aBalle())
            {
                T_balle.start();
                etat = ATTENTE_VISION;
                dbug.printf("ATTENTE_VISION\n\r");
                etat_deplacement = TURN_FAST;
            }
            break;
            
        case ATTENTE_VISION:
            if(T_balle.read() > 1.0f)
            {
                T_balle.stop();
                T_balle.reset();
                etat = RAS;
                dbug.printf("RAS\n\r");
            }
            break;
    }
    
    // Conversions past
    droit[1] = droit[0];
    gauche[1] = gauche[0];
}

void securitePos(Robot& robot)
{
    static int etat;
    
    switch(etat)
    {
        case 0:
            if(robot.pos(Robot::X) < 400 || robot.pos(Robot::X) > 3600 || robot.pos(Robot::Y) < 400 || robot.pos(Robot::Y) > 3350)
            {
                forceRAS = true;
                robot.stop();
                etat = 1;
                dbug.printf("GoToXYT\n\r");
            }
            else
                forceRAS = false;
            break;
            
        case 1:
            if(robot.GoToXYT(2000,2000,0))
            {
                forceRAS = false;
                etat = 0;
                dbug.printf("Remis au milieu\n\r");
            }
            break;
    }
}

void automate_testABalle(Robot& robot, bool& ejecte)
{
    typedef enum {ATTENTE, ABALLE} type_etat;
    static type_etat etat = ATTENTE;
    
    switch(etat)
    {
        case ATTENTE:
            if(robot.aBalle())
            {
                robot.stop();
                etat = ABALLE;
                robot.setSpeed(150,800); //vitesse de rotation vers le terrain ennemi
            }
            break;
            
        case ABALLE:
            if( robot.tourne( -robot.pos(Robot::THETA) ) )
            {
                ejecte = true;
                etat = ATTENTE;
                etat_deplacement = TURN_FAST;
            }
            break;
    }
}

void automate_deplacement(Robot& robot)
{    
    switch(etat_deplacement)
    {            
        case TURN_FAST:
            robot.stop();
            robot.setSpeed(90,450);
            etat_deplacement = TURN_FAST_GO;
            break;
            
        case TURN_FAST_GO:
            if(robot.immobile())
                robot.tourne();
            break;
            
        case STRAIGHT:
            robot.stop();
            robot.setSpeed(150,800);
            etat_deplacement = STRAIGHT_GO;
            break;
            
        case STRAIGHT_GO:
            if(robot.immobile())
                robot.avance( robot.getDist(Laser::Gauche,"mm") );
            break;
            
        case CORRECT_GAUCHE:
            robot.stop();
            robot.setSpeed(35,450);
            etat_deplacement = CORRECT_GAUCHE_GO;
            break;
            
        case CORRECT_GAUCHE_GO:
            if(robot.immobile())
                robot.tourne(-100);
            break;
    }
}

bool automate_fin_de_partie(Robot& robot)
{
    typedef enum{AVANCE, INTERMEDIAIRE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
    static type_etat etat = AVANCE;
    
    static Timer T_fin;
    
    switch(etat)
    {
        case AVANCE:
            if(robot.tourne( -robot.pos(Robot::THETA) ))//if(robot.GoToXYT(robot.pos(Robot::X),3600,0))
                etat = INTERMEDIAIRE;
            break;
            
        case INTERMEDIAIRE:
            if(robot.avance( 4000 - robot.pos(Robot::Y) - 350))
                etat = EXPLOSE_BALLON;
            break;
            
        case EXPLOSE_BALLON:
            if(robot.leveBras())
            {
                robot.exploseBallon();
                T_fin.start();
                etat = FIN_PARTIE;
            }
            break;
            
        case FIN_PARTIE:
            if(T_fin.read() >= 2.0f && robot.baisseBras())
            {
                robot.arreteDisquette();
                robot.stopRouleau();
                return true;
            }           
    }
    
    return false;
}

bool automate_aveugle(Robot& robot, bool& ejecte)
{
    typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
    static type_etat etat = STRAIGHT1;
    
    static Timer T_ejecte;
    static int cptBalles = 0;
    
    if(cptBalles == 2)
        return true;
    
    switch(etat)
    {
        case STRAIGHT1:
            if(robot.avance(1850 - DIST_CENTRE))
            {
                ejecte = true;
                etat = PREMIER_TOURNE_D;
            }
            break;
            
        case PREMIER_TOURNE_D:
            if(robot.tourne(ANGLE_DROIT))
                etat = STRAIGHT2;
            break;
            
        case TOURNE_D:                
            if(robot.tourne(ANGLE_DROIT))
                etat = STRAIGHT2;
            break;
            
        case STRAIGHT2:
            if(robot.avance(1500))
                etat = TOURNE_G;
            break;
            
        case TOURNE_G:
            if(robot.tourne(-ANGLE_DROIT))
            {
                ejecte = true;
                cptBalles++;
                etat = TOURNE_D;
            }
            break;
    }
    
    return false;
}

void automate_ejecte(Robot& robot, bool& ejecte)
{
    typedef enum{GOBE, EJECTE, ATTENTE} type_etat;
    static type_etat etat = GOBE;
    
    static Timer timer_ejecte;
    
    switch(etat)
    {
        case GOBE:
            robot.gobe();
            if(ejecte)
                etat = EJECTE;
            break;
            
        case EJECTE:
            robot.ejecte();
            timer_ejecte.start();
            etat = ATTENTE;
            break;
            
        case ATTENTE:
            if( timer_ejecte >= 1.0f ){
                timer_ejecte.stop();
                timer_ejecte.reset();
                ejecte = false;
                etat = GOBE;   
            }
            break;
    }
}