CRAC Team / Utilisation_MX12

Programme d'utilisation servomotors MX12 V1

main.cpp@0:80df663dd15e, 2017-05-19 (annotated)

Committer:
R66Y
Date:
Fri May 19 14:32:14 2017 +0000
Revision:
0:80df663dd15e
programme pour utiliser les servomoteurs MX12.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
R66Y 0:80df663dd15e 1 /**************************************************************************************************/
R66Y 0:80df663dd15e 2 /* cette fonction de controle de l'MX12 est réalisée à partir de la même bibliothèque que les AX12*/
R66Y 0:80df663dd15e 3 /**************************************************************************************************/
R66Y 0:80df663dd15e 4
R66Y 0:80df663dd15e 5 #include "mbed.h"
R66Y 0:80df663dd15e 6 #include "AX12.h"
R66Y 0:80df663dd15e 7 #include "cmsis.h"
R66Y 0:80df663dd15e 8 #include "ident_crac.h"
R66Y 0:80df663dd15e 9
R66Y 0:80df663dd15e 10 #define AX12_INITIALISATION 0
R66Y 0:80df663dd15e 11 #define AX12_PREPARATION_PRISE 1
R66Y 0:80df663dd15e 12 #define AX12_STOCKAGE_HAUT 2
R66Y 0:80df663dd15e 13 #define AX12_STOCKAGE_BAS 3
R66Y 0:80df663dd15e 14 #define AX12_DEPOSER 4
R66Y 0:80df663dd15e 15 #define AX12_PREPARATION_DEPOT_BAS 5
R66Y 0:80df663dd15e 16 #define AX12_PREPARATION_DEPOT_HAUT 6
R66Y 0:80df663dd15e 17 #define AX12_POUSSER_MODULE 7
R66Y 0:80df663dd15e 18 #define AX12_DEFAUT 20
R66Y 0:80df663dd15e 19
R66Y 0:80df663dd15e 20 #define TIME 0.8
R66Y 0:80df663dd15e 21 #define TOLERANCE_AX12 50
R66Y 0:80df663dd15e 22 #define SIZE_FIFO 25
R66Y 0:80df663dd15e 23
R66Y 0:80df663dd15e 24 /* DECLARATION VARIABLES */
R66Y 0:80df663dd15e 25 CAN can1(p30,p29);
R66Y 0:80df663dd15e 26 CANMessage msgRxBuffer[SIZE_FIFO];
R66Y 0:80df663dd15e 27 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
R66Y 0:80df663dd15e 28 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
R66Y 0:80df663dd15e 29
R66Y 0:80df663dd15e 30 extern "C" void mbed_reset();//Pour pouvoir reset la carte
R66Y 0:80df663dd15e 31
R66Y 0:80df663dd15e 32 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
R66Y 0:80df663dd15e 33 static float TAB_ANGLE1[4], TAB_ANGLE2[4];
R66Y 0:80df663dd15e 34 static char TAB_POSITION[4];
R66Y 0:80df663dd15e 35 short vitesse=700;
R66Y 0:80df663dd15e 36 float angle=0.0;
R66Y 0:80df663dd15e 37 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;
R66Y 0:80df663dd15e 38 AX12 *un_myMX12;
R66Y 0:80df663dd15e 39
R66Y 0:80df663dd15e 40
R66Y 0:80df663dd15e 41 /* PROTOTYPES DE FONCTIONS ET POINTEURS */
R66Y 0:80df663dd15e 42
R66Y 0:80df663dd15e 43
R66Y 0:80df663dd15e 44 /****************************************************************************************/
R66Y 0:80df663dd15e 45 /* FUNCTION NAME: CAN2_wrFilter */
R66Y 0:80df663dd15e 46 /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */
R66Y 0:80df663dd15e 47 /****************************************************************************************/
R66Y 0:80df663dd15e 48 void CAN2_wrFilter (uint32_t id);
R66Y 0:80df663dd15e 49
R66Y 0:80df663dd15e 50 /****************************************************************************************/
R66Y 0:80df663dd15e 51 /* FUNCTION NAME: canProcessRx */
R66Y 0:80df663dd15e 52 /* DESCRIPTION : Fonction de traitement des messages CAN */
R66Y 0:80df663dd15e 53 /****************************************************************************************/
R66Y 0:80df663dd15e 54 void canProcessRx(void);
R66Y 0:80df663dd15e 55
R66Y 0:80df663dd15e 56 /****************************************************************************************/
R66Y 0:80df663dd15e 57 /* FUNCTION NAME: canRx_ISR */
R66Y 0:80df663dd15e 58 /* DESCRIPTION : Interruption en réception sur le CAN */
R66Y 0:80df663dd15e 59 /****************************************************************************************/
R66Y 0:80df663dd15e 60 void canRx_ISR (void);
R66Y 0:80df663dd15e 61
R66Y 0:80df663dd15e 62 /****************************************************************************************/
R66Y 0:80df663dd15e 63 /* FUNCTION NAME: SendRawId */
R66Y 0:80df663dd15e 64 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
R66Y 0:80df663dd15e 65 /****************************************************************************************/
R66Y 0:80df663dd15e 66 void SendRawId (unsigned short id);
R66Y 0:80df663dd15e 67
R66Y 0:80df663dd15e 68 /****************************************************************************************/
R66Y 0:80df663dd15e 69 /* FUNCTION NAME: Fin_action */
R66Y 0:80df663dd15e 70 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
R66Y 0:80df663dd15e 71 /****************************************************************************************/
R66Y 0:80df663dd15e 72 void Fin_action(void);
R66Y 0:80df663dd15e 73
R66Y 0:80df663dd15e 74 /****************************************************************************************/
R66Y 0:80df663dd15e 75 /* FUNCTION NAME: Automate_ax12 */
R66Y 0:80df663dd15e 76 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
R66Y 0:80df663dd15e 77 /****************************************************************************************/
R66Y 0:80df663dd15e 78 void AX12_automate(void);
R66Y 0:80df663dd15e 79
R66Y 0:80df663dd15e 80 /****************************************************************************************/
R66Y 0:80df663dd15e 81 /* FUNCTION NAME: Initialisation */
R66Y 0:80df663dd15e 82 /* DESCRIPTION : Fonction qui place les bras en position verticale */
R66Y 0:80df663dd15e 83 /****************************************************************************************/
R66Y 0:80df663dd15e 84 void Initialisation(void);
R66Y 0:80df663dd15e 85
R66Y 0:80df663dd15e 86 /****************************************************************************************/
R66Y 0:80df663dd15e 87 /* FUNCTION NAME: bouger_MX12 */
R66Y 0:80df663dd15e 88 /* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */
R66Y 0:80df663dd15e 89 /****************************************************************************************/
R66Y 0:80df663dd15e 90 void bouger_MX12(unsigned char choix);
R66Y 0:80df663dd15e 91
R66Y 0:80df663dd15e 92 Timer t;
R66Y 0:80df663dd15e 93 Ticker flipper;
R66Y 0:80df663dd15e 94
R66Y 0:80df663dd15e 95
R66Y 0:80df663dd15e 96 /* ANGLE */
R66Y 0:80df663dd15e 97
R66Y 0:80df663dd15e 98 /* 0 = tourner à droite
R66Y 0:80df663dd15e 99 600 = position initiale
R66Y 0:80df663dd15e 100 1200 = tourner à gauche
R66Y 0:80df663dd15e 101 */
R66Y 0:80df663dd15e 102
R66Y 0:80df663dd15e 103 /* NUMERO MX12 : 0x01 */
R66Y 0:80df663dd15e 104
R66Y 0:80df663dd15e 105
R66Y 0:80df663dd15e 106 /* MAIN */
R66Y 0:80df663dd15e 107
R66Y 0:80df663dd15e 108 int main()
R66Y 0:80df663dd15e 109 {
R66Y 0:80df663dd15e 110 can1.frequency(1000000); // fréquence de travail 1Mbit/s
R66Y 0:80df663dd15e 111 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
R66Y 0:80df663dd15e 112 CAN2_wrFilter(SERVO_AX12_ACTION); // 0x96 Trie des messages CAN sur les paramètres suivants.
R66Y 0:80df663dd15e 113 CAN2_wrFilter(SERVO_AX12_ACK); // 0x106
R66Y 0:80df663dd15e 114 CAN2_wrFilter(SERVO_AX12_END); // 0x116
R66Y 0:80df663dd15e 115 CAN2_wrFilter(CHECK_AX12); // 0x65
R66Y 0:80df663dd15e 116
R66Y 0:80df663dd15e 117 // Set l'ID du MX12 et la vitesse de communication. Paramètres : tx rx id baud
R66Y 0:80df663dd15e 118 un_myMX12 = new AX12(p9, p10, 1, 1000000); //Trappe de fermeture du lanceur.
R66Y 0:80df663dd15e 119
R66Y 0:80df663dd15e 120 while(true) {
R66Y 0:80df663dd15e 121 AX12_automate();// Trie les actions de l'MX12
R66Y 0:80df663dd15e 122 canProcessRx(); // Traitement des trames CAN en attente
R66Y 0:80df663dd15e 123 }
R66Y 0:80df663dd15e 124 }
R66Y 0:80df663dd15e 125
R66Y 0:80df663dd15e 126 /* FONCTIONS */
R66Y 0:80df663dd15e 127
R66Y 0:80df663dd15e 128 /****************************************************************************************/
R66Y 0:80df663dd15e 129 /* FUNCTION NAME: canProcessRx */
R66Y 0:80df663dd15e 130 /* DESCRIPTION : Fonction de traitement des messages CAN */
R66Y 0:80df663dd15e 131 /****************************************************************************************/
R66Y 0:80df663dd15e 132 void canProcessRx(void)
R66Y 0:80df663dd15e 133 {
R66Y 0:80df663dd15e 134 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
R66Y 0:80df663dd15e 135
R66Y 0:80df663dd15e 136 CANMessage msgTx=CANMessage(); // Set la structure d'un message CAN Tx : id, data, taille, type, format
R66Y 0:80df663dd15e 137 msgTx.format=CANStandard;
R66Y 0:80df663dd15e 138 msgTx.type=CANData;
R66Y 0:80df663dd15e 139 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
R66Y 0:80df663dd15e 140
R66Y 0:80df663dd15e 141 if(FIFO_occupation<0)
R66Y 0:80df663dd15e 142 FIFO_occupation=FIFO_occupation+SIZE_FIFO; //Taille du CANRx.
R66Y 0:80df663dd15e 143
R66Y 0:80df663dd15e 144 if(FIFO_max_occupation<FIFO_occupation)
R66Y 0:80df663dd15e 145 FIFO_max_occupation=FIFO_occupation;
R66Y 0:80df663dd15e 146
R66Y 0:80df663dd15e 147 if(FIFO_occupation!=0) {
R66Y 0:80df663dd15e 148
R66Y 0:80df663dd15e 149 switch(msgRxBuffer[FIFO_lecture].id) {
R66Y 0:80df663dd15e 150 case CHECK_AX12: // 0x065
R66Y 0:80df663dd15e 151 SendRawId(ALIVE_AX12);
R66Y 0:80df663dd15e 152 flag = 1;
R66Y 0:80df663dd15e 153 break;
R66Y 0:80df663dd15e 154
R66Y 0:80df663dd15e 155 case SERVO_AX12_ACTION : // 0x96
R66Y 0:80df663dd15e 156 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
R66Y 0:80df663dd15e 157
R66Y 0:80df663dd15e 158 //ACK de reception des actions a effectuer
R66Y 0:80df663dd15e 159 msgTx.id = SERVO_AX12_ACK;
R66Y 0:80df663dd15e 160 msgTx.len = 1;
R66Y 0:80df663dd15e 161 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
R66Y 0:80df663dd15e 162 can1.write(msgTx);
R66Y 0:80df663dd15e 163 break;
R66Y 0:80df663dd15e 164 }
R66Y 0:80df663dd15e 165
R66Y 0:80df663dd15e 166 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
R66Y 0:80df663dd15e 167 }
R66Y 0:80df663dd15e 168 }
R66Y 0:80df663dd15e 169
R66Y 0:80df663dd15e 170 /****************************************************************************************/
R66Y 0:80df663dd15e 171 /* FUNCTION NAME: canRx_ISR */
R66Y 0:80df663dd15e 172 /* DESCRIPTION : Interruption en réception sur le CAN */
R66Y 0:80df663dd15e 173 /****************************************************************************************/
R66Y 0:80df663dd15e 174 void canRx_ISR (void)
R66Y 0:80df663dd15e 175 {
R66Y 0:80df663dd15e 176 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
R66Y 0:80df663dd15e 177 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
R66Y 0:80df663dd15e 178 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
R66Y 0:80df663dd15e 179 }
R66Y 0:80df663dd15e 180 }
R66Y 0:80df663dd15e 181
R66Y 0:80df663dd15e 182 /****************************************************************************************/
R66Y 0:80df663dd15e 183 /* FUNCTION NAME: SendRawId */
R66Y 0:80df663dd15e 184 /* DESCRIPTION : Fonction qui permet d'envoyer une trame vide à un ID */
R66Y 0:80df663dd15e 185 /****************************************************************************************/
R66Y 0:80df663dd15e 186 void SendRawId (unsigned short id)
R66Y 0:80df663dd15e 187 {
R66Y 0:80df663dd15e 188 CANMessage msgTx=CANMessage();
R66Y 0:80df663dd15e 189 msgTx.id=id;
R66Y 0:80df663dd15e 190 msgTx.len=0;
R66Y 0:80df663dd15e 191 can1.write(msgTx);
R66Y 0:80df663dd15e 192 }
R66Y 0:80df663dd15e 193
R66Y 0:80df663dd15e 194
R66Y 0:80df663dd15e 195 /****************************************************************************************/
R66Y 0:80df663dd15e 196 /* FUNCTION NAME: Initialisation */
R66Y 0:80df663dd15e 197 /* DESCRIPTION : Place le MX12 en position initiale : lanceur fermé */
R66Y 0:80df663dd15e 198 /****************************************************************************************/
R66Y 0:80df663dd15e 199 void Initialisation(){
R66Y 0:80df663dd15e 200 un_myMX12-> Set_Goal_speed(vitesse);
R66Y 0:80df663dd15e 201 un_myMX12-> Set_Mode(0);
R66Y 0:80df663dd15e 202 }
R66Y 0:80df663dd15e 203
R66Y 0:80df663dd15e 204 /****************************************************************************************/
R66Y 0:80df663dd15e 205 /* FUNCTION NAME: bouger_MX12 */
R66Y 0:80df663dd15e 206 /* DESCRIPTION : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge */
R66Y 0:80df663dd15e 207 /****************************************************************************************/
R66Y 0:80df663dd15e 208 void bouger_MX12(unsigned char choix){
R66Y 0:80df663dd15e 209 AX12 *ptr_myAX12;
R66Y 0:80df663dd15e 210 ptr_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
R66Y 0:80df663dd15e 211 if( choix == 1) // Tourne à droite
R66Y 0:80df663dd15e 212 {
R66Y 0:80df663dd15e 213 ptr_myAX12->Set_Secure_Goal(0); // tourner droite
R66Y 0:80df663dd15e 214 }
R66Y 0:80df663dd15e 215 else if(choix == 2) // Tourne à gauche
R66Y 0:80df663dd15e 216 {
R66Y 0:80df663dd15e 217 ptr_myAX12->Set_Secure_Goal(1200); // tourner gauche
R66Y 0:80df663dd15e 218 }
R66Y 0:80df663dd15e 219 else if (choix == 0)
R66Y 0:80df663dd15e 220 {
R66Y 0:80df663dd15e 221 ptr_myAX12->Set_Secure_Goal(600); // position initiale
R66Y 0:80df663dd15e 222 }
R66Y 0:80df663dd15e 223 }
R66Y 0:80df663dd15e 224
R66Y 0:80df663dd15e 225 /****************************************************************************************/
R66Y 0:80df663dd15e 226 /* FUNCTION NAME: Fin_action */
R66Y 0:80df663dd15e 227 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
R66Y 0:80df663dd15e 228 /****************************************************************************************/
R66Y 0:80df663dd15e 229 void Fin_action(void){
R66Y 0:80df663dd15e 230 CANMessage msgTx=CANMessage();
R66Y 0:80df663dd15e 231 msgTx.format=CANStandard;
R66Y 0:80df663dd15e 232 msgTx.type=CANData;
R66Y 0:80df663dd15e 233
R66Y 0:80df663dd15e 234 msgTx.id = SERVO_AX12_END;
R66Y 0:80df663dd15e 235 msgTx.len = 1;
R66Y 0:80df663dd15e 236 msgTx.data[0] = AX12_PREPARATION_PRISE;
R66Y 0:80df663dd15e 237 can1.write(msgTx); // Envoie le message via le CAN.
R66Y 0:80df663dd15e 238 }
R66Y 0:80df663dd15e 239
R66Y 0:80df663dd15e 240 /****************************************************************************************/
R66Y 0:80df663dd15e 241 /* FUNCTION NAME: Automate_ax12 */
R66Y 0:80df663dd15e 242 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
R66Y 0:80df663dd15e 243 /****************************************************************************************/
R66Y 0:80df663dd15e 244 void AX12_automate(void){
R66Y 0:80df663dd15e 245 switch(etat_ax12){
R66Y 0:80df663dd15e 246 case AX12_INITIALISATION : // Etat = 0
R66Y 0:80df663dd15e 247 if (flag == 1){ // Décide si il faut initialiser le bras droit ou gauche
R66Y 0:80df663dd15e 248 Initialisation();
R66Y 0:80df663dd15e 249 flag = 2;
R66Y 0:80df663dd15e 250 }
R66Y 0:80df663dd15e 251 break;
R66Y 0:80df663dd15e 252
R66Y 0:80df663dd15e 253 case AX12_PREPARATION_PRISE : // Etat = 1
R66Y 0:80df663dd15e 254 bouger_MX12(msgRxBuffer[FIFO_lecture].data[1]);
R66Y 0:80df663dd15e 255 if (action == 0){
R66Y 0:80df663dd15e 256 Fin_action();
R66Y 0:80df663dd15e 257 action ++;
R66Y 0:80df663dd15e 258 }
R66Y 0:80df663dd15e 259 break;
R66Y 0:80df663dd15e 260 case AX12_DEFAUT :
R66Y 0:80df663dd15e 261 action = 0;
R66Y 0:80df663dd15e 262 break;
R66Y 0:80df663dd15e 263 }
R66Y 0:80df663dd15e 264 }
R66Y 0:80df663dd15e 265
R66Y 0:80df663dd15e 266 /****************************************************************************************/
R66Y 0:80df663dd15e 267 /* FUNCTION NAME: CAN2_wrFilter */
R66Y 0:80df663dd15e 268 /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */
R66Y 0:80df663dd15e 269 /****************************************************************************************/
R66Y 0:80df663dd15e 270 void CAN2_wrFilter (uint32_t id) {
R66Y 0:80df663dd15e 271 static int CAN_std_cnt = 0;
R66Y 0:80df663dd15e 272 uint32_t buf0, buf1;
R66Y 0:80df663dd15e 273 int cnt1, cnt2, bound1;
R66Y 0:80df663dd15e 274
R66Y 0:80df663dd15e 275 /* Acceptance Filter Memory full */
R66Y 0:80df663dd15e 276 if (((CAN_std_cnt + 1) >> 1) >= 512)
R66Y 0:80df663dd15e 277 return; /* error: objects full */
R66Y 0:80df663dd15e 278
R66Y 0:80df663dd15e 279 /* Setup Acceptance Filter Configuration
R66Y 0:80df663dd15e 280 Acceptance Filter Mode Register = Off */
R66Y 0:80df663dd15e 281 LPC_CANAF->AFMR = 0x00000001;
R66Y 0:80df663dd15e 282
R66Y 0:80df663dd15e 283 id |= 1 << 13; /* Add controller number(2) */
R66Y 0:80df663dd15e 284 id &= 0x0000F7FF; /* Mask out 16-bits of ID */
R66Y 0:80df663dd15e 285
R66Y 0:80df663dd15e 286 if (CAN_std_cnt == 0) { /* For entering first ID */
R66Y 0:80df663dd15e 287 LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
R66Y 0:80df663dd15e 288 } else if (CAN_std_cnt == 1) { /* For entering second ID */
R66Y 0:80df663dd15e 289 if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
R66Y 0:80df663dd15e 290 LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
R66Y 0:80df663dd15e 291 else
R66Y 0:80df663dd15e 292 LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
R66Y 0:80df663dd15e 293 } else {
R66Y 0:80df663dd15e 294 /* Find where to insert new ID */
R66Y 0:80df663dd15e 295 cnt1 = 0;
R66Y 0:80df663dd15e 296 cnt2 = CAN_std_cnt;
R66Y 0:80df663dd15e 297 bound1 = (CAN_std_cnt - 1) >> 1;
R66Y 0:80df663dd15e 298 while (cnt1 <= bound1) { /* Loop through standard existing IDs */
R66Y 0:80df663dd15e 299 if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id) {
R66Y 0:80df663dd15e 300 cnt2 = cnt1 * 2;
R66Y 0:80df663dd15e 301 break;
R66Y 0:80df663dd15e 302 }
R66Y 0:80df663dd15e 303 if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id) {
R66Y 0:80df663dd15e 304 cnt2 = cnt1 * 2 + 1;
R66Y 0:80df663dd15e 305 break;
R66Y 0:80df663dd15e 306 }
R66Y 0:80df663dd15e 307 cnt1++; /* cnt1 = U32 where to insert new ID */
R66Y 0:80df663dd15e 308 } /* cnt2 = U16 where to insert new ID */
R66Y 0:80df663dd15e 309
R66Y 0:80df663dd15e 310 if (cnt1 > bound1) { /* Adding ID as last entry */
R66Y 0:80df663dd15e 311 if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */
R66Y 0:80df663dd15e 312 LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16);
R66Y 0:80df663dd15e 313 else /* Odd number of IDs exists */
R66Y 0:80df663dd15e 314 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
R66Y 0:80df663dd15e 315 } else {
R66Y 0:80df663dd15e 316 buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */
R66Y 0:80df663dd15e 317 if ((cnt2 & 0x0001) == 0) /* Insert new mask to even address */
R66Y 0:80df663dd15e 318 buf1 = (id << 16) | (buf0 >> 16);
R66Y 0:80df663dd15e 319 else /* Insert new mask to odd address */
R66Y 0:80df663dd15e 320 buf1 = (buf0 & 0xFFFF0000) | id;
R66Y 0:80df663dd15e 321
R66Y 0:80df663dd15e 322 LPC_CANAF_RAM->mask[cnt1] = buf1; /* Insert mask */
R66Y 0:80df663dd15e 323
R66Y 0:80df663dd15e 324 bound1 = CAN_std_cnt >> 1;
R66Y 0:80df663dd15e 325 /* Move all remaining standard mask entries one place up */
R66Y 0:80df663dd15e 326 while (cnt1 < bound1) {
R66Y 0:80df663dd15e 327 cnt1++;
R66Y 0:80df663dd15e 328 buf1 = LPC_CANAF_RAM->mask[cnt1];
R66Y 0:80df663dd15e 329 LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
R66Y 0:80df663dd15e 330 buf0 = buf1;
R66Y 0:80df663dd15e 331 }
R66Y 0:80df663dd15e 332
R66Y 0:80df663dd15e 333 if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */
R66Y 0:80df663dd15e 334 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
R66Y 0:80df663dd15e 335 }
R66Y 0:80df663dd15e 336 }
R66Y 0:80df663dd15e 337 CAN_std_cnt++;
R66Y 0:80df663dd15e 338
R66Y 0:80df663dd15e 339 /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
R66Y 0:80df663dd15e 340 buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
R66Y 0:80df663dd15e 341 buf1 = buf0;
R66Y 0:80df663dd15e 342
R66Y 0:80df663dd15e 343 /* Setup acceptance filter pointers */
R66Y 0:80df663dd15e 344 LPC_CANAF->SFF_sa = 0;
R66Y 0:80df663dd15e 345 LPC_CANAF->SFF_GRP_sa = buf0;
R66Y 0:80df663dd15e 346 LPC_CANAF->EFF_sa = buf0;
R66Y 0:80df663dd15e 347 LPC_CANAF->EFF_GRP_sa = buf1;
R66Y 0:80df663dd15e 348 LPC_CANAF->ENDofTable = buf1;
R66Y 0:80df663dd15e 349
R66Y 0:80df663dd15e 350 LPC_CANAF->AFMR = 0x00000000; /* Use acceptance filter */
R66Y 0:80df663dd15e 351 } // CAN2_wrFilter
R66Y 0:80df663dd15e 352
R66Y 0:80df663dd15e 353 /****************************************************************************************/
R66Y 0:80df663dd15e 354 /* FUNCTION NAME: Check_positionAX12 */
R66Y 0:80df663dd15e 355 /* DESCRIPTION : Fonction qui permet de verifier la position de l'MX12 */
R66Y 0:80df663dd15e 356 /****************************************************************************************/
R66Y 0:80df663dd15e 357 void Check_positionAX12(char* TAB, unsigned char choix){
R66Y 0:80df663dd15e 358 static float TAB_POS_TH[4];
R66Y 0:80df663dd15e 359
R66Y 0:80df663dd15e 360 CANMessage msgTx=CANMessage();
R66Y 0:80df663dd15e 361 msgTx.id=SERVO_AX12_POSITION;
R66Y 0:80df663dd15e 362 msgTx.len=5;
R66Y 0:80df663dd15e 363
R66Y 0:80df663dd15e 364 //PERMET DE VERIFIER LA POSITION De l'MX12
R66Y 0:80df663dd15e 365 TAB_ANGLE1[0] = (unsigned short)(un_myMX12->Get_Position()/0.3);
R66Y 0:80df663dd15e 366
R66Y 0:80df663dd15e 367
R66Y 0:80df663dd15e 368 TAB_POS_TH[0] = (unsigned short) TAB[0] + ((unsigned short)TAB[1]<<8);
R66Y 0:80df663dd15e 369
R66Y 0:80df663dd15e 370 if (choix == 1){
R66Y 0:80df663dd15e 371 if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
R66Y 0:80df663dd15e 372 TAB_POSITION[0] = 1;
R66Y 0:80df663dd15e 373 }
R66Y 0:80df663dd15e 374 else if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
R66Y 0:80df663dd15e 375 TAB_POSITION[0] = 0;
R66Y 0:80df663dd15e 376 }
R66Y 0:80df663dd15e 377 }
R66Y 0:80df663dd15e 378 else if (choix == 2){
R66Y 0:80df663dd15e 379 if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
R66Y 0:80df663dd15e 380 TAB_POSITION[0] = 1;
R66Y 0:80df663dd15e 381 }
R66Y 0:80df663dd15e 382 else if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
R66Y 0:80df663dd15e 383 TAB_POSITION[0] = 0;
R66Y 0:80df663dd15e 384 }
R66Y 0:80df663dd15e 385 }
R66Y 0:80df663dd15e 386
R66Y 0:80df663dd15e 387 msgTx.data[0] = choix;
R66Y 0:80df663dd15e 388 msgTx.data[0] = TAB_POSITION[0];
R66Y 0:80df663dd15e 389 can1.write(msgTx);
R66Y 0:80df663dd15e 390 }
R66Y 0:80df663dd15e 391