FRC_equipe1 / Mbed 2 deprecated FRC_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp

Committer:
Wael_H
Date:
2019-06-05
Revision:
18:a7dc3a63d7eb
Parent:
15:3d4543a6c100

File content as of revision 18:a7dc3a63d7eb:

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

void testLasers(Robot&, bool&);
void automate_testABalle(Robot&);
void automate_deplacement(Robot&, bool&);


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


int main(void)
{
    Robot robot;
    bool autorCorrect = false;
    DigitalIn Jack(PA_15);
    
    robot.setSpeed(100,600);
    robot.gobe(25);
    
    while(1)
    if(!Jack)
    {            
        automate_testABalle(robot);
        
        testLasers(robot, autorCorrect);
        
        if(!robot.aBalle())
            automate_deplacement(robot, autorCorrect);
    }
}

void testLasers(Robot& robot, bool& autorCorrect)
{   
    const int diffMurBalle = 20;
    static float past_gauche, past_droit, actual_gauche, actual_droit;
    
    actual_gauche = robot.getDist(Laser::Gauche);
    actual_droit = robot.getDist(Laser::Droit);
    
    switch(etat_deplacement)
    {
        case TURN_FAST:
            if(actual_droit < past_droit - diffMurBalle)
            {
                robot.stop();
                etat_deplacement = RETROUVE;
                robot.setSpeed(50,300);
            }
            break;
            
        case RETROUVE:
            if(actual_droit < past_droit - diffMurBalle)
            {
                robot.stop();
                etat_deplacement = GO_STRAIGHT;
                robot.setSpeed(100,500);
            }
            break;
            
        /*case GO_STRAIGHT:
            if(actual_droit > past_droit + diffMurBalle)
            {
                etat_deplacement = CORRECT_GAUCHE;
                robot.setSpeed(50,300);
            }
            else if(actual_gauche > past_gauche + diffMurBalle)
            {
                etat_deplacement = CORRECT_DROITE;
                robot.setSpeed(50,300);
            }
            break;*/
        
        /*case CORRECT_GAUCHE:
            if(autorCorrect) //s'il a fini d'avancer
                if(actual_droit < past_droit - diffMurBalle)
                {
                    robot.stop();
                    autorCorrect = false;
                    etat_deplacement = GO_STRAIGHT;
                    robot.setSpeed(80,300);
                }
            break;
            
        case CORRECT_DROITE:
            if(autorCorrect) //s'il a fini d'avancer
                if(actual_gauche < past_gauche - diffMurBalle)
                {
                    robot.stop();
                    autorCorrect = false;
                    etat_deplacement = GO_STRAIGHT;
                    robot.setSpeed(80,300);
                }
            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;
    
    PwmOut Servo_Ballon(PA_10);
    PwmOut Mot_Ballon(PB_6);
    
    Servo_Ballon.period_ms(20);
    Mot_Ballon.period_ms(20);
    
    static Timer T_ejecte;
    
    switch(etat)
    {
        case ATTENTE:
            if(robot.aBalle())
            {
                dbug.printf("A balle\n\r");
                robot.stop();
                //Servo_Ballon.write(1);
                robot.setSpeed(80,500);
                etat = ABALLE;
            }
            else if( T_ejecte.read() >= 1.0f )
            {
                robot.gobe(25);
                //Reset du timer pour la prochaine balle
                T_ejecte.stop();
                T_ejecte.reset();
            }
            break;
            
        case ABALLE:
            /*if(robot.tourne(1800))
            {
                dbug.printf("Tourne\n\r");*/
                robot.ejecte(70);
            //}
            if(!robot.aBalle())
            {
                //Timer qui permet à la balle d'avoir le temps de quitter le reservoir
                T_ejecte.start();
                //
                Servo_Ballon.write(0);
                etat = ATTENTE;
                etat_deplacement = TURN_FAST;
                robot.setSpeed(100,600);
            }   
            break;
    }
}

void automate_deplacement(Robot& robot, bool& autorCorrect)
{    
    switch(etat_deplacement)
    {
        case TURN_FAST:
            robot.tourne();
            break;
            
        case RETROUVE:
            robot.tourne(-450);
            break;
            
        case GO_STRAIGHT:
            robot.avance();
                //autorCorrect = true;
            break;
            
        /*case CORRECT_GAUCHE:
            robot.tourne(-30);
            break;
            
        case CORRECT_DROITE:
            robot.tourne(30);
            break;*/
    }
}



/*int main(void)
{
    Robot robot;
    
    while(1)
    {
        if(etat_deplacement != BALLE_STRAIGHT)
            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)
            {
                dbug.printf("TURN_LEFT\n\r");
                etat = TURN_LEFT;
                robot.stop();
            }
                
            if(actual_droit < past_droit-30)
            {
                dbug.printf("TURN_RIGHT\n\r");
                etat = TURN_RIGHT;
                robot.stop();
            }
            break;
            
            
        case TURN_LEFT:
            etat_deplacement = BALLE_GAUCHE;
            
            if(actual_droit < past_droit-30)
            {
                dbug.printf("VOIT_BALLE\n\r");
                etat = VOIT_BALLE;
                robot.stop();
            }
                
            if(actual_gauche > past_gauche+30)
            {
                dbug.printf("TURN_RIGHT\n\r");
                etat = TURN_RIGHT;
                robot.stop();
            }
            break;
            
            
        case TURN_RIGHT:
            etat_deplacement = BALLE_DROITE;
            
            if(actual_gauche < past_gauche-30)
            {
                dbug.printf("VOIT_BALLE\n\r");
                etat = VOIT_BALLE;
                robot.stop();
            }
                
            if(actual_droit > past_droit+30)
            {
                dbug.printf("TURN_LEFT\n\r");
                etat = TURN_LEFT;
                robot.stop();
            }
            break;
            
            
        case VOIT_BALLE:
            etat_deplacement = BALLE_STRAIGHT;
                
            if(actual_gauche > past_gauche+30)
            {
                dbug.printf("TURN_RIGHT\n\r");
                etat = TURN_RIGHT;
                robot.stop();
            }
                
            if(actual_droit > past_droit+30)
            {
                dbug.printf("TURN_LEFT\n\r");
                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(25);
            if(robot.aBalle())
            {
                dbug.printf("A balle\n\r");
                robot.stop();
                etat = ABALLE;
                etat_deplacement = CHERCHE_BALLE;
            }
            break;
            
        case ABALLE:
            if(robot.tourne(1800))
            {
                dbug.printf("Tourne\n\r");
                robot.ejecte(60);
            }
            if(!robot.aBalle())
                etat = ATTENTE;
            break;
    }
}

void automate_deplacement(Robot& robot)
{    
    switch(etat_deplacement)
    {
        case CHERCHE_BALLE:
            robot.tourne();
            break;
            
        case BALLE_GAUCHE:
            robot.tourne(-30);
            break;
            
        case BALLE_DROITE:
            robot.tourne(30);
            break;
            
        case BALLE_STRAIGHT:
            robot.avance();
            static float allerJusqua;
            
            if(robot.getDist(Laser::Gauche,"mm") < robot.getDist(Laser::Droit,"mm"))
                allerJusqua = 0.95*robot.getDist(Laser::Gauche,"mm");
            else
                allerJusqua = 0.95*robot.getDist(Laser::Droit,"mm");
                
            if(allerJusqua > 2500)
                allerJusqua = 2500;
            
            while(!robot.avance((int)allerJusqua))
            {
                dbug.printf("%f\n\r", robot.getDist(Laser::Gauche,"mm"));
            }
            break;
    }
}*/