code petit robot pour homologation
Fork of CRAC-Strat_2017_V2 by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 21:45:50 by 1.7.2