AX12 gros robot
Fork of command_AX12_gros_robot by
Diff: main.cpp
- Revision:
- 4:92d6a49cd2c5
- Parent:
- 3:6cbd7d2c78f4
--- a/main.cpp Wed Apr 19 15:53:45 2017 +0000 +++ b/main.cpp Thu May 11 12:58:38 2017 +0000 @@ -3,8 +3,19 @@ #include "cmsis.h" #include "ident_crac.h" -#define SIZE_FIFO 14 +#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); @@ -14,7 +25,9 @@ extern "C" void mbed_reset();//Pour pouvoir reset la carte -unsigned char action = 0, choix = 0; +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; @@ -23,8 +36,13 @@ /* 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 */ @@ -44,10 +62,22 @@ 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_position */ /* DESCRIPTION : Fonction qui place les bras en position verticale */ /****************************************************************************************/ -void Initialisation_position(void); +void Initialisation_position(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Preparation_prise */ @@ -77,13 +107,13 @@ /* FUNCTION NAME: Preparation_depos_bas */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ /****************************************************************************************/ -void Preparation_depos_bas(unsigned char choix); +void Preparation_depot_bas(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Preparation_depos_haut */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ /****************************************************************************************/ -void Preparation_depos_haut(unsigned char choix); +void Preparation_depot_haut(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Pousser_module */ @@ -91,14 +121,20 @@ /****************************************************************************************/ void Pousser_module(unsigned char choix); +/****************************************************************************************/ +/* FUNCTION NAME: Check_positionAX12 */ +/* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ +/****************************************************************************************/ +void Check_positionAX12(char* TAB, unsigned char choix); + Timer t; Ticker flipper; -///////////TABLEAU PINCE DROITE /////////////////////////// +///////////TABLEAU PINCE DROITE /////////////////////////// --> choix_bras == 1 static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 0x04,0xF4, 0x01, 0xFF, 0x00, - 0x07,0x50, 0x00, 0xFF, 0x00, - 0x0F,0xB0, 0x03, 0xFF, 0x00}; + 0x07,0x00, 0x01, 0xFF, 0x00, + 0x0F,0x00, 0x03, 0xFF, 0x00}; static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise 0x04,0xE9, 0x00, 0xFF, 0x03, @@ -107,18 +143,18 @@ static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 0x04,0xE9, 0x00, 0xFF, 0x03, - 0x07,0xB5, 0x00, 0xFF, 0x03, - 0x0F,0x39, 0x03, 0xFF, 0x03}; + 0x07,0xD5, 0x00, 0xFF, 0x00, + 0x0F,0x4D, 0x03, 0xFF, 0x00}; static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) 0x04,0x00, 0x02, 0xFF, 0x03, - 0x07,0xB5, 0x00, 0xFF, 0x03, - 0x0F,0x39, 0x03, 0xFF, 0x03}; + 0x07,0xD5, 0x00, 0xFF, 0x00, + 0x0F,0x4D, 0x03, 0xFF, 0x00}; static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) 0x04,0x60, 0x01, 0xFF, 0x00, - 0x07,0xB5, 0x00, 0xFF, 0x00, - 0x0F,0x39, 0x03, 0xFF, 0x00}; + 0x07,0xD5, 0x00, 0xFF, 0x00, + 0x0F,0x4D, 0x03, 0xFF, 0x00}; static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 0x04,0x50, 0x01, 0xFF, 0x03, @@ -126,36 +162,36 @@ 0x0F,0xDD, 0x02, 0xFF, 0x03}; static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) - 0x04,0xDE, 0x03, 0xFF, 0x03, - 0x07,0xB5, 0x00, 0xFF, 0x03, - 0x0F,0x39, 0x03, 0xFF, 0x03}; + 0x04,0x50, 0x03, 0xFF, 0x03, + 0x07,0xD5, 0x00, 0xFF, 0x03, + 0x0F,0x4D, 0x03, 0xFF, 0x03}; -static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras) +static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer (descendre bras) 0x04,0x00, 0x03, 0xFF, 0x00, - 0x07,0xB5, 0x00, 0xFF, 0x00, - 0x0F,0x39, 0x03, 0xFF, 0x00}; + 0x07,0xD5, 0x00, 0xFF, 0x00, + 0x0F,0x4D, 0x03, 0xFF, 0x00}; -static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte) +static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer (pince ouverte) 0x04,0x00, 0x03, 0xFF, 0x03, - 0x07,0xF4, 0x01, 0xFF, 0x03, - 0x0F,0xF4, 0x01, 0xFF, 0x03}; + 0x07,0x0A, 0x01, 0xFF, 0x03, + 0x0F,0xDD, 0x02, 0xFF, 0x03}; -static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut) +static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer (preparation module haut) 0x04,0x00, 0x02, 0xFF, 0x00, - 0x07,0xB5, 0x00, 0xFF, 0x00, - 0x0F,0x39, 0x03, 0xFF, 0x00}; + 0x07,0xD5, 0x00, 0xFF, 0x00, + 0x0F,0x4D, 0x03, 0xFF, 0x00}; static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) 0x04,0x00, 0x03, 0xFF, 0x00, - 0x07,0xB5, 0x00, 0xFF, 0x00, - 0x0F,0x39, 0x03, 0xFF, 0x00}; + 0x07,0x0A, 0x01, 0xFF, 0x03, + 0x0F,0xDD, 0x02, 0xFF, 0x03}; -//////////////////TABLEAU PINCE GAUCHE ///////////////////////////// +//////////////////TABLEAU PINCE GAUCHE ///////////////////////////// --> choix_bras == 2 static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 0x03,0xF4, 0x01, 0xFF, 0x00, - 0x0D,0x50, 0x00, 0xFF, 0x00, - 0x0E,0xB0, 0x03, 0xFF, 0x00}; + 0x0D,0x00, 0x01, 0xFF, 0x00, + 0x0E,0x00, 0x03, 0xFF, 0x00}; static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise 0x03,0xE9, 0x00, 0xFF, 0x03, @@ -164,18 +200,18 @@ static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 0x03,0xE9, 0x00, 0xFF, 0x03, - 0x0D,0xB5, 0x00, 0xFF, 0x03, - 0x0E,0x39, 0x03, 0xFF, 0x03}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) 0x03,0x00, 0x02, 0xFF, 0x03, - 0x0D,0xB5, 0x00, 0xFF, 0x03, - 0x0E,0x39, 0x03, 0xFF, 0x03}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) 0x03,0x60, 0x01, 0xFF, 0x00, - 0x0D,0xB5, 0x00, 0xFF, 0x00, - 0x0E,0x39, 0x03, 0xFF, 0x00}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 0x03,0x50, 0x01, 0xFF, 0x03, @@ -183,29 +219,29 @@ 0x0E,0xDD, 0x02, 0xFF, 0x03}; static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) - 0x03,0xDE, 0x03, 0xFF, 0x03, - 0x0D,0xB5, 0x00, 0xFF, 0x03, - 0x0E,0x39, 0x03, 0xFF, 0x03}; + 0x03,0x50, 0x03, 0xFF, 0x03, + 0x0D,0xD5, 0x00, 0xFF, 0x03, + 0x0E,0x4D, 0x03, 0xFF, 0x03}; -static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras) +static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer(descendre bras) 0x03,0x00, 0x03, 0xFF, 0x00, - 0x0D,0xB5, 0x00, 0xFF, 0x00, - 0x0E,0x39, 0x03, 0xFF, 0x00}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; -static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte) +static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer(pince ouverte) 0x03,0x00, 0x03, 0xFF, 0x03, 0x0D,0xF4, 0x01, 0xFF, 0x03, 0x0E,0xF4, 0x01, 0xFF, 0x03}; -static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut) +static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer(preparation module haut) 0x03,0x00, 0x02, 0xFF, 0x00, - 0x0D,0xB5, 0x00, 0xFF, 0x00, - 0x0E,0x39, 0x03, 0xFF, 0x00}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) 0x03,0x00, 0x03, 0xFF, 0x00, - 0x0D,0xB5, 0x00, 0xFF, 0x00, - 0x0E,0x39, 0x03, 0xFF, 0x00}; + 0x0D,0xD5, 0x00, 0xFF, 0x00, + 0x0E,0x4D, 0x03, 0xFF, 0x00}; /* ANGLE */ @@ -241,11 +277,11 @@ { 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); + CAN2_wrFilter(SERVO_AX12_ACK); + CAN2_wrFilter(SERVO_AX12_END); + CAN2_wrFilter(CHECK_AX12); - CANMessage TrameTx = CANMessage(); - TrameTx.format=CANStandard; - TrameTx.type=CANData; - trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche @@ -256,26 +292,12 @@ quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite - multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); - multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); + multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); //pince droite + multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); //pince gauche - //Initialisation de la position du bras - Initialisation_position(); - while(true) { + AX12_automate(); canProcessRx();//Traitement des trames CAN en attente - - if (action == 0){ - - /* - TrameTx.id = 0x092; - TrameTx.len = 1; - TrameTx.data[0] = 0; - can1.write(TrameTx); - */ - - action = 2; - } } } @@ -288,23 +310,36 @@ 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 SERVO_AX12_PROCESS : - action = 1; + case CHECK_AX12: + SendRawId(ALIVE_AX12); + flag = 1; break; + + case SERVO_AX12_ACTION : + etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; + choix_bras = msgRxBuffer[FIFO_lecture].data[1]; - case CHECK_ACTIONNEURS: - SendRawId (ALIVE_ACTIONNEURS); + //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; @@ -364,11 +399,17 @@ /* FUNCTION NAME: Initialisation_position */ /* DESCRIPTION : Fonction qui place les bras en position verticale */ /****************************************************************************************/ -void Initialisation_position(void){ - multiple_myAX12->multiple_goal_and_speed(4,TAB1); - wait(TIME); - multiple2_myAX12->multiple_goal_and_speed(4,TAB21); - wait(TIME); +void Initialisation_position(unsigned char choix){ + if (choix == 1){ + multiple_myAX12->multiple_goal_and_speed(4,TAB1); + wait(TIME); + } + + else if (choix == 2){ + multiple2_myAX12->multiple_goal_and_speed(4,TAB21); + wait(TIME); + } + } /****************************************************************************************/ @@ -452,10 +493,10 @@ } /****************************************************************************************/ -/* FUNCTION NAME: Preparation_depos_bas */ +/* FUNCTION NAME: Preparation_depot_bas */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ /****************************************************************************************/ -void Preparation_depos_bas(unsigned char choix){ +void Preparation_depot_bas(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB8); wait(TIME); @@ -468,10 +509,10 @@ } /****************************************************************************************/ -/* FUNCTION NAME: Preparation_depos_haut */ +/* FUNCTION NAME: Preparation_depot_haut */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ /****************************************************************************************/ -void Preparation_depos_haut(unsigned char choix){ +void Preparation_depot_haut(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB6); wait(TIME); @@ -511,5 +552,227 @@ } } +/****************************************************************************************/ +/* 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(1); + //Check_positionAX12(&TAB1[25], 1); + Initialisation_position(2); + //Check_positionAX12(&TAB21[25], 2); + flag = 2; + } + else if (choix_bras > 1){ + Initialisation_position(choix_bras); + } + break; + + case AX12_PREPARATION_PRISE : + Preparation_prise(choix_bras); + if (action == 0){ + Fin_action(); + action ++; + } + break; + + case AX12_STOCKAGE_HAUT : + Stockage_haut(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_action(); + break; + + case AX12_STOCKAGE_BAS : + Stockage_bas(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_action(); + break; + + case AX12_DEPOSER : + Deposer(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_action(); + break; + + case AX12_PREPARATION_DEPOT_BAS : + Preparation_depot_bas(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_action(); + break; + + case AX12_PREPARATION_DEPOT_HAUT : + Preparation_depot_haut(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_action(); + break; + + case AX12_POUSSER_MODULE : + Pousser_module(choix_bras); + etat_ax12 = AX12_DEFAUT; + Fin_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 des AX12 */ +/****************************************************************************************/ +void Check_positionAX12(char* TAB, unsigned char choix){ + int k=1, i=0; + static float TAB_POS_TH[4]; + + CANMessage msgTx=CANMessage(); + msgTx.id=SERVO_AX12_POSITION; + msgTx.len=5; + + //PERMET DE VERIFIER LA POSITION D'UN AX12 + TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3); + TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3); + TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3); + TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3); + TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3); + TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3); + TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3); + TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3); + + for(i=0; i<4; i++){ + TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8); + k=k+5; + } + + for(i=0; i<4; i++){ + if (choix == 1){ + if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ + TAB_POSITION[i] = 1; + } + else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ + TAB_POSITION[i] = 0; + } + } + else if (choix == 2){ + if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ + TAB_POSITION[i] = 1; + } + else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ + TAB_POSITION[i] = 0; + } + } + } + + msgTx.data[0] = choix; + for(i=1; i<5; i++){ + msgTx.data[i] = TAB_POSITION[i]; + } + can1.write(msgTx); +} + \ No newline at end of file