File content as of revision 14:1be2f691cbb4:

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

void automate_testLasers(Robot&);
void automate_testABalle(Robot&);
void automate_deplacement(Robot&);


typedef enum{CHERCHE_BALLE,BALLE_GAUCHE,BALLE_DROITE,BALLE_STRAIGHT} type_etat_balle;
type_etat_balle etat_balle = CHERCHE_BALLE;


int main(void)
{
    Robot robot;
    
    while(1)
    {
        automate_testLasers(robot);
        automate_testABalle(robot);
        if(!robot.aBalle())
            automate_deplacement(robot);
    }
}


void automate_testLasers(Robot& robot)
{
    typedef enum {ATTENTE, TURN_RIGHT, TURN_LEFT, VOIT_BALLE} type_etat;
    static type_etat etat = ATTENTE;
    
    static float past_gauche, past_droit, actual_gauche, actual_droit;
    
    actual_gauche = robot.getDist(Laser::Gauche);
    actual_droit = robot.getDist(Laser::Droit);
    
    switch(etat)
    {
        case ATTENTE:            
            if(actual_gauche < past_gauche-30)
            {
                etat = TURN_LEFT;
                robot.stop();
            }
                
            if(actual_droit < past_droit-30)
            {
                etat = TURN_RIGHT;
                robot.stop();
            }
            break;
            
            
        case TURN_LEFT:
            etat_balle = BALLE_GAUCHE;
            
            if(actual_droit < past_droit-30)
            {
                etat = VOIT_BALLE;
                robot.stop();
            }
                
            if(actual_gauche > past_gauche+30)
            {
                etat = TURN_RIGHT;
                robot.stop();
            }
            break;
            
            
        case TURN_RIGHT:
            etat_balle = BALLE_DROITE;
            
            if(actual_gauche < past_gauche-30)
            {
                etat = VOIT_BALLE;
                robot.stop();
            }
                
            if(actual_droit > past_droit+30)
            {
                etat = TURN_LEFT;
                robot.stop();
            }
            break;
            
            
        case VOIT_BALLE:
            etat_balle = BALLE_STRAIGHT;
                
            if(actual_gauche > past_gauche+30)
            {
                etat = TURN_RIGHT;
                robot.stop();
            }
                
            if(actual_droit > past_droit+30)
            {
                etat = TURN_LEFT;
                robot.stop();
            }
            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;
    
    switch(etat)
    {
        case ATTENTE:
            robot.gobe(40);
            if(robot.aBalle())
            {
                //robot.stop();
                etat = ABALLE;
                etat_balle = CHERCHE_BALLE;
            }
            break;
            
        case ABALLE:
            if(robot.tourne(1800))
            {
                robot.ejecte(60);
                etat = ATTENTE;
            }
            break;
    }
}

void automate_deplacement(Robot& robot)
{    
    switch(etat_balle)
    {
        case CHERCHE_BALLE:
            robot.tourne(100);
            break;
            
        case BALLE_GAUCHE:
            robot.tourne(-50);
            break;
            
        case BALLE_DROITE:
            robot.tourne(50);
            break;
            
        case BALLE_STRAIGHT:
            robot.avance((short)robot.getDist(Laser::Gauche,"mm"));
            break;
    }
}