File content as of revision 18:972d29c668a5:

#include "extern.h"
#define SIZE_FIFO 50




CANMessage msgRxBuffer[SIZE_FIFO];
unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
char end_game = 0;

Ventouse* ventouse[NB_VENTOUSES];

BusIn contacts(PC_11, PC_12, PB_4, PB_5, PC_13, PC_14, PC_2, PC_3); 
DigitalIn Contact1(PC_11, PullNone);
DigitalIn Contact2(PC_12, PullNone);
DigitalIn Contact3(PB_4, PullNone);
DigitalIn Contact4(PB_5, PullNone);
DigitalIn Contact5(PC_13, PullNone);
DigitalIn Contact6(PC_14, PullNone);
DigitalIn Contact7(PC_2, PullNone);
DigitalIn Contact8(PC_3, PullNone);

void lecture_contacts(void);

#ifdef ROBOT_SMALL
         
#endif

#ifdef ROBOT_BIG

PwmOut ascenseur[8] = {PC_9, PA_8, PA_9, PA_10, PC_7, PC_8, PB_10 ,PB_2};
PwmOut* ascenseur_D = ascenseur;
PwmOut* ascenseur_G = ascenseur+4;

//ascenseur_D = ascenseur;
//ascenseur_G = ascenseur + 4;
    
#define VIT_COURROIE_HAUT_V 0.5
#define VIT_COURROIE_HAUT_L 0.4
#define VIT_COURROIE_BAS_V 0.6
#define VIT_COURROIE_BAS_L 0.5

void set_ascensseur(int allume);
#endif



void get_etat_ventouses();


CAN can(PB_8,PB_9,1000000);
Serial pc(USBTX,USBRX);

/*********************************************************************************************************/
/* FUNCTION NAME: canRx_ISR                                                                              */
/* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                                 */
/*********************************************************************************************************/
void canRx_ISR (void)
{
    if (can.read(msgRxBuffer[FIFO_ecriture]))
        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
}


/*********************************************************************************************************/
/* FUNCTION NAME: canProcessRx                                                                           */
/* DESCRIPTION  : Fonction de traitement des messages CAN                                                */
/*********************************************************************************************************/
void canProcessRx(void){
    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
    CANMessage msgTx=CANMessage();
    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
    if(FIFO_occupation<0)
        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
    if(FIFO_max_occupation<FIFO_occupation)
        FIFO_max_occupation=FIFO_occupation;
    if(FIFO_occupation!=0) {
    int identifiant=msgRxBuffer[FIFO_lecture].id;
    char BRAS = 0;
    
        switch(identifiant){
            
            case HACHEUR_GET_ATOM:
                BRAS = msgRxBuffer[FIFO_lecture].data[0];
                ventouse[BRAS]->action(1);
                break;
                
            case HACHEUR_RELEASE_ATOM:
                BRAS = msgRxBuffer[FIFO_lecture].data[0];
                ventouse[BRAS]->action(0);
                break;   
            
            case HACHEUR_STATUT_VENTOUSES:
                get_etat_ventouses();
                break;
            
            case GLOBAL_GAME_END:
                end_game = 1;
                break;
                
            case HACHEUR_ETAT_CONTACTS:
                char contacts_tmp = contacts;
                can.write(CANMessage(HACHEUR_ETAT_CONTACTS,&contacts_tmp,1));
                break;
            
            #ifdef ROBOT_SMALL
                       
            
            case HACHEUR_GET_PRESENTOIR_AR:           
                ventouse[AR_GAUCHE]->action(1);
                ventouse[AR_CENTRE]->action(1);
                ventouse[AR_DROIT]->action(1);
                break;
            
            case HACHEUR_RELEASE_AR:           
                ventouse[AR_GAUCHE]->action(0);
                ventouse[AR_CENTRE]->action(0);
                ventouse[AR_DROIT]->action(0);
                break;
                
            case HACHEUR_GET_PRESENTOIR_AV :
                ventouse[AV_GAUCHE]->action(1);
                ventouse[AV_CENTRE]->action(1);
                ventouse[AV_DROIT]->action(1);
                break;
            
            case HACHEUR_RELEASE_AV :
                ventouse[AV_GAUCHE]->action(0);
                ventouse[AV_CENTRE]->action(0);
                ventouse[AV_DROIT]->action(0);
                break;
                
            

            #endif

            #ifdef ROBOT_BIG            
            
            case HACHEUR_GET_PRESENTOIR_AV :
                ventouse[AV_GAUCHE]->action(1);
                ventouse[AV_CENTRE]->action(1);
                ventouse[AV_DROIT]->action(1);
                break;
            
            case HACHEUR_RELEASE_AV :
                ventouse[AV_GAUCHE]->action(0);
                ventouse[AV_CENTRE]->action(0);
                ventouse[AV_DROIT]->action(0);
                break;
            
            case HACHEUR_ID_COUROIES :
                set_ascensseur(msgRxBuffer[FIFO_lecture].data[0]);
                break;
            
            #endif
        }
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
    }
}

int main() {
    
    can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
    wait(1);
    #ifdef ROBOT_SMALL
    pc.printf("\nPetit robot\n\n");
    ventouse[AV_GAUCHE] = new Ventouse(PB_7, PB_6, PC_0, AV_GAUCHE);
    ventouse[AR_CENTRE] = new Ventouse(PC_9, PA_8, PB_0, AR_CENTRE);
    ventouse[AR_DROIT] = new Ventouse(PA_9, PA_10,  PB_1, AR_DROIT);
    //ventouse[AR_BAS] = new Ventouse(PA_11, PA_15, PC_1, AR_BAS);
    ventouse[AR_BAS] = new Ventouse(PA_11, PC_1, AR_BAS);
    
    ventouse[AV_DROIT] = new Ventouse(PA_0, PA_1, PA_4, AV_DROIT);
    ventouse[AV_CENTRE] = new Ventouse(PC_7,PC_8 , PC_5, AV_CENTRE);
    ventouse[AR_GAUCHE] = new Ventouse(PB_10, PB_2, PC_4, AR_GAUCHE);
    //ventouse[AV_BAS] = new Ventouse(PA_6, PA_5, PA_7, AV_BAS); 
    ventouse[AV_BAS] = new Ventouse(PA_6, PA_7, AV_BAS); 
    

    #endif    
    
    #ifdef ROBOT_BIG
    pc.printf("\nGros robot\n\n");
    ventouse[AV_GAUCHE] = new Ventouse(PA_6, PA_5, PA_7, AV_GAUCHE);
    ventouse[AV_CENTRE] = new Ventouse(PA_0, PA_1, PA_4, AV_CENTRE);
    ventouse[AV_DROIT] = new Ventouse(PA_11, PA_15, PC_1, AV_DROIT);
    
    for (int i=0; i<8; i++){
        ascenseur[i].period_us(50);
    }
        
    
    
    set_ascensseur(0);
    wait(1);
    #endif
    
    pc.printf("LAUNCHED");
    while(1) {
        canProcessRx();
        for(int i=0; i<NB_VENTOUSES; i++){
            ventouse[i]->automate();
        }            
        lecture_contacts();
            

        if(end_game){
            for(int i=0; i<NB_VENTOUSES;i++){
                ventouse[i]->setPompe(0.0);
                ventouse[i]->setElectrovanne(0);
            }
#ifdef ROBOT_BIG
            set_ascensseur(0);
#endif
            while(1){;}
        }
    }
}

void lecture_contacts(void)
{
    static char prev_contacts = contacts;
    
    if(contacts != prev_contacts)
    {
        prev_contacts = contacts;
        can.write(CANMessage(HACHEUR_ETAT_CONTACTS,&prev_contacts,1));
    }
}
#ifdef ROBOT_BIG
void set_ascensseur(int allume){
    /*PwmOut* vitH, vitL;
    switch(allume){
        case 1:
            vitH = ascenseur_G;
            vitL = ascenseur_D;
            break;
        case 2:
            vitH = ascenseur_G;
            vitL = ascenseur_D;
            break;
        default:
            vitG = 0;
            vitD = 0;
            break;
    }
    for (int i=0; i<3; i++)
    {
        vitL[i] = VIT_COURROIE_BAS_L;
        vitH[i] = VIT_COURROIE_BAS_V;
    }
    
    vitL[3] = VIT_COURROIE_HEUT_L;
    vitH[3] = VIT_COURROIE_HAUT_V;*/

    
    
   // float vitG, vitD, vitHG, vitHD;
    
    switch(allume){
        case 1:
        /*
            vitG = VIT_COURROIE_BAS_V;
            vitD = VIT_COURROIE_BAS_L;
            vitHG = VIT_COURROIE_HAUT_V;
            vitHD = VIT_COURROIE_HEUT_L;*/
            for (int i=0; i<3; i++)
            {
                ascenseur_D[i] = VIT_COURROIE_BAS_L;
                ascenseur_G[i] = VIT_COURROIE_BAS_V;
            }
        
            ascenseur_D[3] = VIT_COURROIE_HAUT_L;
            ascenseur_G[3] = VIT_COURROIE_HAUT_V;
            break;
        case 2:
        /*
            vitG = VIT_COURROIE_BAS_L;
            vitD = VIT_COURROIE_BAS_V;
            vitHG = VIT_COURROIE_HAUT_L;
            vitHD = VIT_COURROIE_HEUT_V;*/
            for (int i=0; i<3; i++)
            {
                ascenseur_D[i] = VIT_COURROIE_BAS_V;
                ascenseur_G[i] = VIT_COURROIE_BAS_L;
            }
        
            ascenseur_D[3] = VIT_COURROIE_HAUT_V;
            ascenseur_G[3] = VIT_COURROIE_HAUT_L;
            break;
        default:    
            for (int i=0; i<4; i++)
            {
                ascenseur_D[i] = 0;
                ascenseur_G[i] = 0;
            }
            break;
    }
    
    

    
}
#endif

void get_etat_ventouses()
{   
    CANMessage etat_ventouses = CANMessage();
    etat_ventouses.id=HACHEUR_STATUT_VENTOUSES;      
    etat_ventouses.len=2;
    etat_ventouses.format=CANStandard;
    etat_ventouses.type=CANData;
    etat_ventouses.data[0]=0;
    etat_ventouses.data[0]=1;
    for(int i = (NB_VENTOUSES - 1); i >= 0; i--){
        etat_ventouses.data[0] = (etat_ventouses.data[0]<<1) + ventouse[i]->getPompe();
        etat_ventouses.data[1] = (etat_ventouses.data[1]<<1) + ventouse[i]->getPression();       
    }
    can.write(etat_ventouses);

}