actionneurs.cpp

00001 #include "peripheriques.h"
00002 /* contient les fonctions qui servent à utiliser les AX12 et les moteurs sur le petit robot*/
00003 /*
00004 DigitalIn IO1(p23);
00005 DigitalIn IO2(p24);
00006 DigitalIn IO3(p25);
00007 DigitalIn IO4(p26);
00008 
00009 AnalogIn A_in1(p15);
00010 AnalogIn A_in2(p16);
00011 AnalogIn A_in3(p17);
00012 AnalogIn A_in4(p18);
00013 AnalogIn A_in5(p19);
00014 AnalogIn A_in6(p20);
00015 
00016 PwmOut IRL_1(p21);
00017 PwmOut IRL_2(p22);
00018 */
00019 PwmOut motGauche(p21);
00020 PwmOut motDroit(p22);
00021 Timer t;
00022 
00023 unsigned char action_precedente = 0;
00024         
00025                              /*  DECLARATION VARIABLES */
00026 
00027 extern unsigned char FlagAx12;
00028 
00029 extern DigitalOut led2;
00030 extern Serial pc;
00031 extern Timer t;
00032 extern void GetPositionAx12(void);
00033 
00034 AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
00035      *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
00036      *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR; 
00037                              
00038                
00039 
00040 /****************************************************************************************/
00041 /* FUNCTION NAME: Tourner_module_gauche                                                 */
00042 /* DESCRIPTION  : Fonction qui permet de tourner les modules a gauche                   */
00043 /****************************************************************************************/
00044 void Tourner_module_gauche(void){
00045     while(dataCouleurGauche() == false){
00046             printf("ici");
00047             moteurDroitPWM(0.2);
00048         } 
00049     moteurDroitPWM(0);
00050 } 
00051 /****************************************************************************************/
00052 /* FUNCTION NAME: Tourner_module_droit                                                  */
00053 /* DESCRIPTION  : Fonction qui permet de tourner les modules a gauche                   */
00054 /****************************************************************************************/
00055 void Tourner_module_droit(void){
00056     while(dataCouleurDroit() == false){
00057             printf("ici");
00058             moteurGauchePWM(0.2);
00059         } 
00060     moteurGauchePWM(0);
00061 } 
00062 
00063 
00064 /********************************************************************************************************/
00065 
00066 
00067 /*********************************************************************************************************/
00068 /* FUNCTION NAME: moteurGauchePWM                                                                        */
00069 /* DESCRIPTION  : bouge le moteur gauche                                                                 */
00070 /*********************************************************************************************************/
00071 void moteurGauchePWM(float pwm){
00072     motGauche.write(pwm); 
00073 }
00074 
00075 /*********************************************************************************************************/
00076 /* FUNCTION NAME: moteurDroitPWM                                                                        */
00077 /* DESCRIPTION  : bouge le moteur gauche                                                                 */
00078 /*********************************************************************************************************/
00079 void moteurDroitPWM(float pwm){
00080     motDroit.write(pwm); 
00081 }
00082 
00083 /*********************************************************************************************************/
00084 /* FUNCTION NAME: initMoteurs                                                                            */
00085 /* DESCRIPTION  : init les moteurs des mains du petit robot                                              */
00086 /*********************************************************************************************************/
00087 void initMoteurs(void){
00088     motGauche.period(T_MOT);
00089     motDroit.period(T_MOT);
00090     motGauche.write(0.0);
00091     motDroit.write(0.0);
00092 }
00093 
00094 void initialisation_AX12(void) 
00095 {
00096     short vitesse=700;
00097     
00098     BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);  
00099     CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000); 
00100     PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
00101     PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
00102     DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
00103     
00104     CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
00105     CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
00106     EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
00107     
00108     CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
00109     CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
00110     EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
00111     
00112     BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
00113     BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
00114     BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
00115     
00116     BaseBrasCentralPR->Set_Goal_speed(vitesse);  
00117     CoudeBrasCentralPR->Set_Goal_speed(vitesse); 
00118     PinceDBrasCentralPR->Set_Goal_speed(vitesse); 
00119     PinceGBrasCentralPR->Set_Goal_speed(vitesse); 
00120     DoigtBrasCentralPR->Set_Goal_speed(vitesse);
00121     
00122     CrocBrasGauchePR->Set_Goal_speed(vitesse); 
00123     CoudeBrasGauchePR->Set_Goal_speed(vitesse); 
00124     EpauleBrasGauchePR->Set_Goal_speed(vitesse);
00125     
00126     CrocBrasDroitPR->Set_Goal_speed(vitesse); 
00127     CoudeBrasDroitPR->Set_Goal_speed(vitesse); 
00128     EpauleBrasDroitPR->Set_Goal_speed(vitesse);
00129     
00130     BaseBrasCentralPR->Set_Mode(0); 
00131     CoudeBrasCentralPR->Set_Mode(0); 
00132     PinceDBrasCentralPR->Set_Mode(0); 
00133     PinceGBrasCentralPR->Set_Mode(0); 
00134     DoigtBrasCentralPR->Set_Mode(0);
00135     
00136     CrocBrasGauchePR->Set_Mode(0); 
00137     CoudeBrasGauchePR->Set_Mode(0);
00138     EpauleBrasGauchePR->Set_Mode(0);
00139     
00140     CrocBrasDroitPR->Set_Mode(0); 
00141     CoudeBrasDroitPR->Set_Mode(0);
00142     EpauleBrasDroitPR->Set_Mode(0);
00143 } 
00144 
00145 void GetPositionAx12(void) {
00146 
00147     printf("\n\r ");
00148 
00149     printf("BaseC  : %lf \n\r ",   BaseBrasCentralPR->Get_Position()   );
00150     printf("CoudeC : %lf \n\r ",   CoudeBrasCentralPR->Get_Position()  );
00151     printf("PinceCD : %lf \n\r ",   PinceDBrasCentralPR->Get_Position());   
00152     printf("PinceCG : %lf \n\r ",   PinceGBrasCentralPR->Get_Position()); 
00153     printf("DoigtC : %lf \n\r ",   DoigtBrasCentralPR->Get_Position()  );    
00154     
00155     printf("EpauleG : %lf \n\r ",   EpauleBrasGauchePR->Get_Position());   
00156     printf("CoudeG : %lf \n\r ",   CoudeBrasGauchePR->Get_Position() ); 
00157     printf("CrocG : %lf \n\r ",   CrocBrasGauchePR->Get_Position()   ); 
00158     
00159     printf("EpauleD : %lf \n\r ",   EpauleBrasDroitPR->Get_Position());   
00160     printf("CoudeD : %lf \n\r ",   CoudeBrasDroitPR->Get_Position() ); 
00161     printf("CrocD : %lf \n\r ",   CrocBrasDroitPR->Get_Position()   ); 
00162 }
00163 
00164 
00165 /****************************************************************************************/
00166 /* FUNCTION NAME: Automate_ax12                                                         */
00167 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00168 /****************************************************************************************/
00169 void AX12_automate(unsigned char etat_ax12){
00170     
00171     unsigned short speed;
00172     unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
00173                  GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
00174                  GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
00175         
00176     speed = 1000;    
00177         
00178     switch(etat_ax12){
00179                
00180         case AX12_PINCE_CENTRALE_POSITION_INITIALE :
00181             wait_ms(TIME);
00182             speed = 511;
00183             GoalPosDoigt=1150;
00184             GoalPosBase=1490;
00185             GoalPosCoude=1470;
00186             GoalPosPinceG=1090;
00187             GoalPosPinceD=1930;
00188             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00189             break;
00190             
00191         case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
00192             wait_ms(TIME);
00193             GoalPosDoigt=90;
00194             GoalPosBase=170;
00195             GoalPosCoude=1000;
00196             GoalPosPinceG=1090;
00197             GoalPosPinceD=1930;
00198             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00199                
00200             break;    
00201             
00202         case AX12_PINCE_CENTRALE_PRISE_MODULE :
00203             wait_ms(TIME);
00204             GoalPosDoigt=90;
00205             GoalPosBase=170;
00206             GoalPosCoude=1000;
00207             GoalPosPinceG=500;
00208             GoalPosPinceD=2500;
00209             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00210                
00211             break;  
00212             
00213         case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
00214             wait_ms(TIME);
00215             GoalPosDoigt=90;
00216             GoalPosBase=1300;
00217             GoalPosCoude=700;
00218             GoalPosPinceG=500;
00219             GoalPosPinceD=2500;
00220             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00221             
00222             wait(TIME*5);
00223             GoalPosDoigt=90;
00224             GoalPosBase=1450;//1050;
00225             GoalPosCoude=700;//1528;
00226             GoalPosPinceG=500;
00227             GoalPosPinceD=2500;
00228             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00229            
00230             wait(TIME*5);
00231             GoalPosDoigt=90;
00232             GoalPosBase=1450;//1050;
00233             GoalPosCoude=1250;//1528;
00234             GoalPosPinceG=1090;
00235             GoalPosPinceD=1930;
00236             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00237               
00238             break;  
00239         
00240         case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
00241             wait_ms(TIME);
00242             GoalPosDoigt=90;
00243             GoalPosBase=1000;
00244             GoalPosCoude=443;
00245             GoalPosPinceG=500;
00246             GoalPosPinceD=2500;
00247             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00248                
00249             break;
00250         
00251         case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
00252             wait_ms(TIME);
00253             GoalPosDoigt=90;
00254             GoalPosBase=639;
00255             GoalPosCoude=557;
00256             GoalPosPinceG=500;
00257             GoalPosPinceD=2500;
00258             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00259 
00260             wait(TIME*10);
00261             GoalPosDoigt=90;
00262             GoalPosBase=400;
00263             GoalPosCoude=400;
00264             GoalPosPinceG=500;
00265             GoalPosPinceD=2500;
00266             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00267     
00268                
00269             break;
00270             
00271         case AX12_PINCE_CENTRALE_DEPOSER :
00272             //DEPOSER
00273             wait_ms(TIME);
00274             GoalPosDoigt=90;
00275             GoalPosBase=440;
00276             GoalPosCoude=440;
00277             GoalPosPinceG=1090;
00278             GoalPosPinceD=1930;
00279             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00280 
00281                
00282             break;
00283         
00284         case AX12_PINCE_CENTRALE_DEPOT_HAUT :
00285             wait(TIME*5);
00286             GoalPosDoigt=90;
00287             GoalPosBase=1050;
00288             GoalPosCoude=1528;
00289             GoalPosPinceG=1090;
00290             GoalPosPinceD=1930;
00291             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00292             
00293             wait(TIME*10);
00294             GoalPosDoigt=90;
00295             GoalPosBase=1050;
00296             GoalPosCoude=1528;
00297             GoalPosPinceG=500;
00298             GoalPosPinceD=2500;
00299             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00300             
00301             wait(TIME*10);
00302             GoalPosDoigt=90;
00303             GoalPosBase=1100;
00304             GoalPosCoude=700;
00305             GoalPosPinceG=500;
00306             GoalPosPinceD=2500;
00307             mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
00308             
00309                 
00310             break;  
00311             
00312         case AX12_GAUCHE_CROC_OUVERT :
00313             wait_ms(TIME);
00314             CrocBrasGauchePR->Set_Secure_Goal(110);
00315             break;
00316             
00317         case AX12_GAUCHE_CROC_FERME :
00318             wait_ms(TIME);
00319             CrocBrasGauchePR->Set_Secure_Goal(147);
00320             break;     
00321             
00322         case AX12_GAUCHE_CROC_INITIALE :
00323             wait_ms(TIME);
00324             CrocBrasGauchePR->Set_Secure_Goal(232);
00325             break;         
00326             
00327         case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
00328             wait_ms(TIME);
00329             speed = 511;
00330             GoalPosCoudeTournanteG=1450;
00331             GoalPosEpauleTournanteG=600;
00332             mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
00333 
00334                
00335             break;
00336             
00337         case AX12_TOURNANTE_GAUCHE_PREPARATION :
00338             wait_ms(TIME);
00339             speed = 511;
00340             GoalPosCoudeTournanteG=930;
00341             GoalPosEpauleTournanteG=1962;
00342             mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
00343 
00344                
00345             break;
00346             
00347         case AX12_TOURNANTE_GAUCHE_MODULE :
00348             wait_ms(TIME);
00349             speed = 511;
00350             GoalPosCoudeTournanteG=894;
00351             GoalPosEpauleTournanteG=2200;
00352             mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
00353 
00354                
00355             break;
00356             
00357         case AX12_DROIT_CROC_OUVERT :
00358             wait_ms(TIME);
00359             CrocBrasDroitPR->Set_Secure_Goal(189);
00360                
00361             break;
00362             
00363         case AX12_DROIT_CROC_FERME :
00364             wait_ms(TIME);
00365             CrocBrasDroitPR->Set_Secure_Goal(149);
00366                
00367             break;    
00368             
00369         case AX12_DROIT_CROC_INITIALE :
00370             wait_ms(TIME);
00371             CrocBrasDroitPR->Set_Secure_Goal(67);
00372             break;     
00373             
00374         case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
00375             wait_ms(TIME);
00376             speed = 511;
00377             GoalPosCoudeTournanteD=1610;
00378             GoalPosEpauleTournanteD=2337;
00379             mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
00380 
00381                
00382             break;
00383             
00384         case AX12_TOURNANTE_DROIT_PREPARATION :
00385             wait_ms(TIME);
00386             speed = 511;
00387             GoalPosCoudeTournanteD=930;
00388             GoalPosEpauleTournanteD=1962;
00389             mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
00390 
00391                
00392             break;
00393             
00394         case AX12_TOURNANTE_DROIT_MODULE :
00395             wait_ms(TIME);
00396             speed = 511;
00397             GoalPosCoudeTournanteD=894;
00398             GoalPosEpauleTournanteD=2200;
00399             mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
00400 
00401                
00402             break;
00403         
00404         case AX12_DEFAUT :
00405             break;
00406             
00407         case AX12_POSITION :
00408             GetPositionAx12();
00409             break;
00410     }
00411 }
00412 
00413 void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
00414                               unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
00415                               unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
00416                               unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
00417                               unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
00418 {
00419     char TabBrasCentralPR[25];
00420     unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
00421     Timer  timeOut;
00422     
00423           
00424     GPosition1_1=((unsigned long)GPosition1*341/1000);
00425     GPosition2_1=((unsigned long)GPosition2*341/1000);
00426     GPosition3_1=((unsigned long)GPosition3*341/1000);
00427     GPosition4_1=((unsigned long)GPosition4*341/1000);
00428     GPosition5_1=((unsigned long)GPosition5*341/1000);
00429    
00430     TabBrasCentralPR[0] = ID1;
00431     TabBrasCentralPR[1] = GPosition1_1;
00432     TabBrasCentralPR[2] = GPosition1_1>>8; 
00433     TabBrasCentralPR[3] = GSpeed1; 
00434     TabBrasCentralPR[4] = GSpeed1>>8;               
00435              
00436     TabBrasCentralPR[5] = ID2;
00437     TabBrasCentralPR[6] = GPosition2_1;
00438     TabBrasCentralPR[7] = GPosition2_1>>8;
00439     TabBrasCentralPR[8] = GSpeed2;
00440     TabBrasCentralPR[9] = GSpeed2>>8;
00441    
00442     TabBrasCentralPR[10] = ID3;
00443     TabBrasCentralPR[11] = GPosition3_1;
00444     TabBrasCentralPR[12] = GPosition3_1>>8;
00445     TabBrasCentralPR[13] = GSpeed3;
00446     TabBrasCentralPR[14] = GSpeed3>>8  ;  
00447     
00448     TabBrasCentralPR[15] = ID4;
00449     TabBrasCentralPR[16] = GPosition4_1;
00450     TabBrasCentralPR[17] = GPosition4_1>>8;
00451     TabBrasCentralPR[18] = GSpeed4;
00452     TabBrasCentralPR[19] = GSpeed4>>8  ; 
00453     
00454     TabBrasCentralPR[20] = ID5;
00455     TabBrasCentralPR[21] = GPosition5_1;
00456     TabBrasCentralPR[22] = GPosition5_1>>8;
00457     TabBrasCentralPR[23] = GSpeed5;
00458     TabBrasCentralPR[24] = GSpeed5>>8  ; 
00459     
00460                        
00461     BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00462     wait(TIME);
00463     
00464     timeOut.start();
00465     while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() < 100)) {
00466         BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00467         wait(TIME);
00468         
00469     } 
00470     
00471     timeOut.reset();    
00472     while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*90/100) || (timeOut.read_ms() < 100)) {
00473         BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00474         wait(TIME);
00475     } 
00476     
00477     timeOut.reset();
00478     while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*110/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*90/100) || (timeOut.read_ms() < 100)) {
00479         BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00480         wait(TIME);
00481     } 
00482     
00483     timeOut.reset();
00484     while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*110/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*90/100) || (timeOut.read_ms() < 100)) {
00485         BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00486         wait(TIME);
00487     } 
00488     
00489     timeOut.reset();
00490     while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*110/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*90/100) || (timeOut.read_ms() < 100)) {
00491         BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
00492         wait(TIME);
00493     }
00494     
00495    
00496 }
00497 
00498 
00499 void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
00500                               unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
00501 {
00502     char TabBrasGauchePR[10];
00503     unsigned short GPosition1_1, GPosition2_1;
00504     Timer  timeOut;
00505       
00506     GPosition1_1=((unsigned long)GPosition1*341/1000);
00507     GPosition2_1=((unsigned long)GPosition2*341/1000);
00508    
00509     TabBrasGauchePR[0] = ID1;
00510     TabBrasGauchePR[1] = GPosition1_1;
00511     TabBrasGauchePR[2] = GPosition1_1>>8; 
00512     TabBrasGauchePR[3] = GSpeed1; 
00513     TabBrasGauchePR[4] = GSpeed1>>8;               
00514              
00515     TabBrasGauchePR[5] = ID2;
00516     TabBrasGauchePR[6] = GPosition2_1;
00517     TabBrasGauchePR[7] = GPosition2_1>>8;
00518     TabBrasGauchePR[8] = GSpeed2;
00519     TabBrasGauchePR[9] = GSpeed2>>8;
00520     
00521                         
00522     BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
00523     wait(TIME);
00524     
00525     timeOut.start();
00526     while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*90/100)  || (timeOut.read_ms() < 100)) {
00527         BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
00528         wait(TIME);
00529     } 
00530     timeOut.reset();
00531     
00532     while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*90/100)  || (timeOut.read_ms() < 100)) {
00533         BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
00534         wait(TIME);
00535     }  
00536     timeOut.reset();
00537 }
00538 
00539 void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
00540                     unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
00541 {
00542     char TabBrasDroitPR[10];
00543     unsigned short GPosition1_1, GPosition2_1;
00544     Timer  timeOut;
00545       
00546     GPosition1_1=((unsigned long)GPosition1*341/1000);
00547     GPosition2_1=((unsigned long)GPosition2*341/1000);
00548    
00549     TabBrasDroitPR[0] = ID1;
00550     TabBrasDroitPR[1] = GPosition1_1;
00551     TabBrasDroitPR[2] = GPosition1_1>>8; 
00552     TabBrasDroitPR[3] = GSpeed1; 
00553     TabBrasDroitPR[4] = GSpeed1>>8;               
00554              
00555     TabBrasDroitPR[5] = ID2;
00556     TabBrasDroitPR[6] = GPosition2_1;
00557     TabBrasDroitPR[7] = GPosition2_1>>8;
00558     TabBrasDroitPR[8] = GSpeed2;
00559     TabBrasDroitPR[9] = GSpeed2>>8;
00560     
00561                         
00562     BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
00563     wait(TIME);
00564     
00565     timeOut.start();
00566     while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*90/100)  || (timeOut.read_ms() < 100)) {
00567         BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
00568         wait(TIME);
00569     } 
00570     timeOut.reset();
00571     
00572     while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*90/100)  || (timeOut.read_ms() < 100)) {
00573         BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
00574         wait(TIME);
00575     } 
00576     timeOut.reset();
00577 }
00578