File content as of revision 30:ee3e274e4b55:

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

bool automate_aveugle(Robot&);
void automate_testLasers(Robot&);
void automate_testABalle(Robot&);
void automate_deplacement(Robot&);
void automate_vitesse(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;
    
    robot.setSpeed(80,450); // vitesse de rotation de recherche de la balle
    
    while(1)
    if(!Robot::Jack)
    {
        while(!automate_aveugle(robot));
        
        automate_testABalle(robot);
        
        automate_testLasers(robot);
        
        if(!robot.aBalle())
            automate_deplacement(robot);
            
        automate_vitesse(robot);
    }
}

bool automate_aveugle(Robot& robot)
{
    typedef enum{STRAIGHT1, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
    static type_etat etat = STRAIGHT1;
    
    static Timer T_ejecte;
    
    switch(etat)
    {
        case STRAIGHT1:
            robot.avance();
            if(robot.aBalle())
            {
                robot.stop();
                robot.ejecte();
                BendRadius(100,900,1,1);
            }
            else if( T_ejecte.read() >= 1.0f )
            {
                robot.gobe();
                T_ejecte.stop();
                T_ejecte.reset();
            }
            break;
            
        case TOURNE_D:
            if( T_ejecte.read() >= 1.0f )
            {
                robot.gobe();
                T_ejecte.stop();
                T_ejecte.reset();
            }
                
            if(robot.tourne(900))
                etat = STRAIGHT2;
            break;
            
        case STRAIGHT2:
            robot.avance();
            if(robot.aBalle())
            {
                robot.stop();
                etat = TOURNE_G;
            }
            break;
            
        case TOURNE_G:
            if(robot.tourne(-900))
                robot.ejecte();
                
            if(!robot.aBalle())
            {
                T_ejecte.start();
                etat = TOURNE_D;
            } 
            break;
    }
    
    return false;
}


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)
{
    typedef enum {ATTENTE,ABALLE} type_etat;
    static type_etat etat = ATTENTE;
    
    static Timer T_ejecte;
    
    switch(etat)
    {
        case ATTENTE:
            if(robot.aBalle())
            {
                robot.stop();
                etat = ABALLE;
                robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
            }
            else if( T_ejecte.read() >= 1.0f )
            {
                robot.gobe();
                T_ejecte.stop();
                T_ejecte.reset();
            }
            break;
            
        case ABALLE:
            if(robot.tourne(1800))
                robot.ejecte();
                
            if(!robot.aBalle())
            {
                T_ejecte.start();
                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;
    }
}