File content as of revision 29:6bce50d6530c:

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

bool automate_aveugle(Robot&, bool&);
void automate_ejecte(Robot&, bool&);
void automate_testLasers(Robot&);
void automate_testABalle(Robot&, bool&);
void automate_deplacement(Robot&);
void automate_vitesse(Robot&);
void automate_fin_de_partie(Robot&);
bool automate_arretUrgence(Robot&);

typedef enum{TURN_FAST, CORRECT_GAUCHE, CORRECT_DROITE, GO_STRAIGHT} type_etat_deplacement;
type_etat_deplacement etat_deplacement = TURN_FAST;


int main(void)
{
    Robot robot;
    
    bool ejecte = false;
    
    while(1)
    if(!Robot::Jack)
    {
        // Trois balles en début de partie à l'aveugle
        while( !automate_aveugle(robot, ejecte) );
            
        automate_ejecte(robot, ejecte);
            
        automate_fin_de_partie(robot);
        
        // Gestion du lancer de balle
        /*automate_testABalle(robot, ejecte);
        automate_ejecte(robot, ejecte);
        
        // Recherche de balles
        automate_testLasers(robot);
        if(!robot.aBalle())
            automate_deplacement(robot);
        
        // Gestion de la vitesse en fonction de l'état actuel
        automate_vitesse(robot);*/
    }
}

void automate_fin_de_partie(Robot& robot)
{
    typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
    static type_etat etat = AVANCE;
    
    static Timer T_fin;
    
    switch(etat)
    {
        case AVANCE:
            robot.avance();
            if(automate_arretUrgence(robot))
                etat = EXPLOSE_BALLON;
            break;
            
        case EXPLOSE_BALLON:
            robot.stop();
            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();
            }
            break;            
    }
}

bool automate_arretUrgence(Robot& robot)
{
    typedef enum{RAS, PERCEPTION, ARRET_URGENCE, ATTENTE_REPLACEMENT} type_etat;
    static type_etat etat = RAS;
    
    // Timer pour la durée sur la ligne blanche
    static Timer timerCNY;
    
    switch(etat)
    {
        case RAS :
            if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT )){
                etat = PERCEPTION;
                timerCNY.start();
            }
            break;
            
        case PERCEPTION :
            if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT ) && timerCNY.read() >= 0.15f )
                etat = ARRET_URGENCE;
            else if( timerCNY.read() >= 0.15f ){
                etat = RAS;
                timerCNY.stop();
                timerCNY.reset();
            }
            break;
            
        case ARRET_URGENCE :
            timerCNY.stop();
            timerCNY.reset();
            
            etat = ATTENTE_REPLACEMENT;
            
            return true;
            
        case ATTENTE_REPLACEMENT :
            if( !robot.surBlanc( Robot:: CNY_GAUCHE )  && !robot.surBlanc( Robot::CNY_DROIT ))
                etat = RAS;
            break;
    }
    
    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;
    }
}

void automate_testLasers(Robot& robot)
{   
    static const int diffMurBalle = 20;
    static float past_gauche, past_droit, actual_gauche, actual_droit;
    static float distBalle;
    
    actual_gauche = robot.getDist(Laser::Gauche);
    actual_droit = robot.getDist(Laser::Droit);
    
    switch(etat_deplacement)
    {
        case TURN_FAST:
            if(actual_droit < past_droit - diffMurBalle)
            {
                distBalle = actual_droit;
                robot.stop();
                etat_deplacement = CORRECT_GAUCHE;
            }
            break;
            
        case CORRECT_GAUCHE:
            
            break;
            
        case CORRECT_DROITE:
            break;
            
        case GO_STRAIGHT:
            break;
    }
    
    
    past_gauche = actual_gauche;
    past_droit = actual_droit;
}

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(80,500); //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.tourne();
            break;
            
        case CORRECT_GAUCHE:
            robot.tourne(-450);
            break;
            
        case CORRECT_DROITE:
            robot.tourne(450);
            
        case GO_STRAIGHT:
            robot.avance();
            break;
    }
}

void automate_vitesse(Robot& robot)
{
    switch(etat_deplacement)
    {
        case TURN_FAST:
            robot.setSpeed(80,450);
            break;
            
        case CORRECT_GAUCHE:
            robot.setSpeed(50,300);
            break;
            
        case CORRECT_DROITE:
            robot.setSpeed(50,300);
            break;
            
        case GO_STRAIGHT:
            robot.setSpeed(80,400);
            break;
    }
}