CRAC Team
/
command_AX12_gros_robot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
command_AX12
by 
CRAC Team
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "AX12.h" 00003 #include "cmsis.h" 00004 #include "ident_crac.h" 00005 00006 #define AX12_INITIALISATION 0 00007 #define AX12_PREPARATION_PRISE 1 00008 #define AX12_STOCKAGE_HAUT 2 00009 #define AX12_STOCKAGE_BAS 3 00010 #define AX12_DEPOSER 4 00011 #define AX12_PREPARATION_DEPOT_BAS 5 00012 #define AX12_PREPARATION_DEPOT_HAUT 6 00013 #define AX12_POUSSER_MODULE 7 00014 #define AX12_DEFAUT 20 00015 00016 #define TIME 0.8 00017 #define TOLERANCE_AX12 50 00018 #define SIZE_FIFO 25 00019 00020 /* DECLARATION VARIABLES */ 00021 CAN can1(p30,p29); 00022 CANMessage msgRxBuffer[SIZE_FIFO]; 00023 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN 00024 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN 00025 00026 extern "C" void mbed_reset();//Pour pouvoir reset la carte 00027 00028 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0; 00029 static float TAB_ANGLE1[4], TAB_ANGLE2[4]; 00030 static char TAB_POSITION[4]; 00031 short vitesse=700; 00032 float angle=0.0; 00033 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; 00034 AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; 00035 00036 00037 /* PROTOTYPES DE FONCTIONS ET POINTEURS */ 00038 00039 00040 /****************************************************************************************/ 00041 /* FUNCTION NAME: CAN2_wrFilter */ 00042 /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */ 00043 /****************************************************************************************/ 00044 void CAN2_wrFilter (uint32_t id); 00045 00046 /****************************************************************************************/ 00047 /* FUNCTION NAME: canProcessRx */ 00048 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00049 /****************************************************************************************/ 00050 void canProcessRx(void); 00051 00052 /****************************************************************************************/ 00053 /* FUNCTION NAME: canRx_ISR */ 00054 /* DESCRIPTION : Interruption en réception sur le CAN */ 00055 /****************************************************************************************/ 00056 void canRx_ISR (void); 00057 00058 /****************************************************************************************/ 00059 /* FUNCTION NAME: SendRawId */ 00060 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ 00061 /****************************************************************************************/ 00062 void SendRawId (unsigned short id); 00063 00064 /****************************************************************************************/ 00065 /* FUNCTION NAME: Fin_action */ 00066 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ 00067 /****************************************************************************************/ 00068 void Fin_action(void); 00069 00070 /****************************************************************************************/ 00071 /* FUNCTION NAME: Automate_ax12 */ 00072 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ 00073 /****************************************************************************************/ 00074 void AX12_automate(void); 00075 00076 /****************************************************************************************/ 00077 /* FUNCTION NAME: Initialisation_position */ 00078 /* DESCRIPTION : Fonction qui place les bras en position verticale */ 00079 /****************************************************************************************/ 00080 void Initialisation_position(unsigned char choix); 00081 00082 /****************************************************************************************/ 00083 /* FUNCTION NAME: Preparation_prise */ 00084 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ 00085 /****************************************************************************************/ 00086 void Preparation_prise(unsigned char choix); 00087 00088 /****************************************************************************************/ 00089 /* FUNCTION NAME: Stockage_haut */ 00090 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ 00091 /****************************************************************************************/ 00092 void Stockage_haut(unsigned char choix); 00093 00094 /****************************************************************************************/ 00095 /* FUNCTION NAME: Stockage_bas */ 00096 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ 00097 /****************************************************************************************/ 00098 void Stockage_bas(unsigned char choix); 00099 00100 /****************************************************************************************/ 00101 /* FUNCTION NAME: Deposer */ 00102 /* DESCRIPTION : Fonction qui permet de déposer un module */ 00103 /****************************************************************************************/ 00104 void Deposer(unsigned char choix); 00105 00106 /****************************************************************************************/ 00107 /* FUNCTION NAME: Preparation_depos_bas */ 00108 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ 00109 /****************************************************************************************/ 00110 void Preparation_depot_bas(unsigned char choix); 00111 00112 /****************************************************************************************/ 00113 /* FUNCTION NAME: Preparation_depos_haut */ 00114 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ 00115 /****************************************************************************************/ 00116 void Preparation_depot_haut(unsigned char choix); 00117 00118 /****************************************************************************************/ 00119 /* FUNCTION NAME: Pousser_module */ 00120 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ 00121 /****************************************************************************************/ 00122 void Pousser_module(unsigned char choix); 00123 00124 /****************************************************************************************/ 00125 /* FUNCTION NAME: Check_positionAX12 */ 00126 /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ 00127 /****************************************************************************************/ 00128 void Check_positionAX12(char* TAB, unsigned char choix); 00129 00130 Timer t; 00131 Ticker flipper; 00132 00133 ///////////TABLEAU PINCE DROITE /////////////////////////// --> choix_bras == 1 00134 static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 00135 0x04,0xF4, 0x01, 0xFF, 0x00, 00136 0x07,0x00, 0x01, 0xFF, 0x00, 00137 0x0F,0x00, 0x03, 0xFF, 0x00}; 00138 00139 static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise 00140 0x04,0xE9, 0x00, 0xFF, 0x03, 00141 0x07,0xF4, 0x01, 0xFF, 0x03, 00142 0x0F,0xF4, 0x01, 0xFF, 0x03}; 00143 00144 static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 00145 0x04,0xE9, 0x00, 0xFF, 0x03, 00146 0x07,0xD5, 0x00, 0xFF, 0x00, 00147 0x0F,0x4D, 0x03, 0xFF, 0x00}; 00148 00149 static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) 00150 0x04,0x00, 0x02, 0xFF, 0x03, 00151 0x07,0xD5, 0x00, 0xFF, 0x00, 00152 0x0F,0x4D, 0x03, 0xFF, 0x00}; 00153 00154 static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) 00155 0x04,0x60, 0x01, 0xFF, 0x00, 00156 0x07,0xD5, 0x00, 0xFF, 0x00, 00157 0x0F,0x4D, 0x03, 0xFF, 0x00}; 00158 00159 static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00160 0x04,0x50, 0x01, 0xFF, 0x03, 00161 0x07,0x0A, 0x01, 0xFF, 0x03, 00162 0x0F,0xDD, 0x02, 0xFF, 0x03}; 00163 00164 static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) 00165 0x04,0x50, 0x03, 0xFF, 0x03, 00166 0x07,0xD5, 0x00, 0xFF, 0x03, 00167 0x0F,0x4D, 0x03, 0xFF, 0x03}; 00168 00169 static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer (descendre bras) 00170 0x04,0x00, 0x03, 0xFF, 0x00, 00171 0x07,0xD5, 0x00, 0xFF, 0x00, 00172 0x0F,0x4D, 0x03, 0xFF, 0x00}; 00173 00174 static char TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer (pince ouverte) 00175 0x04,0x00, 0x03, 0xFF, 0x03, 00176 0x07,0x0A, 0x01, 0xFF, 0x03, 00177 0x0F,0xDD, 0x02, 0xFF, 0x03}; 00178 00179 static char TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer (preparation module haut) 00180 0x04,0x00, 0x02, 0xFF, 0x00, 00181 0x07,0xD5, 0x00, 0xFF, 0x00, 00182 0x0F,0x4D, 0x03, 0xFF, 0x00}; 00183 00184 static char TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) 00185 0x04,0x00, 0x03, 0xFF, 0x00, 00186 0x07,0x0A, 0x01, 0xFF, 0x03, 00187 0x0F,0xDD, 0x02, 0xFF, 0x03}; 00188 00189 00190 //////////////////TABLEAU PINCE GAUCHE ///////////////////////////// --> choix_bras == 2 00191 static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale 00192 0x03,0xF4, 0x01, 0xFF, 0x00, 00193 0x0D,0x00, 0x01, 0xFF, 0x00, 00194 0x0E,0x00, 0x03, 0xFF, 0x00}; 00195 00196 static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise 00197 0x03,0xE9, 0x00, 0xFF, 0x03, 00198 0x0D,0xF4, 0x01, 0xFF, 0x03, 00199 0x0E,0xF4, 0x01, 0xFF, 0x03}; 00200 00201 static char TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module) 00202 0x03,0xE9, 0x00, 0xFF, 0x03, 00203 0x0D,0xD5, 0x00, 0xFF, 0x00, 00204 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00205 00206 static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air) 00207 0x03,0x00, 0x02, 0xFF, 0x03, 00208 0x0D,0xD5, 0x00, 0xFF, 0x00, 00209 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00210 00211 static char TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige) 00212 0x03,0x60, 0x01, 0xFF, 0x00, 00213 0x0D,0xD5, 0x00, 0xFF, 0x00, 00214 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00215 00216 static char TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00217 0x03,0x50, 0x01, 0xFF, 0x03, 00218 0x0D,0x0A, 0x01, 0xFF, 0x03, 00219 0x0E,0xDD, 0x02, 0xFF, 0x03}; 00220 00221 static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air) 00222 0x03,0x50, 0x03, 0xFF, 0x03, 00223 0x0D,0xD5, 0x00, 0xFF, 0x03, 00224 0x0E,0x4D, 0x03, 0xFF, 0x03}; 00225 00226 static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer(descendre bras) 00227 0x03,0x00, 0x03, 0xFF, 0x00, 00228 0x0D,0xD5, 0x00, 0xFF, 0x00, 00229 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00230 00231 static char TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer(pince ouverte) 00232 0x03,0x00, 0x03, 0xFF, 0x03, 00233 0x0D,0xF4, 0x01, 0xFF, 0x03, 00234 0x0E,0xF4, 0x01, 0xFF, 0x03}; 00235 00236 static char TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer(preparation module haut) 00237 0x03,0x00, 0x02, 0xFF, 0x00, 00238 0x0D,0xD5, 0x00, 0xFF, 0x00, 00239 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00240 00241 static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas) 00242 0x03,0x00, 0x03, 0xFF, 0x00, 00243 0x0D,0xD5, 0x00, 0xFF, 0x00, 00244 0x0E,0x4D, 0x03, 0xFF, 0x00}; 00245 00246 00247 /* ANGLE */ 00248 00249 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 00250 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 00251 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 00252 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 00253 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 00254 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 00255 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 00256 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 00257 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 00258 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ 00259 00260 /* NUMERO AX12 */ 00261 00262 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 00263 1 = 0x01 | 10 = 0x0A 00264 2 = 0x02 | 11 = 0x0B 00265 3 = 0x03 | 12 = 0x0C 00266 4 = 0x04 | 13 = 0x0D 00267 5 = 0x05 | 14 = 0x0E 00268 6 = 0x06 | 15 = 0x0F 00269 7 = 0x07 | 16 = 0x10 00270 8 = 0x08 | 17 = 0x11 */ 00271 00272 00273 00274 /* MAIN */ 00275 00276 int main() 00277 { 00278 can1.frequency(1000000); // fréquence de travail 1Mbit/s 00279 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN 00280 CAN2_wrFilter(SERVO_AX12_ACTION); 00281 CAN2_wrFilter(SERVO_AX12_ACK); 00282 CAN2_wrFilter(SERVO_AX12_END); 00283 CAN2_wrFilter(CHECK_AX12); 00284 00285 trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche 00286 treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche 00287 quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche 00288 huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche 00289 00290 quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite 00291 sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite 00292 quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite 00293 dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite 00294 00295 multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); //pince droite 00296 multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); //pince gauche 00297 00298 while(true) { 00299 AX12_automate(); 00300 canProcessRx();//Traitement des trames CAN en attente 00301 } 00302 } 00303 00304 /* FONCTIONS */ 00305 00306 /****************************************************************************************/ 00307 /* FUNCTION NAME: canProcessRx */ 00308 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00309 /****************************************************************************************/ 00310 void canProcessRx(void) 00311 { 00312 static signed char FIFO_occupation=0,FIFO_max_occupation=0; 00313 00314 CANMessage msgTx=CANMessage(); 00315 msgTx.format=CANStandard; 00316 msgTx.type=CANData; 00317 FIFO_occupation=FIFO_ecriture-FIFO_lecture; 00318 00319 if(FIFO_occupation<0) 00320 FIFO_occupation=FIFO_occupation+SIZE_FIFO; 00321 00322 if(FIFO_max_occupation<FIFO_occupation) 00323 FIFO_max_occupation=FIFO_occupation; 00324 00325 if(FIFO_occupation!=0) { 00326 00327 switch(msgRxBuffer[FIFO_lecture].id) { 00328 case CHECK_AX12: 00329 SendRawId(ALIVE_AX12); 00330 flag = 1; 00331 break; 00332 00333 case SERVO_AX12_ACTION : 00334 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; 00335 choix_bras = msgRxBuffer[FIFO_lecture].data[1]; 00336 00337 //ACK de reception des actions a effectuer 00338 msgTx.id = SERVO_AX12_ACK; 00339 msgTx.len = 1; 00340 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; 00341 can1.write(msgTx); 00342 break; 00343 } 00344 00345 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; 00346 } 00347 } 00348 00349 /****************************************************************************************/ 00350 /* FUNCTION NAME: canRx_ISR */ 00351 /* DESCRIPTION : Interruption en réception sur le CAN */ 00352 /****************************************************************************************/ 00353 void canRx_ISR (void) 00354 { 00355 if (can1.read(msgRxBuffer[FIFO_ecriture])) { 00356 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); 00357 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; 00358 } 00359 } 00360 00361 /****************************************************************************************/ 00362 /* FUNCTION NAME: SendRawId */ 00363 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ 00364 /****************************************************************************************/ 00365 void SendRawId (unsigned short id) 00366 { 00367 CANMessage msgTx=CANMessage(); 00368 msgTx.id=id; 00369 msgTx.len=0; 00370 can1.write(msgTx); 00371 } 00372 void Init_AX12() // Initialisation des différents paramétres 00373 { 00374 deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) 00375 quatre_myAX12-> Set_Goal_speed(vitesse); 00376 sept_myAX12-> Set_Goal_speed(vitesse); 00377 quatorze_myAX12-> Set_Goal_speed(vitesse); 00378 quinze_myAX12-> Set_Goal_speed(vitesse); 00379 dixhuit_myAX12-> Set_Goal_speed(vitesse); 00380 huit_myAX12-> Set_Goal_speed(vitesse); 00381 trois_myAX12-> Set_Goal_speed(vitesse); 00382 quatorze_myAX12-> Set_Goal_speed(vitesse); 00383 treize_myAX12-> Set_Goal_speed(vitesse); 00384 00385 deux_myAX12-> Set_Mode(0); 00386 quatre_myAX12-> Set_Mode(0); 00387 sept_myAX12-> Set_Mode(0); 00388 quatorze_myAX12-> Set_Mode(0); 00389 quinze_myAX12-> Set_Mode(0); 00390 dixhuit_myAX12-> Set_Mode(0); 00391 huit_myAX12-> Set_Mode(0); 00392 trois_myAX12-> Set_Mode(0); 00393 quatorze_myAX12-> Set_Mode(0); 00394 treize_myAX12-> Set_Mode(0); 00395 } 00396 00397 00398 /****************************************************************************************/ 00399 /* FUNCTION NAME: Initialisation_position */ 00400 /* DESCRIPTION : Fonction qui place les bras en position verticale */ 00401 /****************************************************************************************/ 00402 void Initialisation_position(unsigned char choix){ 00403 if (choix == 1){ 00404 multiple_myAX12->multiple_goal_and_speed(4,TAB1); 00405 wait(TIME); 00406 } 00407 00408 else if (choix == 2){ 00409 multiple2_myAX12->multiple_goal_and_speed(4,TAB21); 00410 wait(TIME); 00411 } 00412 00413 } 00414 00415 /****************************************************************************************/ 00416 /* FUNCTION NAME: Preparation_prise */ 00417 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ 00418 /****************************************************************************************/ 00419 void Preparation_prise(unsigned char choix){ 00420 if (choix == 1){ 00421 multiple_myAX12->multiple_goal_and_speed(4,TAB2); 00422 wait(TIME); 00423 } 00424 00425 else if (choix == 2){ 00426 multiple2_myAX12->multiple_goal_and_speed(4,TAB22); 00427 wait(TIME); 00428 } 00429 } 00430 00431 /****************************************************************************************/ 00432 /* FUNCTION NAME: Stockage_haut */ 00433 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ 00434 /****************************************************************************************/ 00435 void Stockage_haut(unsigned char choix){ 00436 if (choix == 1){ 00437 multiple_myAX12->multiple_goal_and_speed(4,TAB3); 00438 wait(TIME); 00439 multiple_myAX12->multiple_goal_and_speed(4,TAB4); 00440 wait(TIME); 00441 multiple_myAX12->multiple_goal_and_speed(4,TAB5); 00442 wait(TIME); 00443 multiple_myAX12->multiple_goal_and_speed(4,TAB6); 00444 wait(TIME); 00445 } 00446 00447 else if (choix == 2){ 00448 multiple2_myAX12->multiple_goal_and_speed(4,TAB23); 00449 wait(TIME); 00450 multiple2_myAX12->multiple_goal_and_speed(4,TAB24); 00451 wait(TIME); 00452 multiple2_myAX12->multiple_goal_and_speed(4,TAB25); 00453 wait(TIME); 00454 multiple2_myAX12->multiple_goal_and_speed(4,TAB26); 00455 wait(TIME); 00456 } 00457 } 00458 00459 /****************************************************************************************/ 00460 /* FUNCTION NAME: Stockage_bas */ 00461 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ 00462 /****************************************************************************************/ 00463 void Stockage_bas(unsigned char choix){ 00464 if (choix == 1){ 00465 multiple_myAX12->multiple_goal_and_speed(4,TAB3); 00466 wait(TIME); 00467 multiple_myAX12->multiple_goal_and_speed(4,TAB7); 00468 wait(TIME); 00469 } 00470 00471 else if (choix == 2){ 00472 multiple2_myAX12->multiple_goal_and_speed(4,TAB23); 00473 wait(TIME); 00474 multiple2_myAX12->multiple_goal_and_speed(4,TAB27); 00475 wait(TIME); 00476 } 00477 } 00478 00479 /****************************************************************************************/ 00480 /* FUNCTION NAME: Deposer */ 00481 /* DESCRIPTION : Fonction qui permet de déposer le module */ 00482 /****************************************************************************************/ 00483 void Deposer(unsigned char choix){ 00484 if (choix == 1){ 00485 multiple_myAX12->multiple_goal_and_speed(4,TAB9); 00486 wait(TIME); 00487 } 00488 00489 else if (choix == 2){ 00490 multiple2_myAX12->multiple_goal_and_speed(4,TAB29); 00491 wait(TIME); 00492 } 00493 } 00494 00495 /****************************************************************************************/ 00496 /* FUNCTION NAME: Preparation_depot_bas */ 00497 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ 00498 /****************************************************************************************/ 00499 void Preparation_depot_bas(unsigned char choix){ 00500 if (choix == 1){ 00501 multiple_myAX12->multiple_goal_and_speed(4,TAB8); 00502 wait(TIME); 00503 } 00504 00505 else if (choix == 2){ 00506 multiple2_myAX12->multiple_goal_and_speed(4,TAB28); 00507 wait(TIME); 00508 } 00509 } 00510 00511 /****************************************************************************************/ 00512 /* FUNCTION NAME: Preparation_depot_haut */ 00513 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ 00514 /****************************************************************************************/ 00515 void Preparation_depot_haut(unsigned char choix){ 00516 if (choix == 1){ 00517 multiple_myAX12->multiple_goal_and_speed(4,TAB6); 00518 wait(TIME); 00519 multiple_myAX12->multiple_goal_and_speed(4,TAB5); 00520 wait(TIME); 00521 multiple_myAX12->multiple_goal_and_speed(4,TAB10); 00522 wait(TIME); 00523 multiple_myAX12->multiple_goal_and_speed(4,TAB8); 00524 wait(TIME); 00525 } 00526 00527 else if (choix == 2){ 00528 multiple2_myAX12->multiple_goal_and_speed(4,TAB26); 00529 wait(TIME); 00530 multiple2_myAX12->multiple_goal_and_speed(4,TAB25); 00531 wait(TIME); 00532 multiple2_myAX12->multiple_goal_and_speed(4,TAB30); 00533 wait(TIME); 00534 multiple2_myAX12->multiple_goal_and_speed(4,TAB28); 00535 wait(TIME); 00536 } 00537 } 00538 00539 /****************************************************************************************/ 00540 /* FUNCTION NAME: Pousser_module */ 00541 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ 00542 /****************************************************************************************/ 00543 void Pousser_module(unsigned char choix){ 00544 if (choix == 1){ 00545 multiple_myAX12->multiple_goal_and_speed(4,TAB11); 00546 wait(TIME); 00547 } 00548 00549 else if (choix == 2){ 00550 multiple2_myAX12->multiple_goal_and_speed(4,TAB31); 00551 wait(TIME); 00552 } 00553 } 00554 00555 /****************************************************************************************/ 00556 /* FUNCTION NAME: Fin_action */ 00557 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ 00558 /****************************************************************************************/ 00559 void Fin_action(void){ 00560 CANMessage msgTx=CANMessage(); 00561 msgTx.format=CANStandard; 00562 msgTx.type=CANData; 00563 00564 msgTx.id = SERVO_AX12_END; 00565 msgTx.len = 1; 00566 msgTx.data[0] = AX12_PREPARATION_PRISE; 00567 can1.write(msgTx); 00568 } 00569 00570 /****************************************************************************************/ 00571 /* FUNCTION NAME: Automate_ax12 */ 00572 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ 00573 /****************************************************************************************/ 00574 void AX12_automate(void){ 00575 switch(etat_ax12){ 00576 case AX12_INITIALISATION : 00577 if (flag == 1){ 00578 Initialisation_position(1); 00579 //Check_positionAX12(&TAB1[25], 1); 00580 Initialisation_position(2); 00581 //Check_positionAX12(&TAB21[25], 2); 00582 flag = 2; 00583 } 00584 else if (choix_bras > 1){ 00585 Initialisation_position(choix_bras); 00586 } 00587 break; 00588 00589 case AX12_PREPARATION_PRISE : 00590 Preparation_prise(choix_bras); 00591 if (action == 0){ 00592 Fin_action(); 00593 action ++; 00594 } 00595 break; 00596 00597 case AX12_STOCKAGE_HAUT : 00598 Stockage_haut(choix_bras); 00599 etat_ax12 = AX12_DEFAUT; 00600 Fin_action(); 00601 break; 00602 00603 case AX12_STOCKAGE_BAS : 00604 Stockage_bas(choix_bras); 00605 etat_ax12 = AX12_DEFAUT; 00606 Fin_action(); 00607 break; 00608 00609 case AX12_DEPOSER : 00610 Deposer(choix_bras); 00611 etat_ax12 = AX12_DEFAUT; 00612 Fin_action(); 00613 break; 00614 00615 case AX12_PREPARATION_DEPOT_BAS : 00616 Preparation_depot_bas(choix_bras); 00617 etat_ax12 = AX12_DEFAUT; 00618 Fin_action(); 00619 break; 00620 00621 case AX12_PREPARATION_DEPOT_HAUT : 00622 Preparation_depot_haut(choix_bras); 00623 etat_ax12 = AX12_DEFAUT; 00624 Fin_action(); 00625 break; 00626 00627 case AX12_POUSSER_MODULE : 00628 Pousser_module(choix_bras); 00629 etat_ax12 = AX12_DEFAUT; 00630 Fin_action(); 00631 break; 00632 00633 case AX12_DEFAUT : 00634 action = 0; 00635 break; 00636 } 00637 } 00638 00639 /****************************************************************************************/ 00640 /* FUNCTION NAME: CAN2_wrFilter */ 00641 /* DESCRIPTION : Fonction qui permet de ne garder que les ID qui nous interessent */ 00642 /****************************************************************************************/ 00643 void CAN2_wrFilter (uint32_t id) { 00644 static int CAN_std_cnt = 0; 00645 uint32_t buf0, buf1; 00646 int cnt1, cnt2, bound1; 00647 00648 /* Acceptance Filter Memory full */ 00649 if (((CAN_std_cnt + 1) >> 1) >= 512) 00650 return; /* error: objects full */ 00651 00652 /* Setup Acceptance Filter Configuration 00653 Acceptance Filter Mode Register = Off */ 00654 LPC_CANAF->AFMR = 0x00000001; 00655 00656 id |= 1 << 13; /* Add controller number(2) */ 00657 id &= 0x0000F7FF; /* Mask out 16-bits of ID */ 00658 00659 if (CAN_std_cnt == 0) { /* For entering first ID */ 00660 LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16); 00661 } else if (CAN_std_cnt == 1) { /* For entering second ID */ 00662 if ((LPC_CANAF_RAM->mask[0] >> 16) > id) 00663 LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16); 00664 else 00665 LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id; 00666 } else { 00667 /* Find where to insert new ID */ 00668 cnt1 = 0; 00669 cnt2 = CAN_std_cnt; 00670 bound1 = (CAN_std_cnt - 1) >> 1; 00671 while (cnt1 <= bound1) { /* Loop through standard existing IDs */ 00672 if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id) { 00673 cnt2 = cnt1 * 2; 00674 break; 00675 } 00676 if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id) { 00677 cnt2 = cnt1 * 2 + 1; 00678 break; 00679 } 00680 cnt1++; /* cnt1 = U32 where to insert new ID */ 00681 } /* cnt2 = U16 where to insert new ID */ 00682 00683 if (cnt1 > bound1) { /* Adding ID as last entry */ 00684 if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ 00685 LPC_CANAF_RAM->mask[cnt1] = 0x0000FFFF | (id << 16); 00686 else /* Odd number of IDs exists */ 00687 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id; 00688 } else { 00689 buf0 = LPC_CANAF_RAM->mask[cnt1]; /* Remember current entry */ 00690 if ((cnt2 & 0x0001) == 0) /* Insert new mask to even address */ 00691 buf1 = (id << 16) | (buf0 >> 16); 00692 else /* Insert new mask to odd address */ 00693 buf1 = (buf0 & 0xFFFF0000) | id; 00694 00695 LPC_CANAF_RAM->mask[cnt1] = buf1; /* Insert mask */ 00696 00697 bound1 = CAN_std_cnt >> 1; 00698 /* Move all remaining standard mask entries one place up */ 00699 while (cnt1 < bound1) { 00700 cnt1++; 00701 buf1 = LPC_CANAF_RAM->mask[cnt1]; 00702 LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16); 00703 buf0 = buf1; 00704 } 00705 00706 if ((CAN_std_cnt & 0x0001) == 0) /* Even number of IDs exists */ 00707 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF); 00708 } 00709 } 00710 CAN_std_cnt++; 00711 00712 /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */ 00713 buf0 = ((CAN_std_cnt + 1) >> 1) << 2; 00714 buf1 = buf0; 00715 00716 /* Setup acceptance filter pointers */ 00717 LPC_CANAF->SFF_sa = 0; 00718 LPC_CANAF->SFF_GRP_sa = buf0; 00719 LPC_CANAF->EFF_sa = buf0; 00720 LPC_CANAF->EFF_GRP_sa = buf1; 00721 LPC_CANAF->ENDofTable = buf1; 00722 00723 LPC_CANAF->AFMR = 0x00000000; /* Use acceptance filter */ 00724 } // CAN2_wrFilter 00725 00726 /****************************************************************************************/ 00727 /* FUNCTION NAME: Check_positionAX12 */ 00728 /* DESCRIPTION : Fonction qui permet de verifier la position des AX12 */ 00729 /****************************************************************************************/ 00730 void Check_positionAX12(char* TAB, unsigned char choix){ 00731 int k=1, i=0; 00732 static float TAB_POS_TH[4]; 00733 00734 CANMessage msgTx=CANMessage(); 00735 msgTx.id=SERVO_AX12_POSITION; 00736 msgTx.len=5; 00737 00738 //PERMET DE VERIFIER LA POSITION D'UN AX12 00739 TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3); 00740 TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3); 00741 TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3); 00742 TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3); 00743 TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3); 00744 TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3); 00745 TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3); 00746 TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3); 00747 00748 for(i=0; i<4; i++){ 00749 TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8); 00750 k=k+5; 00751 } 00752 00753 for(i=0; i<4; i++){ 00754 if (choix == 1){ 00755 if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ 00756 TAB_POSITION[i] = 1; 00757 } 00758 else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ 00759 TAB_POSITION[i] = 0; 00760 } 00761 } 00762 else if (choix == 2){ 00763 if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ 00764 TAB_POSITION[i] = 1; 00765 } 00766 else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){ 00767 TAB_POSITION[i] = 0; 00768 } 00769 } 00770 } 00771 00772 msgTx.data[0] = choix; 00773 for(i=1; i<5; i++){ 00774 msgTx.data[i] = TAB_POSITION[i]; 00775 } 00776 can1.write(msgTx); 00777 } 00778
Generated on Thu Jul 14 2022 14:13:26 by
1.7.2