File content as of revision 21:7c60d6dfcab5:

#include "peripheriques.h"
/* contient les fonctions qui servent à utiliser les AX12 et les moteurs sur le petit robot*/
/*
DigitalIn IO1(p23);
DigitalIn IO2(p24);
DigitalIn IO3(p25);
DigitalIn IO4(p26);

AnalogIn A_in1(p15);
AnalogIn A_in2(p16);
AnalogIn A_in3(p17);
AnalogIn A_in4(p18);
AnalogIn A_in5(p19);
AnalogIn A_in6(p20);

PwmOut IRL_1(p21);
PwmOut IRL_2(p22);
*/
PwmOut motGauche(p21);
PwmOut motDroit(p22);
Timer t;

unsigned char action_precedente = 0;
        
                             /*  DECLARATION VARIABLES */

extern unsigned char FlagAx12;

extern DigitalOut led2;
extern Serial pc;
extern Timer t;
extern void GetPositionAx12(void);

AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
     *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
     *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR; 
                             
               

/****************************************************************************************/
/* FUNCTION NAME: Tourner_module_gauche                                                 */
/* DESCRIPTION  : Fonction qui permet de tourner les modules a gauche                   */
/****************************************************************************************/
void Tourner_module_gauche(void){
    while(dataCouleurGauche() == false){
            printf("ici");
            moteurDroitPWM(0.2);
        } 
    moteurDroitPWM(0);
} 
/****************************************************************************************/
/* FUNCTION NAME: Tourner_module_droit                                                  */
/* DESCRIPTION  : Fonction qui permet de tourner les modules a gauche                   */
/****************************************************************************************/
void Tourner_module_droit(void){
    while(dataCouleurDroit() == false){
            printf("ici");
            moteurGauchePWM(0.2);
        } 
    moteurGauchePWM(0);
} 


/********************************************************************************************************/


/*********************************************************************************************************/
/* FUNCTION NAME: moteurGauchePWM                                                                        */
/* DESCRIPTION  : bouge le moteur gauche                                                                 */
/*********************************************************************************************************/
void moteurGauchePWM(float pwm){
    motGauche.write(pwm); 
}

/*********************************************************************************************************/
/* FUNCTION NAME: moteurDroitPWM                                                                        */
/* DESCRIPTION  : bouge le moteur gauche                                                                 */
/*********************************************************************************************************/
void moteurDroitPWM(float pwm){
    motDroit.write(pwm); 
}

/*********************************************************************************************************/
/* FUNCTION NAME: initMoteurs                                                                            */
/* DESCRIPTION  : init les moteurs des mains du petit robot                                              */
/*********************************************************************************************************/
void initMoteurs(void){
    motGauche.period(T_MOT);
    motDroit.period(T_MOT);
    motGauche.write(0.0);
    motDroit.write(0.0);
}

void initialisation_AX12(void) 
{
    short vitesse=700;
    
    BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);  
    CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000); 
    PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
    PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
    DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
    
    CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
    CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
    EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
    
    CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
    CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
    EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
    
    BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
    BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
    BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
    
    BaseBrasCentralPR->Set_Goal_speed(vitesse);  
    CoudeBrasCentralPR->Set_Goal_speed(vitesse); 
    PinceDBrasCentralPR->Set_Goal_speed(vitesse); 
    PinceGBrasCentralPR->Set_Goal_speed(vitesse); 
    DoigtBrasCentralPR->Set_Goal_speed(vitesse);
    
    CrocBrasGauchePR->Set_Goal_speed(vitesse); 
    CoudeBrasGauchePR->Set_Goal_speed(vitesse); 
    EpauleBrasGauchePR->Set_Goal_speed(vitesse);
    
    CrocBrasDroitPR->Set_Goal_speed(vitesse); 
    CoudeBrasDroitPR->Set_Goal_speed(vitesse); 
    EpauleBrasDroitPR->Set_Goal_speed(vitesse);
    
    BaseBrasCentralPR->Set_Mode(0); 
    CoudeBrasCentralPR->Set_Mode(0); 
    PinceDBrasCentralPR->Set_Mode(0); 
    PinceGBrasCentralPR->Set_Mode(0); 
    DoigtBrasCentralPR->Set_Mode(0);
    
    CrocBrasGauchePR->Set_Mode(0); 
    CoudeBrasGauchePR->Set_Mode(0);
    EpauleBrasGauchePR->Set_Mode(0);
    
    CrocBrasDroitPR->Set_Mode(0); 
    CoudeBrasDroitPR->Set_Mode(0);
    EpauleBrasDroitPR->Set_Mode(0);
} 

void GetPositionAx12(void) {

    printf("\n\r ");

    printf("BaseC  : %lf \n\r ",   BaseBrasCentralPR->Get_Position()   );
    printf("CoudeC : %lf \n\r ",   CoudeBrasCentralPR->Get_Position()  );
    printf("PinceCD : %lf \n\r ",   PinceDBrasCentralPR->Get_Position());   
    printf("PinceCG : %lf \n\r ",   PinceGBrasCentralPR->Get_Position()); 
    printf("DoigtC : %lf \n\r ",   DoigtBrasCentralPR->Get_Position()  );    
    
    printf("EpauleG : %lf \n\r ",   EpauleBrasGauchePR->Get_Position());   
    printf("CoudeG : %lf \n\r ",   CoudeBrasGauchePR->Get_Position() ); 
    printf("CrocG : %lf \n\r ",   CrocBrasGauchePR->Get_Position()   ); 
    
    printf("EpauleD : %lf \n\r ",   EpauleBrasDroitPR->Get_Position());   
    printf("CoudeD : %lf \n\r ",   CoudeBrasDroitPR->Get_Position() ); 
    printf("CrocD : %lf \n\r ",   CrocBrasDroitPR->Get_Position()   ); 
}


/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12                                                         */
/* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
/****************************************************************************************/
void AX12_automate(unsigned char etat_ax12){
    
    unsigned short speed;
    unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
                 GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
                 GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
        
    speed = 1000;    
        
    switch(etat_ax12){
               
        case AX12_PINCE_CENTRALE_POSITION_INITIALE :
            wait_ms(TIME);
            speed = 511;
            GoalPosDoigt=1150;
            GoalPosBase=1490;
            GoalPosCoude=1470;
            GoalPosPinceG=1090;
            GoalPosPinceD=1930;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
            break;
            
        case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=170;
            GoalPosCoude=1000;
            GoalPosPinceG=1090;
            GoalPosPinceD=1930;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
               
            break;    
            
        case AX12_PINCE_CENTRALE_PRISE_MODULE :
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=170;
            GoalPosCoude=1000;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
               
            break;  
            
        case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=1300;
            GoalPosCoude=700;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
            
            wait(TIME*5);
            GoalPosDoigt=90;
            GoalPosBase=1450;//1050;
            GoalPosCoude=700;//1528;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
           
            wait(TIME*5);
            GoalPosDoigt=90;
            GoalPosBase=1450;//1050;
            GoalPosCoude=1250;//1528;
            GoalPosPinceG=1090;
            GoalPosPinceD=1930;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
              
            break;  
        
        case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=1000;
            GoalPosCoude=443;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
               
            break;
        
        case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=639;
            GoalPosCoude=557;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   

            wait(TIME*10);
            GoalPosDoigt=90;
            GoalPosBase=400;
            GoalPosCoude=400;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
    
               
            break;
            
        case AX12_PINCE_CENTRALE_DEPOSER :
            //DEPOSER
            wait_ms(TIME);
            GoalPosDoigt=90;
            GoalPosBase=440;
            GoalPosCoude=440;
            GoalPosPinceG=1090;
            GoalPosPinceD=1930;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   

               
            break;
        
        case AX12_PINCE_CENTRALE_DEPOT_HAUT :
            wait(TIME*5);
            GoalPosDoigt=90;
            GoalPosBase=1050;
            GoalPosCoude=1528;
            GoalPosPinceG=1090;
            GoalPosPinceD=1930;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
            
            wait(TIME*10);
            GoalPosDoigt=90;
            GoalPosBase=1050;
            GoalPosCoude=1528;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
            
            wait(TIME*10);
            GoalPosDoigt=90;
            GoalPosBase=1100;
            GoalPosCoude=700;
            GoalPosPinceG=500;
            GoalPosPinceD=2500;
            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
            
                
            break;  
            
        case AX12_GAUCHE_CROC_OUVERT :
            wait_ms(TIME);
            CrocBrasGauchePR->Set_Secure_Goal(110);
            break;
            
        case AX12_GAUCHE_CROC_FERME :
            wait_ms(TIME);
            CrocBrasGauchePR->Set_Secure_Goal(147);
            break;     
            
        case AX12_GAUCHE_CROC_INITIALE :
            wait_ms(TIME);
            CrocBrasGauchePR->Set_Secure_Goal(232);
            break;         
            
        case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteG=1450;
            GoalPosEpauleTournanteG=600;
            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   

               
            break;
            
        case AX12_TOURNANTE_GAUCHE_PREPARATION :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteG=930;
            GoalPosEpauleTournanteG=1962;
            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   

               
            break;
            
        case AX12_TOURNANTE_GAUCHE_MODULE :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteG=894;
            GoalPosEpauleTournanteG=2200;
            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   

               
            break;
            
        case AX12_DROIT_CROC_OUVERT :
            wait_ms(TIME);
            CrocBrasDroitPR->Set_Secure_Goal(189);
               
            break;
            
        case AX12_DROIT_CROC_FERME :
            wait_ms(TIME);
            CrocBrasDroitPR->Set_Secure_Goal(149);
               
            break;    
            
        case AX12_DROIT_CROC_INITIALE :
            wait_ms(TIME);
            CrocBrasDroitPR->Set_Secure_Goal(67);
            break;     
            
        case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteD=1610;
            GoalPosEpauleTournanteD=2337;
            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   

               
            break;
            
        case AX12_TOURNANTE_DROIT_PREPARATION :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteD=930;
            GoalPosEpauleTournanteD=1962;
            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   

               
            break;
            
        case AX12_TOURNANTE_DROIT_MODULE :
            wait_ms(TIME);
            speed = 511;
            GoalPosCoudeTournanteD=894;
            GoalPosEpauleTournanteD=2200;
            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   

               
            break;
        
        case AX12_DEFAUT :
            break;
            
        case AX12_POSITION :
            GetPositionAx12();
            break;
    }
}

void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
                              unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
                              unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
                              unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
                              unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
{
    char TabBrasCentralPR[25];
    unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
    Timer  timeOut;
    
          
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
    GPosition3_1=((unsigned long)GPosition3*341/1000);
    GPosition4_1=((unsigned long)GPosition4*341/1000);
    GPosition5_1=((unsigned long)GPosition5*341/1000);
   
    TabBrasCentralPR[0] = ID1;
    TabBrasCentralPR[1] = GPosition1_1;
    TabBrasCentralPR[2] = GPosition1_1>>8; 
    TabBrasCentralPR[3] = GSpeed1; 
    TabBrasCentralPR[4] = GSpeed1>>8;               
             
    TabBrasCentralPR[5] = ID2;
    TabBrasCentralPR[6] = GPosition2_1;
    TabBrasCentralPR[7] = GPosition2_1>>8;
    TabBrasCentralPR[8] = GSpeed2;
    TabBrasCentralPR[9] = GSpeed2>>8;
   
    TabBrasCentralPR[10] = ID3;
    TabBrasCentralPR[11] = GPosition3_1;
    TabBrasCentralPR[12] = GPosition3_1>>8;
    TabBrasCentralPR[13] = GSpeed3;
    TabBrasCentralPR[14] = GSpeed3>>8  ;  
    
    TabBrasCentralPR[15] = ID4;
    TabBrasCentralPR[16] = GPosition4_1;
    TabBrasCentralPR[17] = GPosition4_1>>8;
    TabBrasCentralPR[18] = GSpeed4;
    TabBrasCentralPR[19] = GSpeed4>>8  ; 
    
    TabBrasCentralPR[20] = ID5;
    TabBrasCentralPR[21] = GPosition5_1;
    TabBrasCentralPR[22] = GPosition5_1>>8;
    TabBrasCentralPR[23] = GSpeed5;
    TabBrasCentralPR[24] = GSpeed5>>8  ; 
    
                       
    BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
    wait(TIME);
    
    timeOut.start();
    while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() < 100)) {
        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
        wait(TIME);
        
    } 
    
    timeOut.reset();    
    while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*90/100) || (timeOut.read_ms() < 100)) {
        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
        wait(TIME);
    } 
    
    timeOut.reset();
    while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*110/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*90/100) || (timeOut.read_ms() < 100)) {
        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
        wait(TIME);
    } 
    
    timeOut.reset();
    while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*110/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*90/100) || (timeOut.read_ms() < 100)) {
        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
        wait(TIME);
    } 
    
    timeOut.reset();
    while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*110/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*90/100) || (timeOut.read_ms() < 100)) {
        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
        wait(TIME);
    }
    
   
}


void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
                              unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
{
    char TabBrasGauchePR[10];
    unsigned short GPosition1_1, GPosition2_1;
    Timer  timeOut;
      
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
   
    TabBrasGauchePR[0] = ID1;
    TabBrasGauchePR[1] = GPosition1_1;
    TabBrasGauchePR[2] = GPosition1_1>>8; 
    TabBrasGauchePR[3] = GSpeed1; 
    TabBrasGauchePR[4] = GSpeed1>>8;               
             
    TabBrasGauchePR[5] = ID2;
    TabBrasGauchePR[6] = GPosition2_1;
    TabBrasGauchePR[7] = GPosition2_1>>8;
    TabBrasGauchePR[8] = GSpeed2;
    TabBrasGauchePR[9] = GSpeed2>>8;
    
                        
    BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
    wait(TIME);
    
    timeOut.start();
    while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*90/100)  || (timeOut.read_ms() < 100)) {
        BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
        wait(TIME);
    } 
    timeOut.reset();
    
    while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*90/100)  || (timeOut.read_ms() < 100)) {
        BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
        wait(TIME);
    }  
    timeOut.reset();
}

void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
                    unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
{
    char TabBrasDroitPR[10];
    unsigned short GPosition1_1, GPosition2_1;
    Timer  timeOut;
      
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
   
    TabBrasDroitPR[0] = ID1;
    TabBrasDroitPR[1] = GPosition1_1;
    TabBrasDroitPR[2] = GPosition1_1>>8; 
    TabBrasDroitPR[3] = GSpeed1; 
    TabBrasDroitPR[4] = GSpeed1>>8;               
             
    TabBrasDroitPR[5] = ID2;
    TabBrasDroitPR[6] = GPosition2_1;
    TabBrasDroitPR[7] = GPosition2_1>>8;
    TabBrasDroitPR[8] = GSpeed2;
    TabBrasDroitPR[9] = GSpeed2>>8;
    
                        
    BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
    wait(TIME);
    
    timeOut.start();
    while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*90/100)  || (timeOut.read_ms() < 100)) {
        BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
        wait(TIME);
    } 
    timeOut.reset();
    
    while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*90/100)  || (timeOut.read_ms() < 100)) {
        BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
        wait(TIME);
    } 
    timeOut.reset();
}