File content as of revision 2:f5a9120c6c42:

#include "robot_general.h"

DigitalIn Daniels_in(PA_15);

//DigitalIn UBp(USER_BUTTON);  
InterruptIn UBp_it(USER_BUTTON);
Timeout ubp_to; 

Serial dbug(USBTX, USBRX, 115200);   
//Serial dbug(PA_0, PA_1, 38400);

Timer timer_generale;

int flagUBp = 0, flagDaniels = 0, match_en_cours = 0;
int flagEv = 0, flagAv = 1, flagDpl = 0, flag_auto_suiv = 0;
int sensrotation=-1;

short pos_balle[10][4];int nb_balle = 0, balle_retrouve = 0;



unsigned int temps_ecoule = 0;

void isr_UBp();void user_bp_ar();

enum etat_visee_balle_T {init_visee_balle, fin_vir_rapide, debut_vir_lent, fin_vir_lent, avancer_20,debut_vir_lent2, fin_vir_lent2,avancer,rotation, lancer,  fin_visee_balle, error_visee_balle, D, ED, Perdu, debut_vir_retour, fin_vir_rapide_odo,M, balle_disparue, recalage, recalage3, recalage2, retour};
enum etat_visee_balle_T etat_visee_balle = init_visee_balle , etat_visee_balle_prec = Perdu;
int timer_visee_balle=0;
void automate_visee_balle(); 

enum etat_pied_ballon_T {init_pied_ballon_dierct, avance_x_DT_pb, rotation_ballon_x_pb, reculer_ballon_x_pb, recalage_pied_pb,fin_pied_ballon, error_pied_ballon};
enum etat_pied_ballon_T etat_pied_ballon = init_pied_ballon_dierct , etat_pied_ballon_prec = fin_pied_ballon;
int timer_pied_ballon = 0;
void automate_pied_ballon();

enum etat_macro_auto_T {init_macro_auto};
enum etat_macro_auto_T etat_macro_auto = init_macro_auto , etat_macro_auto_prec = init_macro_auto;
int timer_macro_auto = 0;
void automate_macro_auto();

enum etat_tirer_balle_complet_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet};
enum etat_tirer_balle_complet_T etat_tirer_balle_complet = init_tirer_balle_complet , etat_tirer_balle_complet_prec = init_tirer_balle_complet;
int timer_tirer_balle_complet = 0;
void automate_tirer_balle_complet();

//enum etat_3balles_odo_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet};
//enum etat_3balles_odo_T etat_3balles_odo = init_tirer_balle_complet , 
int etat_3balles_odo;
int timer_3balles_odo = 0;
void automate_3balles_odo();


void automate_base();
int etat_auto = 0, etat_auto_prec = 0;
int etat_auto_base = 0, etat_auto_base_prec = 0;

void if_UBp();
void wait_debut_de_partie();

AnalogIn CptD(PC_5);  //premier à droite
AnalogIn CptG(PB_1);  //deuxieme à droite
AnalogIn CptA(PC_4);  //troisieme à droite


int main() 
{
    dbug.printf("\nSETUP    ");
    can_init();
    UBp_it.rise(&isr_UBp);
    //SetOdometrie(26,500,55,0);
    bonus_findepartie(0);
    
    cmd_servo = 0;
    ejection_continue = 1;
    ejection_active = 1;
    temps_ecoule = 0;
    match_en_cours = 0;
    
    //BendRadius(139, 900, -1,0);
    //GoStraight(3800,0,0,0);
    dbug.printf("END SETUP\n");
    /*while(!match_en_cours){
        trait_can();
        wait_debut_de_partie();}*/
        
    timer_generale.start();
    //Rotate(3600);
    //BendRadius(139, -900, -1,0);
    //while(1);
    //dbug.printf("%d\n", flagDaniels);

    while(1)
    {
        //dbug.printf("Capteur droit : %f \n", CptD.read());
        //dbug.printf("Capteur gauche : %f \n", CptG.read());
        dbug.printf("Capteur arriere : %f \n", CptA.read());
        wait(1);
        /*if(temps_ecoule > DUREE_MATCH_TIC)
        {
            match_en_cours = 0;
            SendRawId(ASSERVISSEMENT_STOP);
            ejection_active = 0;
            automate_ejecteur();
            wait(6);
            bonus_findepartie(1);
            while(1);
        }
        else if(timer_generale.read_us() > TIKER_GENERALE_US)
        {timer_generale.reset();temps_ecoule++;
            trait_can();
            f_mesure();
            pos_disquette();
            
            //if_UBp();
            //automate_base();
            automate_pied_ballon();
            automate_visee_balle();
    
            automate_ejecteur();
            if(((temps_ecoule%500) == 0))// execute chaques secondes
            {
                dbug.printf("Pos x:%d, y:%d, te:%d, Tecoule:%d\n", pos[0], pos[1], pos[2],temps_ecoule*TIKER_GENERALE_US/1000000);
                //dbug.printf("Dis D:%f, G:%f, min:%f\n", DTD_avg_Ex, DTG_avg_Ex, DT_avg_min_Ex);
            }
        }*/
            
    }
}

void automate_macro_auto()
{
    timer_macro_auto++;
    switch(etat_macro_auto)
    {
        case init_macro_auto:
            etat_macro_auto = init_macro_auto; timer_macro_auto = 0;
            break;
    }
}

void automate_3balles_odo()
{
    timer_3balles_odo++;
    switch(etat_3balles_odo)
    {
        case 1:
            ejection_active = 1;
            ejection_continue = 1; 
            GoStraight(1805,0,0,0);
            etat_3balles_odo = 2; timer_3balles_odo = 0;
            break;
               
        case 2 :
            if(flagFinDpl)
            {flagFinDpl = 0; 
                flagDaniels = 1;
                Rotate(-930);//rotation acoté de la balle
                etat_3balles_odo = pied_ballon_tbc; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
            
        case 3 :
            if(flagFinDpl)
            {flagFinDpl = 0;
                ejection_continue = 0;
                GoStraight(1500,0,0,0);//on va vers la deuxieme balle
                etat_3balles_odo = 4; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;   
             
        case 4 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(940);//deuxieme balle gobé ,on tourne
                etat_3balles_odo = 5; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
            
        case 5 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                envoyer_balle = 1;ejection_continue = 1;//tir la deuxieme balle
                flagDaniels = 1;
                Rotate(-940);//ici implementer le deuxieme ciblage laser
                etat_3balles_odo = 6; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
        
        case 6:  
            if(flagFinDpl)
            {flagFinDpl = 0;
                ejection_continue = 0;
                GoStraight(1500,0,0,0);
                etat_3balles_odo = 7; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
                 
        case 7:
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(930);
                etat_3balles_odo = 8; timer_3balles_odo = 0;
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
        
        case 8:
            if(flagFinDpl)
            {flagFinDpl = 0;  
                envoyer_balle = 1;ejection_continue = 1;//envoie de la troisième balle
                etat_3balles_odo = 9; timer_3balles_odo = 0;
                sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite 
            }else if(timer_3balles_odo>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10;
            break;
            
        case 9:
            break;
            
        case 10:
            break;
    }
}



void automate_tirer_balle_complet()
{
    timer_tirer_balle_complet++;
    switch(etat_tirer_balle_complet)
    {
        case init_tirer_balle_complet:
            sensrotation = -sensrotation;
            etat_pied_ballon = init_pied_ballon_dierct;
            etat_tirer_balle_complet = pied_ballon_tbc; timer_tirer_balle_complet = 0;
            break;
            
        case pied_ballon_tbc:
            automate_pied_ballon();
            if(etat_pied_ballon == fin_pied_ballon)
            {
                etat_visee_balle = init_visee_balle;
                etat_tirer_balle_complet = visee_balle_tbc; timer_tirer_balle_complet = 0;
            }
            break;
            
        case visee_balle_tbc:
            automate_visee_balle();
            if(etat_visee_balle == fin_visee_balle)
            {
                //etat_visee_balle = init_visee_balle;
                etat_tirer_balle_complet = fin_tirer_balle_complet; timer_tirer_balle_complet = 0;
            }
            else if(etat_visee_balle == error_visee_balle)
            {
                //etat_visee_balle = init_visee_balle;
                etat_tirer_balle_complet = error_tirer_balle_complet; timer_tirer_balle_complet = 0;
            }
            break;
            
        case fin_tirer_balle_complet:
            break;
            
        case error_tirer_balle_complet:
            break;
        
        
    }
}

void automate_pied_ballon()
{
    timer_pied_ballon++;
    switch(etat_pied_ballon)
    {
        case init_pied_ballon_dierct:
            short y_temp;
            if(sensrotation==1)y_temp = 300;
            else y_temp = 3700;
            GoToPosition(3500, y_temp, 0, 0);
            etat_pied_ballon = avance_x_DT_pb; timer_pied_ballon = 0;
            break;
        
        case avance_x_DT_pb:
            if(flagFinDpl)
            {flagFinDpl = 0;
                GoStraight(3050-DTA_avg_Ex,0,0,0);
                //SetOdometrie(26,pos[0],y_temp,sensrotation*900);
                etat_pied_ballon = rotation_ballon_x_pb; timer_pied_ballon = 0;
            }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon;
            break; 
            
        case rotation_ballon_x_pb:
           if(flagFinDpl)
            {flagFinDpl = 0;
                if(DTA_avg_Ex<3070 && DTA_avg_Ex>3020){ 
                    Rotate(sensrotation*900);
                    etat_pied_ballon = reculer_ballon_x_pb; timer_pied_ballon = 0;
                }
            }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon;
            break;
            
         case reculer_ballon_x_pb:
           if(flagFinDpl)
            {flagFinDpl = 0;
                GoStraight(-250,0,0,0);
                etat_pied_ballon = recalage_pied_pb; timer_pied_ballon = 0;
            }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon;
            break; 
              
        case recalage_pied_pb:
           if(flagFinDpl)
            {flagFinDpl = 0;
                short y_temp;
                if(sensrotation==1)y_temp = 55;
                else y_temp = 3945;
                SetOdometrie(26,3050,y_temp,sensrotation*900);
                etat_pied_ballon = fin_pied_ballon; timer_pied_ballon = 0;
            }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon;
            break;
            
        case fin_pied_ballon:
            break;
            
        case error_pied_ballon:
            break;
    }    
    if(etat_pied_ballon != etat_pied_ballon_prec)dbug.printf("Etat visee_balle : %d\n",etat_pied_ballon);
    etat_pied_ballon_prec = etat_pied_ballon;
}



void automate_visee_balle()
{   
    timer_visee_balle++;
    
    switch(etat_visee_balle)
    {
        case init_visee_balle ://--------------------------------------0
            SendSpeed(80, 600);
            Rotate(sensrotation*900);
            etat_visee_balle = fin_vir_rapide;
            timer_visee_balle=0;
            break;
            
        case fin_vir_rapide : //--------------------------------------1
            if(balleg  || balled )
            {flagFinDpl = 0;
                dbug.printf("balle 1-----1\n");
                pos_balle[0][2] = pos[2];
                SendRawId(ASSERVISSEMENT_STOP);
                timer_visee_balle = 0;
                etat_visee_balle = debut_vir_lent;
            } else if(flagFinDpl || timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;
                
        case debut_vir_lent://--------------------------------------2
            if(flagFinDpl)
            {flagFinDpl = 0;
            dbug.printf("balle 2-----2\n");
            SendSpeed(18, 300);
            Rotate((pos_balle[0][2]-pos[2])*2);
            etat_visee_balle = fin_vir_lent;
            }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;   
        
        case balle_disparue:
            break;
            
        case fin_vir_lent://--------------------------------------3
            if(balleg && balled)
            {flagFinDpl = 0;
                pos_balle[0][2] = pos[2];
                SendRawId(ASSERVISSEMENT_STOP);
                SendSpeed(100, 600);
                if(DT_avg_min_Ex<2000)etat_visee_balle = avancer;
                else etat_visee_balle = avancer_20;
                timer_visee_balle = 0;
            }else{
                 etat_visee_balle = debut_vir_lent;
                 flagFinDpl = 1;
            }
            break;   
            
            
        case avancer_20://--------------------------------------4
            if(flagFinDpl)
            {flagFinDpl = 0;
                GoStraight(DT_avg_min_Ex-D_ROULEAU_AXE-500,0,0,0);
                etat_visee_balle = debut_vir_lent2;
                timer_visee_balle = 0;
            }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;
            
        case debut_vir_lent2://--------------------------------------5
            if(flagFinDpl)
            {flagFinDpl = 0;
            
                SendSpeed(18, 300);
                
                if(!balleg && !balled)
                {
                    etat_visee_balle = error_visee_balle;
                }
                else{
                    if(!balleg && balled)
                    {
                        BendRadius(139, sensrotation*30, sensrotation,0);
                        etat_visee_balle = fin_vir_lent2;
                    }
                    else if(balleg && !balled)
                    {
                        BendRadius(139, 30, sensrotation,0);
                        etat_visee_balle = fin_vir_lent2;
                    }
                    else if(balleg && balled)
                    {
                        flagFinDpl = 1;
                        SendSpeed(100, 600);
                        etat_visee_balle = avancer;
                        timer_visee_balle = 0;
                    } 
                }
            }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;   
            
        case fin_vir_lent2://--------------------------------------6
            if(balleg && balled)
            {flagFinDpl = 0;
                SendRawId(ASSERVISSEMENT_STOP);
                SendSpeed(100, 600);
                etat_visee_balle = avancer;
                timer_visee_balle = 0;
            }else{flagFinDpl = 1;
                 etat_visee_balle = debut_vir_lent2;
            }
            break;  
            
            
        case avancer://--------------------------------------7
            if(flagFinDpl)
            {flagFinDpl = 0;
                ejection_continue = 0;
                GoStraight(DT_avg_min_Ex-(D_ROULEAU_AXE),0,0,0);
                etat_visee_balle = rotation;
            } 
            if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;
            
        case rotation://--------------------------------------8
            if(flagFinDpl)
            {flagFinDpl = 0;   
                if(balle_gobbee)
                {
                    short dTeta = -pos[2];
                    Rotate(dTeta);
                    etat_visee_balle = lancer;
                    timer_visee_balle = 0;
                }else etat_visee_balle = error_visee_balle;
            }
            break;
            
        case lancer:
            if(flagFinDpl)
            {flagFinDpl = 0; 
                envoyer_balle = 1;
                ejection_continue = 1;
                etat_visee_balle = fin_visee_balle;
                
            } if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break; 
           
        case fin_visee_balle:
                ejection_continue = 1;
            break;
        case error_visee_balle:
                ejection_continue = 1;
            break;
            
        case retour :
            GoToPosition(300, 0, 0, 0);
            etat_visee_balle = recalage;
            timer_visee_balle = 0;
            break;
            
        case recalage :
            if(flagFinDpl)
            {flagFinDpl = 0;
                GoStraight(-295,0,0,0);
            etat_visee_balle = recalage2;
            timer_visee_balle = 0;
            }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;
        case recalage2 :
            if(flagFinDpl)
            {flagFinDpl = 0;
                GoStraight(30,2,50,0);
            etat_visee_balle = recalage2;
            timer_visee_balle = 0;
            }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle;
            break;
        case recalage3 :
            if(flagFinDpl)
            {flagFinDpl = 0;
                etat_visee_balle = init_visee_balle;
            }
            break;      
    }
    if(etat_visee_balle != etat_visee_balle_prec)dbug.printf("Etat visee_balle : %d\n",etat_visee_balle);
    etat_visee_balle_prec = etat_visee_balle;
}

void automate_base()
{  
    switch(etat_auto_base)
    {
        case 0 :
            temps_ecoule = 0;
            etat_auto_base = 1;
            break;
            
        case 1 :
            ejection_active = 1;
            ejection_continue = 1; 
            GoStraight(1805,0,0,0);
            etat_auto_base=2;
            break;
        
        case 2 :
            if(flagFinDpl)
            {flagFinDpl = 0; 
                flagDaniels = 1;
                Rotate(-850);//rotation acoté de la balle
                etat_auto_base=201;

            }
            break;
            
        case 201 :
            if(flagFinDpl)
            {flagFinDpl = 0; 
                sensrotation=-1;
                etat_visee_balle = init_visee_balle;
                etat_auto_base=202;
            }
            break;
                
        case 202 :
            automate_visee_balle(); //ciblage vers la gauche
            if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle
            {
                etat_visee_balle = init_visee_balle;
                etat_auto_base = 3;
            }
            break;
//---------------------FIN---------------------------
            
        case 3 :
            ejection_continue = 0;
            GoStraight(1500,0,0,0);//on va vers la deuxieme balle
            etat_auto_base=4;
            break;   
             
        case 4 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(900);//deuxieme balle gobé ,on tourne
                etat_auto_base=5;
            }
            break;
            
        case 5 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                envoyer_balle = 1;//tir la deuxieme balle
                flagDaniels = 1;
                Rotate(-850);//ici implementer le deuxieme ciblage laser
                etat_auto_base=601;
            }
            break;
            
        case 601 :
            if(flagFinDpl)
            {flagFinDpl = 0; 
                sensrotation=-1;
                etat_visee_balle = init_visee_balle;
                etat_auto_base=602;
            }
            break;
                
        case 602 :
            automate_visee_balle(); //ciblage vers la gauche
            if(etat_visee_balle == 4)//on attend d'etre sur la balle
            {
                etat_visee_balle = init_visee_balle;
                etat_auto_base = 7;
            }
            break;
        
        case 7:  
                ejection_continue = 0;
                GoStraight(1500,0,0,0);
                etat_auto_base=8;
            break;
                 
        case 8 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(900);
                etat_auto_base=9;
            }
            break;
        
        case 9 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                envoyer_balle = 1;//envoie de la troisième balle
                etat_auto_base=10;
                sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite 
            }
            break;
            
         case 10:
            GoStraight(1500,0,0,0);
            etat_auto_base=11;
            break;   
        
        case 11 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(900);
               //cmd_servo = 2;
                etat_auto_base=12;
            }
            break;
        
        case 12 :   
            if(flagFinDpl)
            {flagFinDpl = 0;  
                GoStraight(100,2,0,0);
                cmd_servo = 2;
                etat_auto_base=13;
            }
            break;
        case 13 :   
            if(flagFinDpl)
            {flagFinDpl = 0;  
                GoStraight(3800,0,0,0);
                cmd_servo = 1;
                etat_auto_base=14;
            }
            break;
            
         case 14 :   
            if(flagFinDpl)
            {flagFinDpl = 0;  
                GoStraight(-500,0,0,0);
                etat_auto_base=15;
            }
            break;   
            
         case 15 :   
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(1800);
                cmd_servo = 0;
                etat_auto_base=16;
                sensrotation=1;
            }
            break;   
         case 16 :
               
            break;   
            
            
            
        case 100 :
            etat_visee_balle = init_visee_balle;//On passe dans l'automate de ciblage
                etat_auto_base=101;
            break;    
            
        case 101 :
            automate_visee_balle(); //ciblage vers la gauche
            if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle
            {
                etat_visee_balle = init_visee_balle;
                etat_auto_base = 102;
            }
            break;
            
        case 102 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                ejection_continue = 0;
                GoStraight(DTD_avg_Ex,0,0,0);
                etat_auto_base=103;
            }
            break;
                 
        case 103 :
            if(flagFinDpl)
            {flagFinDpl = 0;  
                Rotate(-pos[2]);
                etat_auto_base=100;
            }
            break;
        default :
            
            break; 
            
    }
    if(etat_auto_base != etat_auto_base_prec)dbug.printf("Etat auto_base: %d\n",etat_auto_base);
    etat_auto_base_prec = etat_auto_base;
}

void wait_debut_de_partie()
{
    if(!Daniels_in)
    {
        dbug.printf("-----DEBUT DE PARTIE------\n");
        match_en_cours = 1;
    }
}

void user_bp_ar(){UBp_it.enable_irq();}
void isr_UBp()
{
    UBp_it.disable_irq();dbug.printf("ubp\n");
    ubp_to.attach(&user_bp_ar, 0.3);
    flagUBp = 1;
}