Actionneur.cpp

00001 #include "Actionneur.h"
00002 
00003 extern CAN can;       
00004         
00005                              /*  DECLARATION VARIABLES */
00006 
00007 extern unsigned char FlagAx12;
00008 extern "C" void mbed_reset();//Pour pouvoir reset la carte
00009 
00010 extern DigitalOut led2;
00011 extern Serial pc;
00012 extern void GetPositionAx12(void);
00013 extern void gerer_turbine(unsigned char pwm_turbine);
00014 extern PwmOut PompeDroite;
00015 extern unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
00016                                 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
00017                                 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3);
00018 
00019 static float TAB_ANGLE1[4], TAB_ANGLE2[4];
00020 static char TAB_POSITION[4];
00021 AX12 *un_myMX12, *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; 
00022 AX12 *TrappeLanceur, *MilieuBrasPompe, *HautBrasPompe, *BasBrasPompe, *BrasPompeAx12, *TabPompeBras;                                 
00023                                  
00024                      
00025                    
00026                             /*   ANGLE   */
00027                             
00028 /*         10° =    0x21, 0x00   |    110°= 0x6E, 0x01    |   210°= 0xBC, 0x02
00029            20° =    0x42, 0x00   |    120°= 0x90, 0x01    |   220°= 0xDD, 0x02
00030            30° =    0x64, 0x00   |    130°= 0xB1, 0x01
00031            40° =    0x85, 0x00   |    140°= 0xD2, 0x01
00032            50° =    0xA6, 0x00   |    150°= 0xF4, 0x01
00033            60° =    0xC8, 0x00   |    160°= 0x15, 0x02
00034            70° =    0xE9, 0x00   |    170°= 0x36, 0x02
00035            80° =    0x0A, 0x01   |    180°= 0x58, 0x02
00036            90° =    0x2C, 0x01   |    190°= 0x79, 0x02
00037            100°=    0x4D, 0x01   |    200°= 0x9A, 0x02                         */                   
00038 
00039                             /*  NUMERO AX12  */
00040                             
00041 /*         0 =    0x00   |    9  =    0x09  |  18 =   0x12
00042            1 =    0x01   |    10 =    0x0A 
00043            2 =    0x02   |    11 =    0x0B
00044            3 =    0x03   |    12 =    0x0C
00045            4 =    0x04   |    13 =    0x0D
00046            5 =    0x05   |    14 =    0x0E
00047            6 =    0x06   |    15 =    0x0F
00048            7 =    0x07   |    16 =    0x10
00049            8 =    0x08   |    17 =    0x11                      */
00050 
00051 
00052                   
00053                                         /* MAIN */                 
00054 
00055 void initialisation_AX12(void) 
00056 {
00057     short vitesse=700;
00058     
00059     TrappeLanceur = new AX12(p9, p10, 12, 1000000); // MX12
00060     
00061     HautBrasPompe = new AX12(p9, p10, 3, 1000000);  
00062     MilieuBrasPompe = new AX12(p9, p10, 2, 1000000); 
00063     BasBrasPompe = new AX12(p9, p10, 1, 1000000);
00064     
00065     BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
00066     
00067     TrappeLanceur->Set_Goal_speed(vitesse);  // MX12
00068     
00069     HautBrasPompe->Set_Goal_speed(vitesse);  
00070     MilieuBrasPompe->Set_Goal_speed(vitesse); 
00071     BasBrasPompe->Set_Goal_speed(vitesse); 
00072     
00073     HautBrasPompe->Set_Mode(0); 
00074     MilieuBrasPompe->Set_Mode(0); 
00075     BasBrasPompe->Set_Mode(0); 
00076   
00077 } 
00078 
00079 void GetPositionAx12(void) {
00080     
00081     pc.printf("\n\r * Bras Pompe * \n\r");
00082     
00083     pc.printf("Haut  : %lf \n\r ",   HautBrasPompe->Get_Position()      );
00084     pc.printf("Milieu : %lf \n\r ",   MilieuBrasPompe->Get_Position()    );
00085     pc.printf("Bas: %lf \n\r ",   BasBrasPompe->Get_Position() );
00086     pc.printf("Bas: %lf \n\r ",   TrappeLanceur->Get_Position() );   // MX12
00087     
00088 }
00089 
00090 
00091 /****************************************************************************************/
00092 /* FUNCTION NAME: Automate_ax12                                                         */
00093 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00094 /****************************************************************************************/
00095 void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras){
00096     
00097     unsigned short speed;
00098     
00099     static unsigned char action = 0;
00100     unsigned int GoalPos1, GoalPos2, GoalPos3;
00101     switch(etat_ax12){
00102         
00103         case AX12_GET_POSITION: // 10 (10)
00104             GetPositionAx12();
00105             break;
00106         
00107         
00108         case AX12_VIDER_FUSEE_POMPE: // 8 (10)
00109             
00110             speed=511;
00111             
00112             GoalPos1=1500;
00113             GoalPos2=624;
00114             GoalPos3=1413;
00115             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00116             GoalPos1+=900;
00117             GoalPos2=202;
00118             GoalPos3=2205;
00119             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(20*TIME);
00120             GoalPos3=2557;
00121             GoalPos2=173;
00122             GoalPos1=1947;
00123             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
00124             GoalPos3=2557;
00125             GoalPos2=370;
00126             GoalPos1=1798;
00127             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00128             GoalPos3=2548;
00129             GoalPos2=683;
00130             GoalPos1=1499;
00131             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00132             GoalPos3=2405;
00133             GoalPos2=1150;
00134             GoalPos1=1158;
00135             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00136               
00137             PompeDroite.write(0);
00138 
00139             Fin_action();          
00140             break;
00141             
00142         case AX12_VIDER_FUSEE_POMPEBLEU: // 11 (10)
00143             
00144             speed=511;
00145             
00146             GoalPos3=2205;
00147             GoalPos2=202;
00148             GoalPos1=598;
00149             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00150             GoalPos3=2557;
00151             GoalPos2=173;
00152             GoalPos1=1050;
00153             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(5*TIME);
00154             GoalPos3=2557;
00155             GoalPos2=370;
00156             GoalPos1=1199;
00157             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
00158             GoalPos3=2548;
00159             GoalPos2=683;
00160             GoalPos1=1499;
00161             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00162             GoalPos3=2405;
00163             GoalPos2=1150;
00164             GoalPos1=1947;
00165             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
00166             
00167             PompeDroite.write(0);
00168 
00169             Fin_action();          
00170             break;
00171             
00172          case AX12_REMPLIR_BASE_POMPE: // 9 (10)
00173          
00174             speed=511;
00175             
00176             GoalPos3=2440;
00177             GoalPos2=302;
00178             GoalPos1=1950;
00179             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00180             GoalPos3=1865;
00181             GoalPos2=252;
00182             GoalPos1=2400;
00183             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00184             GoalPos3=1500;
00185             GoalPos2=460;
00186             GoalPos1=2400;
00187             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
00188             GoalPos3=1396;
00189             GoalPos2=600;
00190             GoalPos1=1399;
00191             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00192             
00193             GoalPos3=1413;
00194             GoalPos2=624;
00195             GoalPos1=1500;
00196             mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
00197             
00198             Fin_action();
00199             break;   
00200         
00201         case AX12_INITIALISATION : // 0
00202             if (FlagAx12 == 1){
00203                 Initialisation_position(1);
00204                 //Check_positionAX12(&TAB1[25], 1);
00205                 Initialisation_position(2);
00206                 //Check_positionAX12(&TAB21[25], 2);
00207                 FlagAx12 = 2;
00208             }
00209             else if (choix_bras > 1){
00210                 Initialisation_position(choix_bras);
00211             }
00212             break;
00213                         
00214         case AX12_PREPARATION_PRISE : // 1 (10)
00215             Preparation_prise(choix_bras);
00216             if (action == 0){
00217                 Fin_action();
00218                 action ++;
00219             }
00220             break;
00221                         
00222         case AX12_STOCKAGE_HAUT :       // 2 (10)
00223             Stockage_haut(choix_bras);
00224             etat_ax12 = AX12_DEFAUT;
00225             Fin_action();
00226             break;
00227                         
00228         case AX12_STOCKAGE_BAS :        // 3 (10)
00229             Stockage_bas(choix_bras);
00230             etat_ax12 = AX12_DEFAUT;
00231             Fin_action();
00232             break;
00233                         
00234         case AX12_DEPOSER : // 4 (10)
00235             Deposer(choix_bras);
00236             etat_ax12 = AX12_DEFAUT;
00237             Fin_action();
00238             break;
00239                         
00240         case AX12_PREPARATION_DEPOT_BAS :       // 5 (10)
00241             Preparation_depot_bas(choix_bras);
00242             etat_ax12 = AX12_DEFAUT;
00243             Fin_action();
00244             break;
00245                         
00246         case AX12_PREPARATION_DEPOT_HAUT :      // 6 (10)
00247             Preparation_depot_haut(choix_bras);
00248             etat_ax12 = AX12_DEFAUT;
00249             Fin_action();
00250             break;
00251                         
00252         case AX12_POUSSER_MODULE : // 7 (10)
00253             Pousser_module(choix_bras);
00254             etat_ax12 = AX12_DEFAUT;
00255             Fin_action();
00256             break;
00257         
00258         case BOUGER_MX12 : // 12 (10)
00259             bouger_MX12(choix_bras); // Le choix du bras détermine le sens de rotation de l'MX12 : 0 position initiale | 1 tourne à droite | 2 tourne à gauche.
00260             etat_ax12 = AX12_DEFAUT;
00261             Fin_action();
00262             break;
00263             
00264         case AX12_DEFAUT : // 20 (10)
00265         action = 0;
00266             break;
00267     }
00268 }
00269    
00270 
00271 unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
00272                                 unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
00273                                 unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3)
00274 {
00275    char TabPompeBras[15];
00276    unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
00277    
00278    GPosition1_1=((unsigned long)GPosition1*341/1000);
00279    GPosition2_1=((unsigned long)GPosition2*341/1000);
00280    GPosition3_1=((unsigned long)GPosition3*341/1000);
00281    
00282    TabPompeBras[0] = ID1;
00283    TabPompeBras[1] = GPosition1_1;
00284    TabPompeBras[2] = GPosition1_1>>8; 
00285    TabPompeBras[3] = GSpeed1; 
00286    TabPompeBras[4] = GSpeed1>>8;               /// (haut, milieu, bas)  
00287              
00288    TabPompeBras[5] = ID2;
00289     TabPompeBras[6] = GPosition2_1;
00290     TabPompeBras[7] = GPosition2_1>>8;
00291     TabPompeBras[8] = GSpeed2;
00292     TabPompeBras[9] = GSpeed2>>8;
00293    
00294     TabPompeBras[10] = ID3;
00295     TabPompeBras[11] = GPosition3_1;
00296     TabPompeBras[12] = GPosition3_1>>8;
00297     TabPompeBras[13] = GSpeed3;
00298     TabPompeBras[14] = GSpeed3>>8  ;  
00299                         
00300    BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
00301    wait(TIME);
00302    
00303    
00304    while (((unsigned short)(HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
00305         BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
00306         wait(TIME*5);
00307     }
00308     
00309     while (((unsigned short)(MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
00310         BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
00311         wait(TIME*5);
00312     }
00313     
00314     
00315     while (((unsigned short)(BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
00316         BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
00317         wait(TIME*5);
00318     }
00319     
00320    
00321     
00322 }
00323 
00324   
00325 
00326 /****************************************************************************************/
00327 /* FUNCTION NAME: Initialisation_position                                               */
00328 /* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
00329 /****************************************************************************************/
00330 void Initialisation_position(unsigned char choix){
00331     TrappeLanceur->Set_Secure_Goal(600); // Initialise la position de l'MX12 à 180°
00332     if (choix == 1){
00333         multiple_myAX12->multiple_goal_and_speed(4,TAB1);
00334         wait(TIME);  
00335     }
00336     
00337     else if (choix == 2){
00338         multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
00339         wait(TIME);
00340     }
00341     else if (choix == 3){
00342         BrasPompeAx12->multiple_goal_and_speed(3,TabBrasPompePosInit);
00343         wait(TIME);
00344     }    
00345 }
00346 
00347 /****************************************************************************************/
00348 /* FUNCTION NAME: Preparation_prise                                                     */
00349 /* DESCRIPTION  : Fonction qui prepare le robot pour prendre les modules                */
00350 /****************************************************************************************/
00351 void Preparation_prise(unsigned char choix){    
00352     if (choix == 1){
00353         multiple_myAX12->multiple_goal_and_speed(4,TAB2);
00354         wait(TIME);
00355     }
00356     
00357     else if (choix == 2){
00358         multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
00359         wait(TIME);
00360     }
00361 }
00362 
00363 /****************************************************************************************/
00364 /* FUNCTION NAME: Stockage_haut                                                         */
00365 /* DESCRIPTION  : Fonction qui prend et stocke les modules dans la position haute       */
00366 /****************************************************************************************/
00367 void Stockage_haut(unsigned char choix){
00368     if (choix == 1){
00369         multiple_myAX12->multiple_goal_and_speed(4,TAB3);
00370         wait(TIME);
00371         multiple_myAX12->multiple_goal_and_speed(4,TAB4);
00372         wait(TIME);
00373         multiple_myAX12->multiple_goal_and_speed(4,TAB5);
00374         wait(TIME);
00375         multiple_myAX12->multiple_goal_and_speed(4,TAB6);
00376         wait(TIME);
00377     }
00378     
00379     else if (choix == 2){
00380         multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
00381         wait(TIME);
00382         multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
00383         wait(TIME);
00384         multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
00385         wait(TIME);
00386         multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
00387         wait(TIME);
00388     }
00389 }
00390 
00391 /****************************************************************************************/
00392 /* FUNCTION NAME: Stockage_bas                                                          */
00393 /* DESCRIPTION  : Fonction qui prend et stocke un module dans la pince                  */
00394 /****************************************************************************************/
00395 void Stockage_bas(unsigned char choix){
00396     if (choix == 1){
00397         multiple_myAX12->multiple_goal_and_speed(4,TAB3);
00398         wait(TIME);
00399         multiple_myAX12->multiple_goal_and_speed(4,TAB7);
00400         wait(TIME);
00401     } 
00402     
00403     else if (choix == 2){
00404         multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
00405         wait(TIME);
00406         multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
00407         wait(TIME);
00408     }       
00409 }
00410 
00411 /****************************************************************************************/
00412 /* FUNCTION NAME: Deposer                                                               */
00413 /* DESCRIPTION  : Fonction qui permet de déposer le module                              */
00414 /****************************************************************************************/
00415 void Deposer(unsigned char choix){
00416     if (choix == 1){
00417         multiple_myAX12->multiple_goal_and_speed(4,TAB9);
00418         wait(TIME);
00419     }  
00420     
00421     else if (choix == 2){
00422         multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
00423         wait(TIME);
00424     }        
00425 }
00426 
00427 /****************************************************************************************/
00428 /* FUNCTION NAME: Preparation_depot_bas                                                 */
00429 /* DESCRIPTION  : Fonction qui prépare le depos d'un module en bas                      */
00430 /****************************************************************************************/
00431 void Preparation_depot_bas(unsigned char choix){
00432     if (choix == 1){
00433         multiple_myAX12->multiple_goal_and_speed(4,TAB8);
00434         wait(TIME);
00435     }
00436     
00437     else if (choix == 2){
00438         multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
00439         wait(TIME);
00440     }
00441 }
00442 
00443 /****************************************************************************************/
00444 /* FUNCTION NAME: Preparation_depot_haut                                                */
00445 /* DESCRIPTION  : Fonction qui prépare le depos d'un module en haut                     */
00446 /****************************************************************************************/
00447 void Preparation_depot_haut(unsigned char choix){
00448     if (choix == 1){
00449         multiple_myAX12->multiple_goal_and_speed(4,TAB6);
00450         wait(TIME);
00451         multiple_myAX12->multiple_goal_and_speed(4,TAB5);
00452         wait(TIME);
00453         multiple_myAX12->multiple_goal_and_speed(4,TAB10);
00454         wait(TIME); 
00455         multiple_myAX12->multiple_goal_and_speed(4,TAB8);
00456         wait(TIME);   
00457     }
00458     
00459     else if (choix == 2){
00460         multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
00461         wait(TIME);
00462         multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
00463         wait(TIME);
00464         multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
00465         wait(TIME); 
00466         multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
00467         wait(TIME);   
00468     }
00469 }
00470 
00471 /****************************************************************************************/
00472 /* FUNCTION NAME: bouger_MX12                                                           */
00473 /* DESCRIPTION  : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge    */
00474 /****************************************************************************************/
00475 void bouger_MX12(unsigned char choix){
00476     if( choix == 1) // Tourne à droite
00477     {
00478         TrappeLanceur->Set_Secure_Goal(0); // tourner droite
00479         wait(TIME);
00480     }
00481     else if(choix == 2) // Tourne à gauche
00482     {
00483         TrappeLanceur->Set_Secure_Goal(1200); // tourner gauche
00484         wait(TIME);
00485     }
00486     else if (choix == 0)
00487     {
00488         TrappeLanceur->Set_Secure_Goal(600); // position initiale
00489         wait(TIME);
00490     }
00491 }
00492 
00493 
00494 /****************************************************************************************/
00495 /* FUNCTION NAME: Pousser_module                                                        */
00496 /* DESCRIPTION  : Fonction qui permet pousser le module situé à l'entrée de la bas      */
00497 /****************************************************************************************/
00498 void Pousser_module(unsigned char choix){
00499     if (choix == 1){
00500         multiple_myAX12->multiple_goal_and_speed(4,TAB11);
00501         wait(TIME);   
00502     }
00503     
00504     else if (choix == 2){
00505         multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
00506         wait(TIME);   
00507     }
00508 }
00509 
00510 /****************************************************************************************/
00511 /* FUNCTION NAME: Fin_action                                                            */
00512 /* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
00513 /****************************************************************************************/
00514 void Fin_action(void){
00515     CANMessage msgTx=CANMessage();
00516     msgTx.format=CANStandard;
00517     msgTx.type=CANData;
00518     
00519     msgTx.id = SERVO_AX12_END;
00520     msgTx.len = 1;  
00521     msgTx.data[0] = AX12_PREPARATION_PRISE;
00522     can.write(msgTx);  
00523 }
00524 
00525 
00526 
00527 
00528 /****************************************************************************************/
00529 /* FUNCTION NAME: Check_positionAX12                                                    */
00530 /* DESCRIPTION  : Fonction qui permet de verifier la position des AX12                  */
00531 /****************************************************************************************/
00532 void Check_positionAX12(char* TAB, unsigned char choix){
00533     int k=1, i=0;
00534     static float TAB_POS_TH[4];
00535     
00536     CANMessage msgTx=CANMessage();
00537     msgTx.id=SERVO_AX12_POSITION;
00538     msgTx.len=5;
00539     
00540     //PERMET DE VERIFIER LA POSITION D'UN AX12
00541     TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3);
00542     TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3);
00543     TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3);
00544     TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3);
00545     TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3);
00546     TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3);
00547     TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3);
00548     TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3);
00549      
00550     for(i=0; i<4; i++){
00551         TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8);    
00552         k=k+5;
00553     }
00554     
00555     for(i=0; i<4; i++){
00556         if (choix == 1){
00557             if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
00558                 TAB_POSITION[i] = 1;
00559                 }
00560             else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
00561                 TAB_POSITION[i] = 0;
00562                 }
00563             }
00564         else if (choix == 2){
00565             if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
00566                 TAB_POSITION[i] = 1;
00567                 }
00568             else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
00569                 TAB_POSITION[i] = 0;
00570                 }
00571             }
00572      }
00573     
00574     msgTx.data[0] = choix;
00575     for(i=1; i<5; i++){
00576         msgTx.data[i] = TAB_POSITION[i];
00577     }
00578     can.write(msgTx);
00579 }            
00580