File content as of revision 0:80df663dd15e:

/**************************************************************************************************/
/* cette fonction de controle de l'MX12 est réalisée à partir de la même bibliothèque que les AX12*/
/**************************************************************************************************/

#include "mbed.h"
#include "AX12.h"
#include "cmsis.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 TIME 0.8
#define TOLERANCE_AX12 50
#define SIZE_FIFO 25        
        
                             /*  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;
static float TAB_ANGLE1[4], TAB_ANGLE2[4];
static char TAB_POSITION[4];
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;  
AX12 *un_myMX12;
                                 
                                 
                    /*       PROTOTYPES DE FONCTIONS ET POINTEURS       */
                    
  
/****************************************************************************************/
/* FUNCTION NAME: CAN2_wrFilter                                                         */
/* DESCRIPTION  : Fonction qui permet de ne garder que les ID qui nous interessent      */
/****************************************************************************************/                    
void CAN2_wrFilter (uint32_t id);                    
                                  
/****************************************************************************************/
/* 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);

/****************************************************************************************/
/* FUNCTION NAME: Initialisation                                               */
/* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
/****************************************************************************************/
void Initialisation(void);

/****************************************************************************************/
/* FUNCTION NAME: bouger_MX12                                                           */
/* DESCRIPTION  : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge    */
/****************************************************************************************/
void bouger_MX12(unsigned char choix);

Timer t;
Ticker flipper;

                   
                            /*   ANGLE   */
                            
/*              0 = tourner à droite
                600 = position initiale
                1200 = tourner à gauche
                                                             */                   

                            /*  NUMERO MX12 : 0x01 */

                  
                                        /* 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
    CAN2_wrFilter(SERVO_AX12_ACTION); // 0x96 Trie des messages CAN sur les paramètres suivants.
    CAN2_wrFilter(SERVO_AX12_ACK);  // 0x106
    CAN2_wrFilter(SERVO_AX12_END);  // 0x116
    CAN2_wrFilter(CHECK_AX12);      // 0x65

    // Set l'ID du MX12 et la vitesse de communication. Paramètres : tx rx id baud
    un_myMX12 = new AX12(p9, p10, 1, 1000000);   //Trappe de fermeture du lanceur.

     while(true) {
        AX12_automate();// Trie les actions de l'MX12
        canProcessRx(); // Traitement des trames CAN en attente  
    }
}    

                        /*              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(); // Set la structure d'un message CAN Tx : id, data, taille, type, format
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
    
    if(FIFO_occupation<0)
        FIFO_occupation=FIFO_occupation+SIZE_FIFO; //Taille du CANRx.
    
    if(FIFO_max_occupation<FIFO_occupation) 
        FIFO_max_occupation=FIFO_occupation;
    
    if(FIFO_occupation!=0) {
        
        switch(msgRxBuffer[FIFO_lecture].id) {
            case CHECK_AX12: // 0x065
                SendRawId(ALIVE_AX12);
                flag = 1;
                break;
            
            case SERVO_AX12_ACTION : // 0x96
                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;
        }
                    
        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==RESET_STRAT) mbed_reset();
        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
    }
} 
  
/****************************************************************************************/
/* FUNCTION NAME: SendRawId                                                             */
/* DESCRIPTION  : Fonction qui permet d'envoyer une trame vide à un ID                    */
/****************************************************************************************/
void SendRawId (unsigned short id)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=id;
    msgTx.len=0;
    can1.write(msgTx);
}  
 

/****************************************************************************************/
/* FUNCTION NAME: Initialisation                                               */
/* DESCRIPTION  : Place le MX12 en position initiale : lanceur fermé                    */
/****************************************************************************************/
void Initialisation(){
    un_myMX12-> Set_Goal_speed(vitesse);
    un_myMX12-> Set_Mode(0);
}

/****************************************************************************************/
/* FUNCTION NAME: bouger_MX12                                                           */
/* DESCRIPTION  : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge    */
/****************************************************************************************/
void bouger_MX12(unsigned char choix){
    AX12 *ptr_myAX12;
    ptr_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
    if( choix == 1) // Tourne à droite
    {
            ptr_myAX12->Set_Secure_Goal(0); // tourner droite
    }
    else if(choix == 2) // Tourne à gauche
    {
            ptr_myAX12->Set_Secure_Goal(1200); // tourner gauche
    }
    else if (choix == 0)
    {
        ptr_myAX12->Set_Secure_Goal(600); // position initiale
    }
}

/****************************************************************************************/
/* 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);  // Envoie le message via le CAN.
}

/****************************************************************************************/
/* 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 : // Etat = 0
            if (flag == 1){ // Décide si il faut initialiser le bras droit ou gauche
                Initialisation();
                flag = 2;
            }
            break;
                        
        case AX12_PREPARATION_PRISE : // Etat = 1
            bouger_MX12(msgRxBuffer[FIFO_lecture].data[1]);
            if (action == 0){
                Fin_action();
                action ++;
            }
            break;
        case AX12_DEFAUT :
        action = 0;
            break;
    }
}

/****************************************************************************************/
/* FUNCTION NAME: CAN2_wrFilter                                                         */
/* DESCRIPTION  : Fonction qui permet de ne garder que les ID qui nous interessent      */
/****************************************************************************************/
void CAN2_wrFilter (uint32_t id)  {
    static int CAN_std_cnt = 0;
    uint32_t buf0, buf1;
    int cnt1, cnt2, bound1;
 
    /* Acceptance Filter Memory full */
    if (((CAN_std_cnt + 1) >> 1) >= 512)
        return;                                       /* error: objects full */
 
    /* Setup Acceptance Filter Configuration
      Acceptance Filter Mode Register = Off  */
    LPC_CANAF->AFMR = 0x00000001;
 
    id |= 1 << 13;                        /* Add controller number(2) */
    id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
 
    if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
        LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
    }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
        if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
        else
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
    }  else  {
        /* Find where to insert new ID */
        cnt1 = 0;
        cnt2 = CAN_std_cnt;
        bound1 = (CAN_std_cnt - 1) >> 1;
        while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
            if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
                cnt2 = cnt1 * 2;
                break;
            }
            if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
                cnt2 = cnt1 * 2 + 1;
                break;
            }
            cnt1++;                                  /* cnt1 = U32 where to insert new ID */
        }                                          /* cnt2 = U16 where to insert new ID */
 
        if (cnt1 > bound1)  {                      /* Adding ID as last entry */
            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
            else                                     /* Odd  number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
        }  else  {
            buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
            if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
                buf1 = (id << 16) | (buf0 >> 16);
            else                                     /* Insert new mask to odd  address */
                buf1 = (buf0 & 0xFFFF0000) | id;
 
            LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
 
            bound1 = CAN_std_cnt >> 1;
            /* Move all remaining standard mask entries one place up */
            while (cnt1 < bound1)  {
                cnt1++;
                buf1  = LPC_CANAF_RAM->mask[cnt1];
                LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
                buf0  = buf1;
            }
 
            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
        }
    }
    CAN_std_cnt++;
 
    /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
    buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
    buf1 = buf0;
 
    /* Setup acceptance filter pointers */
    LPC_CANAF->SFF_sa     = 0;
    LPC_CANAF->SFF_GRP_sa = buf0;
    LPC_CANAF->EFF_sa     = buf0;
    LPC_CANAF->EFF_GRP_sa = buf1;
    LPC_CANAF->ENDofTable = buf1;
 
    LPC_CANAF->AFMR = 0x00000000;                  /* Use acceptance filter */
} // CAN2_wrFilter

/****************************************************************************************/
/* FUNCTION NAME: Check_positionAX12                                                    */
/* DESCRIPTION  : Fonction qui permet de verifier la position de l'MX12                 */
/****************************************************************************************/
void Check_positionAX12(char* TAB, unsigned char choix){
    static float TAB_POS_TH[4];
    
    CANMessage msgTx=CANMessage();
    msgTx.id=SERVO_AX12_POSITION;
    msgTx.len=5;
    
    //PERMET DE VERIFIER LA POSITION De l'MX12
    TAB_ANGLE1[0] = (unsigned short)(un_myMX12->Get_Position()/0.3);


        TAB_POS_TH[0] = (unsigned short) TAB[0] + ((unsigned short)TAB[1]<<8);    
    
        if (choix == 1){
            if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
                TAB_POSITION[0] = 1;
                }
            else if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
                TAB_POSITION[0] = 0;
                }
            }
        else if (choix == 2){
            if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
                TAB_POSITION[0] = 1;
                }
            else if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
                TAB_POSITION[0] = 0;
                }
     }
    
    msgTx.data[0] = choix;
    msgTx.data[0] = TAB_POSITION[0];
    can1.write(msgTx);
}