File content as of revision 10:fba0699b7a74:

#include "actions_Gr.h"


#ifdef ROBOT_BIG
#define MASK_PRESENTOIR_AV 0x07
#define MASK_PRESENTOIR_AR 0x70

#define MASK_GOLDENIUM_AV 0x02
#define MASK_GOLDENIUM_AR 0x20

#define MASK_SOL_AV 0x08
#define MASK_SOL_AR 0x80

#define MASK_AV_DROIT 0x01
#define MASK_AR_DROIT 0x10

#define MASK_AV_DROIT_GAUCHE 0x05
#define MASK_AR_DROIT_GAUCHE 0x50

#define MASK_FC_DROIT   0x01
#define MASK_CT_DROIT   0x02

#define MASK_FC_GAUCHE  0x04
#define MASK_CT_GAUCHE  0x08

#define POS_DOIGT_GAUCHE 585
#define POS_DOIGT_DROIT  389

char status_pompe=0;
bool flag_ascenseur = 0;

char fpresentoir_avant=0, fpresentoir_arriere=0;
char fgoldenium_avant=0, fgoldenium_arriere=0;
char fsol_avant=0,fsol_arriere=0;
char fsol_avant_relache=0,fsol_arriere_relache=0;
char fbalance_avant=0,fbalance_arriere=0;
char favant_relache=0,farriere_relache=0;
char faccelerateur_avant=0,faccelerateur_arriere=0;


DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2

char buffer_couleur_bas[SIZE_FIFO];
unsigned char FIFO_couleur_ecriture=0;
signed char FIFO_couleur_lecture=0;
char status_contact=0;


void convoyeur_gauche_jaune(void){
    
    typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
    static type_etat etat = init;
    static int16_t pos = 0;
    static int16_t previous_pos = getPos(stockage_G,1);
    
    
    switch(etat){
            
        case init : 
            velocityControl(stockage_G,-512,BLED_ON,1);
            pos = getPos(stockage_G,1);
            if(previous_pos != pos){
                etat = etalonnage;
                pc.printf("ETALONNAGE\n");
            }
            break;

        case etalonnage :
            pos = getPos(stockage_G,1);
            if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ 
                velocityControl(stockage_G,0,GLED_ON,1);
                wait_ms(500);
                pos = getPos(stockage_G,1) ;
                positionControl(stockage_G,pos+180,1,BLED_ON,1);
                etat = tmp_pret;
                pc.printf("TMP_PRET\n");
            }
            break;
        
        case tmp_pret :
            previous_pos = pos;
            pos = getPos(stockage_G,1) ;
            if(pos> (previous_pos-5)){
                etat = pret; 
                pc.printf("PRET\n");         
            } 
            break;
            
        case pret : 
            if((status_contact & MASK_CT_GAUCHE)){
                pos = getPos(stockage_G,1)+550;
                deverouillage_torque_convoyeurs_gauche();
                wait_us(500);
                positionControl(stockage_G,pos,1,BLED_ON,1);
                previous_pos = pos;
                etat = pousse;
                pc.printf("POUSSE\n");
            }
            break;
            
        case pousse :
            pos = getPos(stockage_G,1);
            if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){   
                pos = pos - 550;        
                deverouillage_torque_convoyeurs_gauche();
                wait_us(500);
                positionControl(stockage_G,pos,1,BLED_ON,1);
                previous_pos = pos;
                etat = retour;
                pc.printf("RETOUR\n");
            }
                
            break;
        case retour :
            pos = getPos(stockage_G,1) ;
            if(pos> (previous_pos-5)){
                etat = pret; 
                pc.printf("PRET\n");         
            } 
            break;
    }   
} 

void convoyeur_droit_jaune(void){
    
    typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
    static type_etat etat = init;
    static int16_t pos = 0;
    static int16_t previous_pos = getPos(stockage_D,3);
    
    
    switch(etat){
            
        case init : 
            velocityControl(stockage_D,512,BLED_ON,3);
                wait_us(500);
            pos = getPos(stockage_D,3);
            if(previous_pos != pos){
                etat = etalonnage;
                //pc.printf("ETALONNAGE\n");
            }
            break;

        case etalonnage :
            pos = getPos(stockage_D,3);
            if(pos>600 and (status_contact & MASK_FC_DROIT)){ 
                velocityControl(stockage_D,0,GLED_ON,3);
                wait_ms(500);
                pos = getPos(stockage_D,3) ;
                //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
                compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
                wait_us(500);
                etat = tmp_pret;
                //pc.printf("TMP_PRET\n");
            }
            break;
        
        case tmp_pret :
            previous_pos = pos;
            pos = getPos(stockage_D,3) ;
                wait_us(500);
            if(pos> (previous_pos+5)){
                etat = pret; 
                //pc.printf("PRET\n");         
            } 
            break;
            
        case pret : 
            if((status_contact & MASK_CT_DROIT)){
                //pc.printf("%d\n",pos);
                pos = getPos(stockage_D,3)+ 550;
                deverouillage_torque_convoyeurs_droit();
                wait_us(500);
                positionControl(stockage_D,pos,1,BLED_ON,3);
                wait_us(500);
                previous_pos = pos;
                etat = pousse;
                //pc.printf("POUSSE\n");
            }
            break;
            
        case pousse :
            pos = getPos(stockage_D,3);
                wait_us(500);
            if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){   
                //pc.printf("%d\n",pos);
                pos = pos - 550;        
                deverouillage_torque_convoyeurs_droit();
                wait_us(500);
                positionControl(stockage_D,pos,1,BLED_ON,3);
                wait_us(500);
                previous_pos = pos;
                etat = retour;
                //pc.printf("RETOUR\n");
            }
                
            break;
        
        case retour :
            pos = getPos(stockage_D,3) ;
                wait_us(500);
            if(pos< (previous_pos+5)){
                etat = pret; 
                //pc.printf("PRET\n");         
            } 
            break;
    }   
}

void convoyeur_gauche_violet(void){
    
   typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
    static type_etat etat = init;
    static int16_t pos = 0;
    static int16_t previous_pos = getPos(stockage_G,1);
    
    
    switch(etat){
            
        case init : 
            velocityControl(stockage_G,-512,BLED_ON,1);
                wait_us(500);
            pos = getPos(stockage_G,1);
                wait_us(500);
            if(previous_pos != pos){
                etat = etalonnage;
               // pc.printf("ETALONNAGE\n");
            }
            break;

        case etalonnage :
            pos = getPos(stockage_G,1);
            pc.printf("%d\n",pos);
            if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){ 
                velocityControl(stockage_G,0,GLED_ON,1);
                wait_ms(500);
                pos = getPos(stockage_G,1) ;
                wait_us(500);
                compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1);
                wait_us(500);
                previous_pos = pos;
                etat = tmp_pret;
                ////pc.printf("TMP_PRET\n");
            }
            break;
        
        case tmp_pret :
            pos = getPos(stockage_G,1) ;
                wait_us(500);
            if(pos> (previous_pos-5)){
                etat = pret; 
                pc.printf("PRET\n");         
            } 
            break;
            
        case pret : 
            if((status_contact & MASK_CT_GAUCHE)){
                //pc.printf("%d\n",pos);
                pos = getPos(stockage_G,1)- 550;
                wait_us(500);
                deverouillage_torque_convoyeurs_gauche();
                wait_us(500);
                positionControl(stockage_G,pos,1,BLED_ON,1);
                wait_us(500);
                previous_pos = pos;
                etat = pousse;
                //pc.printf("POUSSE\n");
            }
            break;
            
        case pousse :
            pos = getPos(stockage_G,1);
                wait_us(500);
            if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){   
                pc.printf("%d\n",pos);
                pos = pos + 550;        
                deverouillage_torque_convoyeurs_gauche();
                wait_us(500);
                positionControl(stockage_G,pos,1,BLED_ON,1);
                wait_us(500);
                previous_pos = pos;
                etat = retour;
                //pc.printf("RETOUR\n");
            }
                
            break;
        
        case retour :
            pos = getPos(stockage_G,1) ;
                wait_us(500);
            if(pos> (previous_pos-5)){
                etat = pret; 
                pc.printf("PRET\n");         
            } 
            break;
    }   
}

void convoyeur_droit_violet(void){
    
   typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
    static type_etat etat = init;
    static int16_t pos = 0;
    static int16_t previous_pos = getPos(stockage_D,3);
    
    
    switch(etat){
            
        case init : 
            velocityControl(stockage_D,+512,BLED_ON,3);
                wait_us(500);
            pos = getPos(stockage_D,3);
                wait_us(500);
            if(previous_pos != pos){
                //pc.printf("ETALONNAGE\n");
                etat = etalonnage;
            }
            break;

        case etalonnage :
            pos = getPos(stockage_D,3);
                wait_us(500);
            //pc.printf("%d\n", pos);
            if(pos>900 and (status_contact & MASK_FC_DROIT)){ 
                velocityControl(stockage_D,0,GLED_ON,3);
                wait_ms(500);
                pos = getPos(stockage_D,3) ;
                wait_us(500);
                positionControl(stockage_D,pos-180,1,BLED_ON,3);
                wait_us(500);
                previous_pos = pos;
                etat = tmp_pret;
               // pc.printf("TMP_PRET\n");
            }
            break;
        
        case tmp_pret :  
            pos = getPos(stockage_D,3) ;
                wait_us(500);
            if(pos< (previous_pos+5)){
                etat = pret; 
                //pc.printf("PRET\n");         
            } 
            break;
            
        case pret : 
            if((status_contact & MASK_CT_DROIT)){
                pos = getPos(stockage_D,3)- 550;
                wait_us(500);
                deverouillage_torque_convoyeurs_droit();
                wait_us(500);
                positionControl(stockage_D,pos,1,BLED_ON,3);
                wait_us(500);
                previous_pos = pos;
                etat = pousse;
                //pc.printf("POUSSE\n");
            }
            break;
            
        case pousse :
            pos = getPos(stockage_D,3);
                wait_us(500);
            if(pos<(previous_pos+5) and (status_contact & MASK_CT_DROIT)==0){   
                pos = pos + 550;        
                deverouillage_torque_convoyeurs_droit();
                wait_us(500);
                positionControl(stockage_D,pos,1,BLED_ON,3);
                wait_us(500);
                previous_pos = pos;
                etat = retour;
                //pc.printf("RETOUR\n");
            }
                
            break;
        case retour :
            pos = getPos(stockage_D,3) ;
                wait_us(500);
            if(pos> (previous_pos-5)){
                etat = pret; 
                //pc.printf("PRET\n");         
            } 
            break;
    }   
} 
void gabarit_robot(void)
{
    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};

    uint16_t pos_av_centre[2] = {550,550};//470,350


    int speed=100;

    deverouillage_torque();
    positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
    wait(0.5);
    

}


void fifo_couleur(void)
{

    typedef enum {n_atome, atome, tmp} type_etat ;
    static type_etat etat = n_atome;

    int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
    switch(etat) {
        case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
            if(etat_cap) {
                buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
                flag_ascenseur = 1;
                if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
                    oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
                }
                
                FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
                etat = tmp;
            }
            break;

        case tmp : //on attend que l'atome traité soit totalement passé
            if(!etat_cap) {
                etat = n_atome;
            }
            break;

    }

}


void ascenseur(void)
{
    typedef enum {init, atome, tmp} type_etat;
    static type_etat etat = init;

    int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
    static int flag_sortie = 0;
    
    switch(etat) {
        case init :
            //on attend le premier atome et place le herkulex en fonction
            if(flag_ascenseur) {
                //SendCharCan(HACHEUR_ID_COUROIES,1);
                etat = atome;
            }
            break;

        case atome :
            //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
            if(etat_cap != 0)
            {
                oriente_doigt(etat_cap);
                etat = tmp;
            }
            /*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
            {
                oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO);
            }*/
            break;

        case tmp :
            //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
            if(etat_cap == 0) {
                char memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
                FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
                if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
                {
                    pc.printf("Fifo vide\n");
                    if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
                        oriente_doigt(2);
                    else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
                        oriente_doigt(1);
                }
                etat = atome;
            }
            break;
    }
}

void oriente_doigt(int palet)
{
    if(cote) {
        if(palet == 1) {//position herkulex stockage bleu
            positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
            SendCharCan(HACHEUR_ID_COUROIES,2);
        } else if (palet == 2) {//position herkulex stockage rouge/vert
            positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
            SendCharCan(HACHEUR_ID_COUROIES,1);
            
        }
    } else {
        if(palet== 1) {//position herkulex stockage bleu
            positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
            SendCharCan(HACHEUR_ID_COUROIES,1);
        } else if (palet== 2) {//position herkulex stockage rouge/vert
            positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
            SendCharCan(HACHEUR_ID_COUROIES,2);
        }
    }
}

void presentoir_avant(void)
{
    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};

    uint16_t pos_av_centre[2] = {512,512};

    int speed=1;

    positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
    verification();
}


void automate_ventouse_presentoir_avant(void)
{
    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
    static type_etat etat = init;

    switch(etat) {
        case init: //attente d'initialisation
            if(fpresentoir_avant)
                etat=envoi_instruction;
            break;

        case envoi_instruction://envoi instruction
            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
            presentoir_avant();
            SendRawId(HACHEUR_GET_PRESENTOIR_AV);
            SendRawId(HACHEUR_STATUT_VENTOUSES);
            etat = attente_ack_ventouse;
            break;

        case attente_ack_ventouse:
            if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
                fpresentoir_avant=0;
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                etat = init;
            }
            break;

    }
}

void automate_ventouse_relache_avant(void)
{

    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
    static type_etat etat = init;

    switch(etat) {
        case init: //attente d'initialisation
            if(favant_relache)
                etat=envoi_instruction;
            break;

        case envoi_instruction://envoi instruction
            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
            SendRawId(HACHEUR_STATUT_VENTOUSES);
            etat = attente_ack_ventouse;
            break;

        case attente_ack_ventouse:
            SendRawId(HACHEUR_RELEASE_AV);
            if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
                favant_relache=0;
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                etat = init;
            }
            break;

    }

}

void goldenium_avant(void)
{

}

void automate_ventouse_goldenium_avant (void)
{
}

void accelerateur_avant(void)
{
}

void automate_ventouse_accelerateur_avant(void)
{

}
#endif