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 }