CRAC Team
AX12 gros robot
Fork of
command_AX12_gros_robot
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 Fri Jul 15 2022 17:38:54 by
1.7.2