File content as of revision 7:ad4a19e26b84:

#include "mbed.h"
#include "AX12.h"
#include "actionneurs.h"
#include "global.h"

#include "ident_crac.h"

#define AX12_INITIALISATION 0
#define AX12_PREPARATION_PRISE 1
#define AX12_STOCKAGE_HAUT 2
#define AX12_STOCKAGE_BAS 3
#define AX12_DEPOSER 4
#define AX12_PREPARATION_DEPOT_BAS 5
#define AX12_PREPARATION_DEPOT_HAUT 6
#define AX12_POUSSER_MODULE 7
#define AX12_DEFAUT 20


#define SIZE_FIFO 25     


extern "C" void mbed_reset();//Pour pouvoir reset la carte

/****************************************************************************************/
/* FUNCTION NAME: canProcessRx                                                          */
/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
/****************************************************************************************/
void canProcessRx(void);

/****************************************************************************************/
/* FUNCTION NAME: canRx_ISR                                                             */
/* DESCRIPTION  : Interruption en réception sur le CAN                                  */
/****************************************************************************************/
void canRx_ISR (void);

/****************************************************************************************/
/* FUNCTION NAME: SendRawId                                                             */
/* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
/****************************************************************************************/
void SendRawId (unsigned short id);

/****************************************************************************************/
/* FUNCTION NAME: Fin_action                                                            */
/* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
/****************************************************************************************/
void Fin_action(void);

/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12                                                         */
/* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
/****************************************************************************************/
void AX12_automate(void);
   
        
                             /*  DECLARATION VARIABLES */
CAN can1(p30,p29);
CANMessage msgRxBuffer[SIZE_FIFO];
unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN

extern "C" void mbed_reset();//Pour pouvoir reset la carte

unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
short vitesse=700;
float angle=0.0;
float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
 
                  
                                        /* MAIN */                 

int main() 
{
    can1.frequency(1000000); // fréquence de travail 1Mbit/s
    can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN

    declarationAX12(); 

    //Demonstration bras gauche
    /*Initialisation_gauche();
    wait(1);
    Preparation_prise_gauche();
    wait(1);
    Prendre_module_gauche();
    wait(1);
    Preparation_module_gauche();
    wait(1);
    Tourner_module_gauche();
    wait(1);
    Preparation_module_gauche();
    wait(1);
    Tourner_module_gauche();
    wait(1);
    Preparation_module_gauche();
    wait(1);
    Tourner_module_gauche();*/
    float position =0;
     while(true) {
        //AX12_automate();
        canProcessRx();
        
    }
}    

                        /*              FONCTIONS                */
    
/****************************************************************************************/
/* 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();
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    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) {
        
        switch(msgRxBuffer[FIFO_lecture].id) {
            case CHECK_AX12:
                SendRawId(ALIVE_AX12);
                flag = 1;
                mbed_reset();
                break;
            
            case SERVO_AX12_ACTION : 
                etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
                
                //ACK de reception des actions a effectuer
                msgTx.id = SERVO_AX12_ACK;
                msgTx.len = 1;
                msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
                can1.write(msgTx);
                break;
            case 0x123:
                SendRawId(ALIVE_AX12);
                if(msgRxBuffer[FIFO_lecture].data[0] == 0){
                    getPosiotionCentrale();
                }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){
                    getPosiotionGauche();
                }else{
                    getPosiotionDroite();
                }
                break;
            case 0x456:
                //Demonstration pince centrale
                switch(msgRxBuffer[FIFO_lecture].data[0]){
                case 0:
                    Initialisation_position();
                    break;
                case 1:
                    Preparation_prise();
                    break;
                case 2:
                    Stockage_haut();
                    break;
                case 3:
                    Stockage_bas();
                    break;
                case 4:
                    Pousser_module();
                    break;
                case 5:
                    Preparation_depot_bas();
                    break;
                case 6:
                    Deposer();
                    break;
                case 7:
                    Preparation_depot_haut();
                    break;
                
                }
                wait(1);
                break;
        }
                    
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
    }
}    
 
/****************************************************************************************/
/* FUNCTION NAME: canRx_ISR                                                             */
/* DESCRIPTION  : Interruption en réception sur le CAN                                  */
/****************************************************************************************/
void canRx_ISR (void)
{
    if (can1.read(msgRxBuffer[FIFO_ecriture])) {
        if(msgRxBuffer[FIFO_ecriture].id==0x65) mbed_reset();
        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
    }
} 
  
/****************************************************************************************/
/* FUNCTION NAME: SendRawId                                                             */
/* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
/****************************************************************************************/
void SendRawId (unsigned short id)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=id;
    msgTx.len=0;
    can1.write(msgTx);
}  
  


/****************************************************************************************/
/* FUNCTION NAME: Fin_action                                                            */
/* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
/****************************************************************************************/
void Fin_action(void){
    CANMessage msgTx=CANMessage();
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    
    msgTx.id = SERVO_AX12_END;
    msgTx.len = 1;  
    msgTx.data[0] = AX12_PREPARATION_PRISE;
    can1.write(msgTx);  
}

/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12                                                         */
/* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
/****************************************************************************************/
void AX12_automate(void){
    switch(etat_ax12){
        case AX12_INITIALISATION :
            if (flag == 1){
                Initialisation_position();
                flag = 2;
            }
            
            break;
                        
        case AX12_PREPARATION_PRISE :
            Preparation_prise();
            if (action == 0){
                Fin_action();
                action ++;
            }
            break;
                        
        case AX12_STOCKAGE_HAUT :
            Stockage_haut();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_STOCKAGE_BAS :
            Stockage_bas();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_DEPOSER :
            Deposer();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_PREPARATION_DEPOT_BAS :
            Preparation_depot_bas();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_PREPARATION_DEPOT_HAUT :
            Preparation_depot_haut();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_POUSSER_MODULE :
            Pousser_module();
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
        
        case AX12_DEFAUT :
        action = 0;
            break;
    }
}