CRAC Team
/
carte_strategie_2019
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: peripheriques/actionneurs.cpp
- Revision:
- 29:41e02746041d
- Parent:
- 28:acd18776ed2d
- Child:
- 30:a1e37af4bbde
--- a/peripheriques/actionneurs.cpp Sat May 27 05:40:26 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,594 +0,0 @@
-#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(TIME);
- GoalPosDoigt=150;
- 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);
-
- wait_ms(TIME*5);
- GoalPosDoigt=150; //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=150; //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);
- speed = 700;
- GoalPosDoigt=150; //90
- GoalPosBase=1200;//1050; //1450
- GoalPosCoude=1320;//1528;//1250
- 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);
- speed = 700;
- GoalPosDoigt=150; //90
- GoalPosBase=1200;//1050; //1450
- GoalPosCoude=1320;//1528;//1250
- 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();
-}
-