File content as of revision 3:a630a1ccf5f0:

#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


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
DigitalIn presence_droit(PB_5);
DigitalIn presence_gauche(PB_4);


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





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);

}


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) {
                    if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
                        positionControl(doigt,384,10,BLED_ON,2);   //position herkulex stockage bleu
                    } else {
                        positionControl(doigt,640,10,GLED_ON,2);   //position herkulex stockage rouge/vert
                    }
                }
                //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
                FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
                etat = tmp;
            }
            break;

        case atome : //on stocke dans la fifo la couleur de l'atome
            buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
            flag_ascenseur = 1;
            /*if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu
                } else {
                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert
                }*/
            //pc.printf("fifo ecriture %d\n", 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) {
                /*if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {
                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu
                } else {
                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert
                }*/
                etat = atome;
            }
            break;

        case atome :
            //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
            if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
                if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) {
                    /*if(flag_sortie) {
                        if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
                            positionControl(doigt,384,10,BLED_ON,2);
                        } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
                            positionControl(doigt,640,10,GLED_ON,2);
                        }
                        flag_sortie = 0;
                    }
                    pc.printf("--fifo lecture %d\n", buffer_couleur_bas[FIFO_couleur_lecture]);
                    //on change la position du herkulex si le prochain atome est différent
                    if(buffer_couleur_bas[FIFO_couleur_lecture] != buffer_couleur_bas[FIFO_couleur_lecture+1] ) {
                        flag_sortie = 1;
                    }*/
                 if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
                        positionControl(doigt,384,10,BLED_ON,2);
                    } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
                        positionControl(doigt,640,10,GLED_ON,2);
                    }
                    etat = tmp;
                } else if(etat_cap !=  0) {
                    //pc.printf("--fifo lecture %d --- vu : %d\n", buffer_couleur_bas[FIFO_couleur_lecture], etat_cap);
                    etat = tmp;
                }
            }
            break;

        case tmp :
            //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
            if(etat_cap == 0) {
                //pc.printf("\t\tfifo + 1\n");
                FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
                etat = atome;
            }
            break;
    }
}

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