CRAC Team
AX12 gros robot
Fork of
command_AX12_gros_robot
by 
CRAC Team
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