CRAC Team
/
command_AX12_petit_robot_v2
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_petit_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 SIZE_FIFO 25 00018 00019 /* DECLARATION VARIABLES */ 00020 CAN can1(p30,p29); 00021 CANMessage msgRxBuffer[SIZE_FIFO]; 00022 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN 00023 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN 00024 00025 extern "C" void mbed_reset();//Pour pouvoir reset la carte 00026 00027 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 0; 00028 short vitesse=700; 00029 float angle=0.0; 00030 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; 00031 AX12 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale 00032 AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche 00033 00034 00035 /* PROTOTYPES DE FONCTIONS ET POINTEURS */ 00036 00037 00038 00039 /****************************************************************************************/ 00040 /* FUNCTION NAME: canProcessRx */ 00041 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00042 /****************************************************************************************/ 00043 void canProcessRx(void); 00044 00045 /****************************************************************************************/ 00046 /* FUNCTION NAME: canRx_ISR */ 00047 /* DESCRIPTION : Interruption en réception sur le CAN */ 00048 /****************************************************************************************/ 00049 void canRx_ISR (void); 00050 00051 /****************************************************************************************/ 00052 /* FUNCTION NAME: SendRawId */ 00053 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ 00054 /****************************************************************************************/ 00055 void SendRawId (unsigned short id); 00056 00057 /****************************************************************************************/ 00058 /* FUNCTION NAME: Fin_action */ 00059 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ 00060 /****************************************************************************************/ 00061 void Fin_action(void); 00062 00063 /****************************************************************************************/ 00064 /* FUNCTION NAME: Automate_ax12 */ 00065 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ 00066 /****************************************************************************************/ 00067 void AX12_automate(void); 00068 00069 /****************************************************************************************/ 00070 /* FUNCTION NAME: Initialisation_position */ 00071 /* DESCRIPTION : Fonction qui place les bras en position verticale */ 00072 /****************************************************************************************/ 00073 void Initialisation_position(void); 00074 00075 /****************************************************************************************/ 00076 /* FUNCTION NAME: Preparation_prise */ 00077 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ 00078 /****************************************************************************************/ 00079 void Preparation_prise(void); 00080 00081 /****************************************************************************************/ 00082 /* FUNCTION NAME: Stockage_haut */ 00083 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ 00084 /****************************************************************************************/ 00085 void Stockage_haut(void); 00086 00087 /****************************************************************************************/ 00088 /* FUNCTION NAME: Stockage_bas */ 00089 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ 00090 /****************************************************************************************/ 00091 void Stockage_bas(void); 00092 00093 /****************************************************************************************/ 00094 /* FUNCTION NAME: Deposer */ 00095 /* DESCRIPTION : Fonction qui permet de déposer un module */ 00096 /****************************************************************************************/ 00097 void Deposer(void); 00098 00099 /****************************************************************************************/ 00100 /* FUNCTION NAME: Preparation_depos_bas */ 00101 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ 00102 /****************************************************************************************/ 00103 void Preparation_depot_bas(void); 00104 00105 /****************************************************************************************/ 00106 /* FUNCTION NAME: Preparation_depos_haut */ 00107 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ 00108 /****************************************************************************************/ 00109 void Preparation_depot_haut(void); 00110 00111 /****************************************************************************************/ 00112 /* FUNCTION NAME: Pousser_module */ 00113 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ 00114 /****************************************************************************************/ 00115 void Pousser_module(void); 00116 00117 /****************************************************************************************/ 00118 /* FUNCTION NAME: Initialisation_gauche */ 00119 /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ 00120 /****************************************************************************************/ 00121 void Initialisation_gauche(void); 00122 00123 /****************************************************************************************/ 00124 /* FUNCTION NAME: Preparation_prise_gauche */ 00125 /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ 00126 /****************************************************************************************/ 00127 void Preparation_prise_gauche(void); 00128 00129 /****************************************************************************************/ 00130 /* FUNCTION NAME: Prendre_module_gauche */ 00131 /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ 00132 /****************************************************************************************/ 00133 void Prendre_module_gauche(void); 00134 00135 /****************************************************************************************/ 00136 /* FUNCTION NAME: Tourner_module_gauche */ 00137 /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ 00138 /****************************************************************************************/ 00139 void Tourner_module_gauche(void); 00140 00141 /****************************************************************************************/ 00142 /* FUNCTION NAME: Preparatio_module_gauche */ 00143 /* DESCRIPTION : Fonction qui prepare le tournante */ 00144 /****************************************************************************************/ 00145 void Preparation_module_gauche(void); 00146 00147 Timer t; 00148 Ticker flipper; 00149 00150 ////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// 00151 static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale 00152 0x10,0x00, 0x02, 0xFF, 0x00, 00153 0x07,0x00, 0x01, 0xFF, 0x00, 00154 0x08,0x00, 0x03, 0xFF, 0x00}; 00155 00156 static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise 00157 0x10,0x50, 0x01, 0xFF, 0x03, 00158 0x07,0xF4, 0x01, 0xFF, 0x03, 00159 0x08,0xF4, 0x01, 0xFF, 0x03}; 00160 00161 static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) 00162 0x10,0x50, 0x01, 0xFF, 0x03, 00163 0x07,0xC5, 0x00, 0xFF, 0x03, 00164 0x08,0x4D, 0x03, 0xFF, 0x03}; 00165 00166 static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) 00167 0x10,0x50, 0x01, 0xFF, 0x03, 00168 0x07,0xC5, 0x00, 0xFF, 0x03, 00169 0x08,0x4D, 0x03, 0xFF, 0x03}; 00170 00171 static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) 00172 0x10,0xF4, 0x01, 0xFF, 0x00, 00173 0x07,0xC5, 0x00, 0xFF, 0x03, 00174 0x08,0x4D, 0x03, 0xFF, 0x03}; 00175 00176 static char TAB6[25]= {0x01,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00177 0x10,0xF4, 0x01, 0xFF, 0x03, 00178 0x07,0x00, 0x01, 0xFF, 0x03, 00179 0x08,0x00, 0x03, 0xFF, 0x03}; 00180 00181 static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) 00182 0x10,0xB0, 0x00, 0xFF, 0x03, 00183 0x07,0xC5, 0x00, 0xFF, 0x00, 00184 0x08,0x4D, 0x03, 0xFF, 0x00}; 00185 00186 static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas 00187 0x10,0xF4, 0x01, 0xFF, 0x03, 00188 0x07,0xC5, 0x00, 0xFF, 0x00, 00189 0x08,0x4D, 0x03, 0xFF, 0x00}; 00190 00191 static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer 00192 0x10,0xF4, 0x01, 0xFF, 0x03, 00193 0x07,0xD0, 0x00, 0xFF, 0x00, 00194 0x08,0x35, 0x03, 0xFF, 0x00}; 00195 00196 static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) 00197 0x10,0x00, 0x01, 0xFF, 0x00, 00198 0x07,0xC5, 0x00, 0xFF, 0x03, 00199 0x08,0x4D, 0x03, 0xFF, 0x03}; 00200 00201 static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module 00202 0x10,0xF4, 0x01, 0xFF, 0x03, 00203 0x07,0xC5, 0x00, 0xFF, 0x00, 00204 0x08,0x4D, 0x03, 0xFF, 0x00}; 00205 00206 static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale 00207 0x10,0x00, 0x02, 0xFF, 0x03, 00208 0x07,0xF4, 0x01, 0xFF, 0x03, 00209 0x08,0xF4, 0x01, 0xFF, 0x03}; 00210 00211 static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer 00212 0x10,0xA0, 0x02, 0xFF, 0x03, 00213 0x07,0xD0, 0x00, 0xFF, 0x00, 00214 0x08,0x35, 0x03, 0xFF, 0x00}; 00215 00216 ////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// 00217 static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale 00218 0x02,0xF4, 0x01, 0xFF, 0x03}; 00219 00220 static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner 00221 0x02,0x40, 0x03, 0xFF, 0x03}; 00222 00223 static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module 00224 0x02,0xE5, 0x02, 0xFF, 0x03}; 00225 00226 00227 00228 00229 /* ANGLE */ 00230 00231 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 00232 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 00233 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 00234 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 00235 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 00236 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 00237 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 00238 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 00239 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 00240 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ 00241 00242 /* NUMERO AX12 */ 00243 00244 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 00245 1 = 0x01 | 10 = 0x0A 00246 2 = 0x02 | 11 = 0x0B 00247 3 = 0x03 | 12 = 0x0C 00248 4 = 0x04 | 13 = 0x0D 00249 5 = 0x05 | 14 = 0x0E 00250 6 = 0x06 | 15 = 0x0F 00251 7 = 0x07 | 16 = 0x10 00252 8 = 0x08 | 17 = 0x11 */ 00253 00254 00255 00256 /* MAIN */ 00257 00258 int main() 00259 { 00260 can1.frequency(1000000); // fréquence de travail 1Mbit/s 00261 can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN 00262 00263 //Pince centrale 00264 un_myAX12 = new AX12(p9, p10, 1, 1000000); 00265 sept_myAX12 = new AX12(p9, p10, 7, 1000000); 00266 huit_myAX12 = new AX12(p9, p10, 8, 1000000); 00267 seize_myAX12 = new AX12(p9, p10, 16, 1000000); 00268 multiple_myAX12 = new AX12(p9,p10,0xFE,1000000); 00269 00270 //Bras de gauche 00271 un_myAX12_2 = new AX12(p13, p14, 1, 1000000); 00272 deux_myAX12_2 = new AX12(p13, p14, 2, 1000000); 00273 trois_myAX12_2 = new AX12(p13, p14, 3, 1000000); 00274 multiple_myAX12_2 = new AX12(p13,p14,0xFE,1000000); 00275 00276 //Demonstration pince centrale 00277 /* 00278 Initialisation_position(); 00279 wait(1); 00280 Preparation_prise(); 00281 wait(1); 00282 Stockage_haut(); 00283 wait(1); 00284 Preparation_prise(); 00285 wait(1); 00286 Stockage_bas(); 00287 wait(1); 00288 Pousser_module(); 00289 wait(1); 00290 Preparation_depot_bas(); 00291 wait(1); 00292 Deposer(); 00293 wait(1); 00294 Preparation_depot_haut(); 00295 wait(1); 00296 Deposer(); 00297 wait(1); 00298 */ 00299 00300 //Demonstration bras gauche 00301 Initialisation_gauche(); 00302 wait(1); 00303 Preparation_prise_gauche(); 00304 wait(1); 00305 Prendre_module_gauche(); 00306 wait(1); 00307 Preparation_module_gauche(); 00308 wait(1); 00309 Tourner_module_gauche(); 00310 wait(1); 00311 Preparation_module_gauche(); 00312 wait(1); 00313 Tourner_module_gauche(); 00314 wait(1); 00315 Preparation_module_gauche(); 00316 wait(1); 00317 Tourner_module_gauche(); 00318 00319 while(true) { 00320 //AX12_automate(); 00321 } 00322 } 00323 00324 /* FONCTIONS */ 00325 00326 /****************************************************************************************/ 00327 /* FUNCTION NAME: canProcessRx */ 00328 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00329 /****************************************************************************************/ 00330 void canProcessRx(void) 00331 { 00332 static signed char FIFO_occupation=0,FIFO_max_occupation=0; 00333 00334 CANMessage msgTx=CANMessage(); 00335 msgTx.format=CANStandard; 00336 msgTx.type=CANData; 00337 FIFO_occupation=FIFO_ecriture-FIFO_lecture; 00338 00339 if(FIFO_occupation<0) 00340 FIFO_occupation=FIFO_occupation+SIZE_FIFO; 00341 00342 if(FIFO_max_occupation<FIFO_occupation) 00343 FIFO_max_occupation=FIFO_occupation; 00344 00345 if(FIFO_occupation!=0) { 00346 00347 switch(msgRxBuffer[FIFO_lecture].id) { 00348 case CHECK_AX12: 00349 SendRawId(ALIVE_AX12); 00350 flag = 1; 00351 break; 00352 00353 case SERVO_AX12_ACTION : 00354 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; 00355 00356 //ACK de reception des actions a effectuer 00357 msgTx.id = SERVO_AX12_ACK; 00358 msgTx.len = 1; 00359 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; 00360 can1.write(msgTx); 00361 break; 00362 } 00363 00364 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; 00365 } 00366 } 00367 00368 /****************************************************************************************/ 00369 /* FUNCTION NAME: canRx_ISR */ 00370 /* DESCRIPTION : Interruption en réception sur le CAN */ 00371 /****************************************************************************************/ 00372 void canRx_ISR (void) 00373 { 00374 if (can1.read(msgRxBuffer[FIFO_ecriture])) { 00375 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); 00376 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; 00377 } 00378 } 00379 00380 /****************************************************************************************/ 00381 /* FUNCTION NAME: SendRawId */ 00382 /* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ 00383 /****************************************************************************************/ 00384 void SendRawId (unsigned short id) 00385 { 00386 CANMessage msgTx=CANMessage(); 00387 msgTx.id=id; 00388 msgTx.len=0; 00389 can1.write(msgTx); 00390 } 00391 void Init_AX12() // Initialisation des différents paramétres 00392 { 00393 un_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) 00394 sept_myAX12-> Set_Goal_speed(vitesse); 00395 huit_myAX12-> Set_Goal_speed(vitesse); 00396 seize_myAX12-> Set_Goal_speed(vitesse); 00397 00398 un_myAX12-> Set_Mode(0); 00399 sept_myAX12-> Set_Mode(0); 00400 huit_myAX12-> Set_Mode(0); 00401 seize_myAX12-> Set_Mode(0); 00402 } 00403 00404 00405 /****************************************************************************************/ 00406 /* FUNCTION NAME: Initialisation_position */ 00407 /* DESCRIPTION : Fonction qui place les bras en position verticale */ 00408 /****************************************************************************************/ 00409 void Initialisation_position(void){ 00410 multiple_myAX12->multiple_goal_and_speed(4,TAB12); 00411 wait(TIME); 00412 multiple_myAX12->multiple_goal_and_speed(4,TAB1); 00413 wait(TIME); 00414 } 00415 00416 /****************************************************************************************/ 00417 /* FUNCTION NAME: Preparation_prise */ 00418 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ 00419 /****************************************************************************************/ 00420 void Preparation_prise(void){ 00421 if (action_precedente == 0){ 00422 multiple_myAX12->multiple_goal_and_speed(4,TAB12); 00423 wait(TIME); 00424 action_precedente = 1; 00425 } 00426 multiple_myAX12->multiple_goal_and_speed(4,TAB2); 00427 wait(TIME); 00428 } 00429 00430 /****************************************************************************************/ 00431 /* FUNCTION NAME: Stockage_haut */ 00432 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ 00433 /****************************************************************************************/ 00434 void Stockage_haut(void){ 00435 multiple_myAX12->multiple_goal_and_speed(4,TAB3); 00436 wait(TIME); 00437 multiple_myAX12->multiple_goal_and_speed(4,TAB4); 00438 wait(TIME); 00439 multiple_myAX12->multiple_goal_and_speed(4,TAB5); 00440 wait(TIME); 00441 multiple_myAX12->multiple_goal_and_speed(4,TAB6); 00442 wait(TIME); 00443 } 00444 00445 /****************************************************************************************/ 00446 /* FUNCTION NAME: Stockage_bas */ 00447 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ 00448 /****************************************************************************************/ 00449 void Stockage_bas(void){ 00450 multiple_myAX12->multiple_goal_and_speed(4,TAB3); 00451 wait(TIME); 00452 multiple_myAX12->multiple_goal_and_speed(4,TAB7); 00453 wait(TIME); 00454 } 00455 00456 /****************************************************************************************/ 00457 /* FUNCTION NAME: Deposer */ 00458 /* DESCRIPTION : Fonction qui permet de déposer le module */ 00459 /****************************************************************************************/ 00460 void Deposer(void){ 00461 multiple_myAX12->multiple_goal_and_speed(4,TAB9); 00462 wait(TIME/5); 00463 multiple_myAX12->multiple_goal_and_speed(4,TAB8); 00464 wait(TIME/5); 00465 multiple_myAX12->multiple_goal_and_speed(4,TAB13); 00466 wait(TIME/5); 00467 } 00468 00469 /****************************************************************************************/ 00470 /* FUNCTION NAME: Preparation_depot_bas */ 00471 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ 00472 /****************************************************************************************/ 00473 void Preparation_depot_bas(void){ 00474 multiple_myAX12->multiple_goal_and_speed(4,TAB8); 00475 wait(TIME); 00476 } 00477 00478 /****************************************************************************************/ 00479 /* FUNCTION NAME: Preparation_depot_haut */ 00480 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ 00481 /****************************************************************************************/ 00482 void Preparation_depot_haut(void){ 00483 multiple_myAX12->multiple_goal_and_speed(4,TAB6); 00484 wait(TIME); 00485 multiple_myAX12->multiple_goal_and_speed(4,TAB5); 00486 wait(TIME); 00487 multiple_myAX12->multiple_goal_and_speed(4,TAB10); 00488 wait(TIME); 00489 multiple_myAX12->multiple_goal_and_speed(4,TAB8); 00490 wait(TIME); 00491 } 00492 00493 /****************************************************************************************/ 00494 /* FUNCTION NAME: Pousser_module */ 00495 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ 00496 /****************************************************************************************/ 00497 void Pousser_module(void){ 00498 multiple_myAX12->multiple_goal_and_speed(4,TAB11); 00499 wait(TIME); 00500 } 00501 00502 /****************************************************************************************/ 00503 /* FUNCTION NAME: Initialisation_gauche */ 00504 /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ 00505 /****************************************************************************************/ 00506 void Initialisation_gauche(void){ 00507 trois_myAX12_2->Set_Secure_Goal(235); 00508 multiple_myAX12_2->multiple_goal_and_speed(4,TAB22); 00509 wait(TIME); 00510 multiple_myAX12_2->multiple_goal_and_speed(4,TAB21); 00511 wait(TIME); 00512 } 00513 00514 /****************************************************************************************/ 00515 /* FUNCTION NAME: Preparation_prise_gauche */ 00516 /* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ 00517 /****************************************************************************************/ 00518 void Preparation_prise_gauche(void){ 00519 trois_myAX12_2->Set_Secure_Goal(120); 00520 } 00521 00522 /****************************************************************************************/ 00523 /* FUNCTION NAME: Prendre_module_gauche */ 00524 /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ 00525 /****************************************************************************************/ 00526 void Prendre_module_gauche(void){ 00527 trois_myAX12_2->Set_Secure_Goal(160); 00528 } 00529 00530 /****************************************************************************************/ 00531 /* FUNCTION NAME: Preparatio_module_gauche */ 00532 /* DESCRIPTION : Fonction qui prepare le tournante */ 00533 /****************************************************************************************/ 00534 void Preparation_module_gauche(void){ 00535 multiple_myAX12_2->multiple_goal_and_speed(4,TAB22); 00536 wait(TIME); 00537 } 00538 00539 /****************************************************************************************/ 00540 /* FUNCTION NAME: Tourner_module_gauche */ 00541 /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ 00542 /****************************************************************************************/ 00543 void Tourner_module_gauche(void){ 00544 multiple_myAX12_2->multiple_goal_and_speed(4,TAB23); 00545 wait(TIME); 00546 } 00547 00548 00549 /****************************************************************************************/ 00550 /* FUNCTION NAME: Fin_action */ 00551 /* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */ 00552 /****************************************************************************************/ 00553 void Fin_action(void){ 00554 CANMessage msgTx=CANMessage(); 00555 msgTx.format=CANStandard; 00556 msgTx.type=CANData; 00557 00558 msgTx.id = SERVO_AX12_END; 00559 msgTx.len = 1; 00560 msgTx.data[0] = AX12_PREPARATION_PRISE; 00561 can1.write(msgTx); 00562 } 00563 00564 /****************************************************************************************/ 00565 /* FUNCTION NAME: Automate_ax12 */ 00566 /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ 00567 /****************************************************************************************/ 00568 void AX12_automate(void){ 00569 switch(etat_ax12){ 00570 case AX12_INITIALISATION : 00571 if (flag == 1){ 00572 Initialisation_position(); 00573 flag = 2; 00574 } 00575 00576 break; 00577 00578 case AX12_PREPARATION_PRISE : 00579 Preparation_prise(); 00580 if (action == 0){ 00581 Fin_action(); 00582 action ++; 00583 } 00584 break; 00585 00586 case AX12_STOCKAGE_HAUT : 00587 Stockage_haut(); 00588 etat_ax12 = AX12_DEFAUT; 00589 Fin_action(); 00590 break; 00591 00592 case AX12_STOCKAGE_BAS : 00593 Stockage_bas(); 00594 etat_ax12 = AX12_DEFAUT; 00595 Fin_action(); 00596 break; 00597 00598 case AX12_DEPOSER : 00599 Deposer(); 00600 etat_ax12 = AX12_DEFAUT; 00601 Fin_action(); 00602 break; 00603 00604 case AX12_PREPARATION_DEPOT_BAS : 00605 Preparation_depot_bas(); 00606 etat_ax12 = AX12_DEFAUT; 00607 Fin_action(); 00608 break; 00609 00610 case AX12_PREPARATION_DEPOT_HAUT : 00611 Preparation_depot_haut(); 00612 etat_ax12 = AX12_DEFAUT; 00613 Fin_action(); 00614 break; 00615 00616 case AX12_POUSSER_MODULE : 00617 Pousser_module(); 00618 etat_ax12 = AX12_DEFAUT; 00619 Fin_action(); 00620 break; 00621 00622 case AX12_DEFAUT : 00623 action = 0; 00624 break; 00625 } 00626 }
Generated on Tue Jul 12 2022 20:39:16 by
1.7.2