Programme d'utilisation servomotors MX12 V1
Diff: main.cpp
- Revision:
- 0:80df663dd15e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri May 19 14:32:14 2017 +0000 @@ -0,0 +1,391 @@ +/**************************************************************************************************/ +/* 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); +} + \ No newline at end of file