AX12 gros robot
Fork of command_AX12_gros_robot by
main.cpp
- Committer:
- SquirrelGod
- Date:
- 2017-05-11
- Revision:
- 4:92d6a49cd2c5
- Parent:
- 3:6cbd7d2c78f4
File content as of revision 4:92d6a49cd2c5:
#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 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; /* 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_position */ /* DESCRIPTION : Fonction qui place les bras en position verticale */ /****************************************************************************************/ void Initialisation_position(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Preparation_prise */ /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ /****************************************************************************************/ void Preparation_prise(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Stockage_haut */ /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ /****************************************************************************************/ void Stockage_haut(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Stockage_bas */ /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ /****************************************************************************************/ void Stockage_bas(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Deposer */ /* DESCRIPTION : Fonction qui permet de déposer un module */ /****************************************************************************************/ void Deposer(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Preparation_depos_bas */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ /****************************************************************************************/ 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_depot_haut(unsigned char choix); /****************************************************************************************/ /* FUNCTION NAME: Pousser_module */ /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ /****************************************************************************************/ 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 /////////////////////////// --> choix_bras == 1 static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 0x04,0xF4, 0x01, 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, 0x07,0xF4, 0x01, 0xFF, 0x03, 0x0F,0xF4, 0x01, 0xFF, 0x03}; static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 0x04,0xE9, 0x00, 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,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,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, 0x07,0x0A, 0x01, 0xFF, 0x03, 0x0F,0xDD, 0x02, 0xFF, 0x03}; static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) 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 (descendre bras) 0x04,0x00, 0x03, 0xFF, 0x00, 0x07,0xD5, 0x00, 0xFF, 0x00, 0x0F,0x4D, 0x03, 0xFF, 0x00}; static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer (pince ouverte) 0x04,0x00, 0x03, 0xFF, 0x03, 0x07,0x0A, 0x01, 0xFF, 0x03, 0x0F,0xDD, 0x02, 0xFF, 0x03}; static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer (preparation module haut) 0x04,0x00, 0x02, 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,0x0A, 0x01, 0xFF, 0x03, 0x0F,0xDD, 0x02, 0xFF, 0x03}; //////////////////TABLEAU PINCE GAUCHE ///////////////////////////// --> choix_bras == 2 static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 0x03,0xF4, 0x01, 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, 0x0D,0xF4, 0x01, 0xFF, 0x03, 0x0E,0xF4, 0x01, 0xFF, 0x03}; static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 0x03,0xE9, 0x00, 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,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,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, 0x0D,0x0A, 0x01, 0xFF, 0x03, 0x0E,0xDD, 0x02, 0xFF, 0x03}; static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) 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(descendre bras) 0x03,0x00, 0x03, 0xFF, 0x00, 0x0D,0xD5, 0x00, 0xFF, 0x00, 0x0E,0x4D, 0x03, 0xFF, 0x00}; 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(preparation module haut) 0x03,0x00, 0x02, 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,0xD5, 0x00, 0xFF, 0x00, 0x0E,0x4D, 0x03, 0xFF, 0x00}; /* ANGLE */ /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ /* NUMERO AX12 */ /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 1 = 0x01 | 10 = 0x0A 2 = 0x02 | 11 = 0x0B 3 = 0x03 | 12 = 0x0C 4 = 0x04 | 13 = 0x0D 5 = 0x05 | 14 = 0x0E 6 = 0x06 | 15 = 0x0F 7 = 0x07 | 16 = 0x10 8 = 0x08 | 17 = 0x11 */ /* 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); CAN2_wrFilter(SERVO_AX12_ACK); CAN2_wrFilter(SERVO_AX12_END); CAN2_wrFilter(CHECK_AX12); 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 huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite 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); //pince droite multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); //pince gauche while(true) { AX12_automate(); 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(); 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 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]; //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'envoi une trame vide à un ID */ /****************************************************************************************/ void SendRawId (unsigned short id) { CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=0; can1.write(msgTx); } void Init_AX12() // Initialisation des différents paramétres { deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) quatre_myAX12-> Set_Goal_speed(vitesse); sept_myAX12-> Set_Goal_speed(vitesse); quatorze_myAX12-> Set_Goal_speed(vitesse); quinze_myAX12-> Set_Goal_speed(vitesse); dixhuit_myAX12-> Set_Goal_speed(vitesse); huit_myAX12-> Set_Goal_speed(vitesse); trois_myAX12-> Set_Goal_speed(vitesse); quatorze_myAX12-> Set_Goal_speed(vitesse); treize_myAX12-> Set_Goal_speed(vitesse); deux_myAX12-> Set_Mode(0); quatre_myAX12-> Set_Mode(0); sept_myAX12-> Set_Mode(0); quatorze_myAX12-> Set_Mode(0); quinze_myAX12-> Set_Mode(0); dixhuit_myAX12-> Set_Mode(0); huit_myAX12-> Set_Mode(0); trois_myAX12-> Set_Mode(0); quatorze_myAX12-> Set_Mode(0); treize_myAX12-> Set_Mode(0); } /****************************************************************************************/ /* FUNCTION NAME: Initialisation_position */ /* DESCRIPTION : Fonction qui place les bras en position verticale */ /****************************************************************************************/ 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); } } /****************************************************************************************/ /* FUNCTION NAME: Preparation_prise */ /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ /****************************************************************************************/ void Preparation_prise(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB2); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB22); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Stockage_haut */ /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ /****************************************************************************************/ void Stockage_haut(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB3); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB4); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB5); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB6); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB23); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB24); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB25); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB26); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Stockage_bas */ /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ /****************************************************************************************/ void Stockage_bas(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB3); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB7); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB23); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB27); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Deposer */ /* DESCRIPTION : Fonction qui permet de déposer le module */ /****************************************************************************************/ void Deposer(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB9); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB29); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Preparation_depot_bas */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ /****************************************************************************************/ void Preparation_depot_bas(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB8); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB28); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Preparation_depot_haut */ /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ /****************************************************************************************/ void Preparation_depot_haut(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB6); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB5); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB10); wait(TIME); multiple_myAX12->multiple_goal_and_speed(4,TAB8); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB26); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB25); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB30); wait(TIME); multiple2_myAX12->multiple_goal_and_speed(4,TAB28); wait(TIME); } } /****************************************************************************************/ /* FUNCTION NAME: Pousser_module */ /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ /****************************************************************************************/ void Pousser_module(unsigned char choix){ if (choix == 1){ multiple_myAX12->multiple_goal_and_speed(4,TAB11); wait(TIME); } else if (choix == 2){ multiple2_myAX12->multiple_goal_and_speed(4,TAB31); wait(TIME); } } /****************************************************************************************/ /* 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); }