CRAC Team
/
command_AX12_petit_robot_V3
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_v2
by 
CRAC Team
Embed:
(wiki syntax)
Show/hide line numbers
actionneurs.cpp
00001 #include"AX12.h" 00002 #include"mbed.h" 00003 #include "global.h" 00004 00005 #define TIME 0.8 00006 #define VITESSE 700 00007 DigitalOut led(LED1); 00008 00009 // AX12 partie centrale du petit robot 00010 AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral; 00011 AX12 *partieDoigtCentre, *multipleDoigtCentre; 00012 // AX12 partie gauche du robot 00013 AX12 *partieBasseGauche, *partieMainGauche, *multipleBrasGauche; 00014 AX12 *partiePorteGauche, *multiplePorteGauche; 00015 // AX12 partie droite du robot 00016 AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite; 00017 AX12 *partiePorteDroite, *multiplePorteDroite; 00018 00019 Serial pc(USBTX, USBRX); 00020 00021 Timer t; 00022 Ticker flipper; 00023 unsigned char action_precedente = 0; 00024 ////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// 00025 static char TAB1[25]= {0x01,0x15, 0x02, 0xFF, 0x00, ///Position initiale 00026 0x10,0x00, 0x02, 0xFF, 0x00, 00027 0x07,0x90, 0x01, 0xB1, 0x00, 00028 0x08,0x58, 0x02, 0x79, 0x00}; 00029 00030 static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x02, ///Preparation prise 00031 0x10,0x50, 0x01, 0xFF, 0x02, 00032 0x07,0xF4, 0x01, 0xFF, 0x02, 00033 0x08,0xF4, 0x01, 0xFF, 0x02}; 00034 00035 static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) 00036 0x10,0x50, 0x01, 0xFF, 0x03, 00037 0x07,0xC5, 0x00, 0xFF, 0x03, 00038 0x08,0x4D, 0x03, 0xFF, 0x03}; 00039 00040 static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) 00041 0x10,0x50, 0x01, 0xFF, 0x03, 00042 0x07,0xC5, 0x00, 0xFF, 0x03, 00043 0x08,0x4D, 0x03, 0xFF, 0x03}; 00044 00045 static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) 00046 0x10,0xF4, 0x01, 0xFF, 0x00, 00047 0x07,0xC5, 0x00, 0xFF, 0x03, 00048 0x08,0x4D, 0x03, 0xFF, 0x03}; 00049 00050 static char TAB6[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00051 0x10,0xF4, 0x01, 0xFF, 0x03, 00052 0x07,0x4d, 0x01, 0xFF, 0x03, 00053 0x08,0x9a, 0x02, 0xFF, 0x03}; 00054 00055 static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) 00056 0x10,0xB0, 0x00, 0xFF, 0x03, 00057 0x07,0xC5, 0x00, 0xFF, 0x00, 00058 0x08,0x4D, 0x03, 0xFF, 0x00}; 00059 00060 static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas 00061 0x10,0xF4, 0x01, 0xFF, 0x03, 00062 0x07,0xC5, 0x00, 0xFF, 0x00, 00063 0x08,0x4D, 0x03, 0xFF, 0x00}; 00064 00065 static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer 00066 0x10,0xF4, 0x01, 0xFF, 0x03, 00067 0x07,0xD0, 0x00, 0xFF, 0x00, 00068 0x08,0x35, 0x03, 0xFF, 0x00}; 00069 00070 static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) 00071 0x10,0x00, 0x01, 0xFF, 0x00, 00072 0x07,0xC5, 0x00, 0xFF, 0x03, 00073 0x08,0x4D, 0x03, 0xFF, 0x03}; 00074 00075 static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module 00076 0x10,0xF4, 0x01, 0xFF, 0x03, 00077 0x07,0xC5, 0x00, 0xFF, 0x00, 00078 0x08,0x4D, 0x03, 0xFF, 0x00}; 00079 00080 static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale 00081 0x10,0x00, 0x02, 0xFF, 0x03, 00082 0x07,0xF4, 0x01, 0xFF, 0x03, 00083 0x08,0xF4, 0x01, 0xFF, 0x03}; 00084 00085 static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer 00086 0x10,0xA0, 0x02, 0xFF, 0x03, 00087 0x07,0xD0, 0x00, 0xFF, 0x00, 00088 0x08,0x35, 0x03, 0xFF, 0x00}; 00089 00090 static char TAB14[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00091 0x10,0x42, 0x00, 0xFF, 0x03, 00092 0x07,0x4d, 0x01, 0xFF, 0x03, 00093 0x08,0x9a, 0x02, 0xFF, 0x03}; 00094 00095 static char TAB15[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige) 00096 0x10,0x42, 0x00, 0xFF, 0x00, 00097 0x07,0xC5, 0x00, 0xFF, 0x03, 00098 0x08,0x4D, 0x03, 0xFF, 0x03}; 00099 00100 static char TAB16[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) 00101 0x10,0x42, 0x00, 0xFF, 0x00, 00102 0x07,0x2c, 0x01, 0xFF, 0x03, 00103 0x08,0x4D, 0x03, 0xFF, 0x03}; 00104 ////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// 00105 static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale 00106 0x02,0xF4, 0x01, 0xFF, 0x03}; 00107 00108 static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner 00109 0x02,0x40, 0x03, 0xFF, 0x03}; 00110 00111 static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module 00112 0x02,0xE5, 0x02, 0xFF, 0x03}; 00113 00114 00115 00116 00117 /* ANGLE */ 00118 00119 /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 00120 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 00121 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 00122 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 00123 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 00124 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 00125 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 00126 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 00127 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 00128 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ 00129 00130 /* NUMERO AX12 */ 00131 00132 /* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 00133 1 = 0x01 | 10 = 0x0A 00134 2 = 0x02 | 11 = 0x0B 00135 3 = 0x03 | 12 = 0x0C 00136 4 = 0x04 | 13 = 0x0D 00137 5 = 0x05 | 14 = 0x0E 00138 6 = 0x06 | 15 = 0x0F 00139 7 = 0x07 | 16 = 0x10 00140 8 = 0x08 | 17 = 0x11 */ 00141 00142 00143 00144 00145 00146 00147 void declarationAX12(void){ 00148 00149 //Pince centrale 00150 //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral; 00151 partieBasseCentre = new AX12(p9, p10, 1, 1000000); 00152 partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000); 00153 partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000); 00154 partiePoignetCentre = new AX12(p9, p10, 16, 1000000); 00155 multipleBrasCentral = new AX12(p9,p10,0xFE,1000000); 00156 00157 //Bras de gauche 00158 partieBasseGauche = new AX12(p13, p14, 1, 1000000); 00159 partieMainGauche = new AX12(p13, p14, 2, 1000000); 00160 multipleBrasGauche = new AX12(p13,p14,0xFE,1000000); 00161 //Porte gauche 00162 partiePorteGauche = new AX12(p13, p14, 3, 1000000); 00163 multiplePorteGauche = new AX12(p13, p14, 0xFE, 1000000); 00164 00165 //Bras de droite 00166 partieBasseDroite = new AX12(p28, p27, 1, 1000000); 00167 partieMainDroite = new AX12(p28, p27, 2, 1000000); 00168 multipleBrasDroite = new AX12(p28,p27,0xFE,1000000); 00169 //Porte droite 00170 partiePorteDroite = new AX12(p28, p27, 3, 1000000); 00171 multiplePorteDroite = new AX12(p28, p27, 0xFE, 1000000); 00172 } 00173 00174 00175 void Init_AX12() // Initialisation des différents paramétres 00176 { 00177 partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023) 00178 partieMainGauche-> Set_Goal_speed(VITESSE); 00179 partieMainDroite-> Set_Goal_speed(VITESSE); 00180 partiePoignetCentre-> Set_Goal_speed(VITESSE); 00181 00182 partieBasseCentre-> Set_Mode(0); 00183 partieMainGauche-> Set_Mode(0); 00184 partieMainDroite-> Set_Mode(0); 00185 partiePoignetCentre-> Set_Mode(0); 00186 } 00187 00188 00189 /****************************************************************************************/ 00190 /* FUNCTION NAME: Initialisation_position */ 00191 /* DESCRIPTION : Fonction qui place les bras en position verticale */ 00192 /****************************************************************************************/ 00193 void Initialisation_position(void){ 00194 multipleBrasCentral->multiple_goal_and_speed(4,TAB12); 00195 wait(TIME); 00196 multipleBrasCentral->multiple_goal_and_speed(4,TAB1); 00197 wait(TIME); 00198 } 00199 00200 /****************************************************************************************/ 00201 /* FUNCTION NAME: Preparation_prise */ 00202 /* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ 00203 /****************************************************************************************/ 00204 void Preparation_prise(void){ 00205 /*if (action_precedente == 0){ 00206 multipleBrasCentral->multiple_goal_and_speed(4,TAB12); 00207 wait(TIME); 00208 action_precedente = 1; 00209 }*/ 00210 multipleBrasCentral->multiple_goal_and_speed(4,TAB2); 00211 wait(TIME); 00212 } 00213 00214 /****************************************************************************************/ 00215 /* FUNCTION NAME: Stockage_haut */ 00216 /* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ 00217 /****************************************************************************************/ 00218 void Stockage_haut(void){ 00219 multipleBrasCentral->multiple_goal_and_speed(4,TAB3); 00220 wait(TIME); 00221 multipleBrasCentral->multiple_goal_and_speed(4,TAB4); 00222 wait(TIME); 00223 multipleBrasCentral->multiple_goal_and_speed(4,TAB5); 00224 wait(TIME); 00225 multipleBrasCentral->multiple_goal_and_speed(4,TAB6); 00226 wait(TIME); 00227 } 00228 00229 /****************************************************************************************/ 00230 /* FUNCTION NAME: Stockage_bas */ 00231 /* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ 00232 /****************************************************************************************/ 00233 void Stockage_bas(void){ 00234 multipleBrasCentral->multiple_goal_and_speed(4,TAB3); 00235 wait(TIME); 00236 multipleBrasCentral->multiple_goal_and_speed(4,TAB7); 00237 wait(TIME); 00238 } 00239 00240 /****************************************************************************************/ 00241 /* FUNCTION NAME: Deposer */ 00242 /* DESCRIPTION : Fonction qui permet de déposer le module */ 00243 /****************************************************************************************/ 00244 void Deposer(void){ 00245 multipleBrasCentral->multiple_goal_and_speed(4,TAB9); 00246 wait(TIME/5); 00247 multipleBrasCentral->multiple_goal_and_speed(4,TAB8); 00248 wait(TIME/5); 00249 //multipleBrasCentral->multiple_goal_and_speed(4,TAB13); 00250 wait(TIME/5); 00251 } 00252 00253 /****************************************************************************************/ 00254 /* FUNCTION NAME: Preparation_depot_bas */ 00255 /* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ 00256 /****************************************************************************************/ 00257 void Preparation_depot_bas(void){ 00258 multipleBrasCentral->multiple_goal_and_speed(4,TAB8); 00259 wait(TIME); 00260 } 00261 00262 /****************************************************************************************/ 00263 /* FUNCTION NAME: Preparation_depot_haut */ 00264 /* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ 00265 /****************************************************************************************/ 00266 void Preparation_depot_haut(void){ 00267 multipleBrasCentral->multiple_goal_and_speed(4,TAB14); 00268 wait(TIME); 00269 multipleBrasCentral->multiple_goal_and_speed(4,TAB16); 00270 wait(TIME); 00271 multipleBrasCentral->multiple_goal_and_speed(4,TAB15); 00272 wait(TIME); 00273 multipleBrasCentral->multiple_goal_and_speed(4,TAB10); 00274 wait(TIME); 00275 multipleBrasCentral->multiple_goal_and_speed(4,TAB8); 00276 wait(TIME); 00277 } 00278 00279 /****************************************************************************************/ 00280 /* FUNCTION NAME: Pousser_module */ 00281 /* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ 00282 /****************************************************************************************/ 00283 void Pousser_module(void){ 00284 multipleBrasCentral->multiple_goal_and_speed(4,TAB11); 00285 wait(TIME); 00286 } 00287 00288 /****************************************************************************************/ 00289 /* FUNCTION NAME: Initialisation_gauche */ 00290 /* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ 00291 /****************************************************************************************/ 00292 void Initialisation_gauche(void){ 00293 //trois_myAX12_2->Set_Secure_Goal(235); 00294 multipleBrasGauche->multiple_goal_and_speed(4,TAB22); 00295 wait(TIME); 00296 multipleBrasGauche->multiple_goal_and_speed(4,TAB21); 00297 wait(TIME); 00298 } 00299 00300 /****************************************************************************************/ 00301 /* FUNCTION NAME: Preparation_prise_gauche */ 00302 /* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ 00303 /****************************************************************************************/ 00304 void Preparation_prise_gauche(void){ 00305 //trois_myAX12_2->Set_Secure_Goal(120); 00306 } 00307 00308 /****************************************************************************************/ 00309 /* FUNCTION NAME: Prendre_module_gauche */ 00310 /* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ 00311 /****************************************************************************************/ 00312 void Prendre_module_gauche(void){ 00313 //trois_myAX12_2->Set_Secure_Goal(160); 00314 } 00315 00316 /****************************************************************************************/ 00317 /* FUNCTION NAME: Preparatio_module_gauche */ 00318 /* DESCRIPTION : Fonction qui prepare le tournante */ 00319 /****************************************************************************************/ 00320 void Preparation_module_gauche(void){ 00321 multipleBrasGauche->multiple_goal_and_speed(4,TAB22); 00322 wait(TIME); 00323 } 00324 00325 /****************************************************************************************/ 00326 /* FUNCTION NAME: Tourner_module_gauche */ 00327 /* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ 00328 /****************************************************************************************/ 00329 void Tourner_module_gauche(void){ 00330 multipleBrasGauche->multiple_goal_and_speed(4,TAB23); 00331 wait(TIME); 00332 } 00333 00334 void getPosiotionCentrale(void){ 00335 //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, 00336 led = 1; 00337 pc.printf("\n\r * pince avant * \n\r"); 00338 00339 pc.printf("base : %lf \n\r ", partieBasseCentre->Get_Position() ); 00340 pc.printf("coude : %lf \n\r ", partiePoignetCentre->Get_Position() ); 00341 pc.printf("doigtG: %lf \n\r ", partieMainGaucheCentre->Get_Position() ); 00342 pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() ); 00343 00344 wait(0.2); 00345 led = 0; 00346 00347 } 00348 00349 void getPosiotionGauche(void){ 00350 //AX12 *partieBasseGauche, *partieMainGauche 00351 led = 1; 00352 pc.printf("\n\r * bras gauche *\n\r"); 00353 00354 pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position()); 00355 pc.printf("main : %lf \n\r ",partieMainGauche->Get_Position() ); 00356 pc.printf("porte : %lf \n\r ",partiePorteGauche->Get_Position() ); 00357 00358 wait(0.2); 00359 led = 0; 00360 00361 } 00362 00363 void getPosiotionDroite(void){ 00364 //AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite; 00365 led = 1; 00366 pc.printf("\n\r * bras droit *\n\r"); 00367 00368 pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position()); 00369 pc.printf("main : %lf \n\r ",partieMainDroite->Get_Position() ); 00370 pc.printf("porte : %lf \n\r ",partiePorteDroite->Get_Position() ); 00371 00372 00373 wait(0.2); 00374 led = 0; 00375 00376 }
Generated on Thu Jul 14 2022 20:44:47 by
1.7.2