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