code de start qui marche a la fin du premier match, base pour la suite
Fork of CRAC-Strat_2017_homologation_petit_rob by
Diff: peripheriques/actionneurs.cpp
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 19:b4b91258c275
--- a/peripheriques/actionneurs.cpp Fri May 19 17:14:07 2017 +0000 +++ b/peripheriques/actionneurs.cpp Mon May 22 15:01:49 2017 +0000 @@ -1,434 +1,64 @@ #include "peripheriques.h" -/* contient les fonctiones qui servent à utiliser les AX12 et les moteurs sur le petit robot*/ +/* 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); - -// AX12 partie centrale du petit robot -AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre; -AX12 *partieDoigtCentre; -AX12 *multipleCentre; -// AX12 partie gauche du robot -AX12 *partieBasseGauche, *partieMainGauche; -AX12 *partiePorteGauche; -AX12 *multipleGauche; - -// AX12 partie droite du robot -AX12 *partieBasseDroite, *partieMainDroite; -AX12 *partiePorteDroite; -AX12 *multipleDroite; - -Serial pc(USBTX, USBRX); - Timer t; -Ticker flipper; -unsigned char action_precedente = 0; - - -////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// -static char TAB1[20]= {0x05,0x15, 0x02, 0xFF, 0x00, ///Position initiale - 0x06,0x00, 0x02, 0xFF, 0x00, - 0x07,0x90, 0x01, 0xB1, 0x00, - 0x08,0x58, 0x02, 0x79, 0x00}; - -static char TAB2[20]= {0x05,0x50, 0x00, 0xFF, 0x02, ///Preparation prise - 0x06,0x50, 0x01, 0xFF, 0x02, - 0x07,0xF4, 0x01, 0xFF, 0x02, - 0x08,0xF4, 0x01, 0xFF, 0x02}; - -static char TAB3[20]= {0x05,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee) - 0x06,0x50, 0x01, 0xFF, 0x03, - 0x07,0xC5, 0x00, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; -static char TAB4[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air) - 0x06,0x50, 0x01, 0xFF, 0x03, - 0x07,0xC5, 0x00, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; - -static char TAB5[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) - 0x06,0xF4, 0x01, 0xFF, 0x00, - 0x07,0xC5, 0x00, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; - -static char TAB6[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) - 0x06,0xF4, 0x01, 0xFF, 0x03, - 0x07,0x4d, 0x01, 0xFF, 0x03, - 0x08,0x9a, 0x02, 0xFF, 0x03}; - -static char TAB7[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air) - 0x06,0xB0, 0x00, 0xFF, 0x03, - 0x07,0xC5, 0x00, 0xFF, 0x00, - 0x08,0x4D, 0x03, 0xFF, 0x00}; - -static char TAB8[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas - 0x06,0xF4, 0x01, 0xFF, 0x03, - 0x07,0xC5, 0x00, 0xFF, 0x00, - 0x08,0x4D, 0x03, 0xFF, 0x00}; - -static char TAB9[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Deposer - 0x06,0xF4, 0x01, 0xFF, 0x03, - 0x07,0xD0, 0x00, 0xFF, 0x00, - 0x08,0x35, 0x03, 0xFF, 0x00}; - -static char TAB10[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige) - 0x06,0x00, 0x01, 0xFF, 0x00, - 0x07,0xC5, 0x00, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; - -static char TAB11[20]= {0x05,0x60, 0x00, 0xFF, 0x03, ///Pousser_module - 0x06,0xF4, 0x01, 0xFF, 0x03, - 0x07,0xC5, 0x00, 0xFF, 0x00, - 0x08,0x4D, 0x03, 0xFF, 0x00}; - -static char TAB12[20]= {0x05,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale - 0x06,0x00, 0x02, 0xFF, 0x03, - 0x07,0xF4, 0x01, 0xFF, 0x03, - 0x08,0xF4, 0x01, 0xFF, 0x03}; - -static char TAB13[20]= {0x05,0xF4, 0x00, 0xFF, 0x03, ///Deposer - 0x06,0xA0, 0x02, 0xFF, 0x03, - 0x07,0xD0, 0x00, 0xFF, 0x00, - 0x08,0x35, 0x03, 0xFF, 0x00}; - -static char TAB14[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) - 0x06,0x42, 0x00, 0xFF, 0x03, - 0x07,0x4d, 0x01, 0xFF, 0x03, - 0x08,0x9a, 0x02, 0xFF, 0x03}; +unsigned char action_precedente = 0; + + /* DECLARATION VARIABLES */ -static char TAB15[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige) - 0x06,0x42, 0x00, 0xFF, 0x00, - 0x07,0xC5, 0x00, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; - -static char TAB16[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte) - 0x06,0x42, 0x00, 0xFF, 0x00, - 0x07,0x2c, 0x01, 0xFF, 0x03, - 0x08,0x4D, 0x03, 0xFF, 0x03}; -////////////////////// TABLEAU BRAS GAUCHE /////////////////////////// -static char TAB21[10]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale - 0x02,0xF4, 0x01, 0xFF, 0x03}; - -static char TAB22[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner - 0x02,0x40, 0x03, 0xFF, 0x03}; - -static char TAB23[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module - 0x02,0xE5, 0x02, 0xFF, 0x03}; - - - - - /* ANGLE */ - -/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 - 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02 - 30° = 0x64, 0x00 | 130°= 0xB1, 0x01 - 40° = 0x85, 0x00 | 140°= 0xD2, 0x01 - 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01 - 60° = 0xC8, 0x00 | 160°= 0x15, 0x02 - 70° = 0xE9, 0x00 | 170°= 0x36, 0x02 - 80° = 0x0A, 0x01 | 180°= 0x58, 0x02 - 90° = 0x2C, 0x01 | 190°= 0x79, 0x02 - 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */ - - /* NUMERO AX12 */ - -/* 0 = 0x00 | 9 = 0x09 | 18 = 0x12 - 1 = 0x01 | 10 = 0x0A - 2 = 0x02 | 11 = 0x0B - 3 = 0x03 | 12 = 0x0C - 4 = 0x04 | 13 = 0x0D - 5 = 0x05 | 14 = 0x0E - 6 = 0x06 | 15 = 0x0F - 7 = 0x07 | 16 = 0x10 - 8 = 0x08 | 17 = 0x11 */ - - - - - -void declarationAX12(void){ - //Pince centrale - partieBasseCentre = new AX12(p9, p10, 5, 1000000); - partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000); - partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000); - partiePoignetCentre = new AX12(p9, p10, 6, 1000000); - //doigt central - partieDoigtCentre = new AX12(p9, p10, 4, 1000000); - - multipleCentre = new AX12(p9,p10,0xFE,1000000); - - //Bras de gauche - partieBasseGauche = new AX12(p13, p14, 1, 1000000); - partieMainGauche = new AX12(p13, p14, 2, 1000000); - //Porte gauche - partiePorteGauche = new AX12(p13, p14, 3, 1000000); - - multipleGauche = new AX12(p13,p14,0xFE,1000000); - - //Bras de droite - partieBasseDroite = new AX12(p28, p27, 1, 1000000); - partieMainDroite = new AX12(p28, p27, 2, 1000000); - //Porte droite - partiePorteDroite = new AX12(p28, p27, 3, 1000000); +extern unsigned char FlagAx12; - multipleDroite = new AX12(p28,p27,0xFE,1000000); - - - - - - } - - -void initAX12() -{ - declarationAX12(); - - // init des AX12 de la partie CENTRALE du robot - partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023) - - partieMainGauche-> Set_Goal_speed(VITESSE); - partieMainDroite-> Set_Goal_speed(VITESSE); - partiePoignetCentre-> Set_Goal_speed(VITESSE); - partieDoigtCentre-> Set_Goal_speed(VITESSE); - - partieBasseCentre-> Set_Mode(0); - partieMainGauche-> Set_Mode(0); - partieMainDroite-> Set_Mode(0); - partiePoignetCentre-> Set_Mode(0); - partieDoigtCentre-> Set_Mode(0); - - // init des AX12 de ela partie GAUCHE du robot - partieBasseGauche-> Set_Goal_speed(VITESSE); - partieMainGauche-> Set_Goal_speed(VITESSE); - partiePorteGauche-> Set_Goal_speed(VITESSE); - - partieBasseGauche-> Set_Mode(0); - partieMainGauche-> Set_Mode(0); - partiePorteGauche-> Set_Mode(0); - - // init des AX12 de ela partie DROITE du robot - partieBasseDroite-> Set_Goal_speed(VITESSE); - partieMainDroite-> Set_Goal_speed(VITESSE); - //partiePorteDroite-> Set_Goal_speed(VITESSE); - - partieBasseDroite-> Set_Mode(0); - partieMainDroite-> Set_Mode(0); - //partiePorteDroite-> Set_Mode(0); - - -} - - -/****************************************************************************************/ -/* FUNCTION NAME: Initialisation_position */ -/* DESCRIPTION : Fonction qui place les bras en position verticale */ -/****************************************************************************************/ -void Initialisation_position(void){ - multipleCentre->multiple_goal_and_speed(5,TAB12); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB1); - //wait(TIME); - multipleGauche->multiple_goal_and_speed(3,TAB21); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_prise */ -/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ -/****************************************************************************************/ -void Preparation_prise(void){ - /*if (action_precedente == 0){ - multipleCentre->multiple_goal_and_speed(5,TAB12); - wait(TIME); - action_precedente = 1; - }*/ - multipleCentre->multiple_goal_and_speed(5,TAB2); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Stockage_haut */ -/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ -/****************************************************************************************/ -void Stockage_haut(void){ - multipleCentre->multiple_goal_and_speed(5,TAB3); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB4); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB5); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB6); - wait(TIME); -} +extern DigitalOut led2; +extern Serial pc; +extern Timer t; +extern void GetPositionAx12(void); -/****************************************************************************************/ -/* FUNCTION NAME: Stockage_bas */ -/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ -/****************************************************************************************/ -void Stockage_bas(void){ - multipleCentre->multiple_goal_and_speed(5,TAB3); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB7); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Deposer */ -/* DESCRIPTION : Fonction qui permet de déposer le module */ -/****************************************************************************************/ -void Deposer(void){ - multipleCentre->multiple_goal_and_speed(5,TAB9); - wait(TIME/5); - multipleCentre->multiple_goal_and_speed(5,TAB8); - wait(TIME/5); - //multipleCentre->multiple_goal_and_speed(5,TAB13); - wait(TIME/5); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_depot_bas */ -/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ -/****************************************************************************************/ -void Preparation_depot_bas(void){ - multipleCentre->multiple_goal_and_speed(5,TAB8); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_depot_haut */ -/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ -/****************************************************************************************/ -void Preparation_depot_haut(void){ - multipleCentre->multiple_goal_and_speed(5,TAB14); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB16); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB15); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB10); - wait(TIME); - multipleCentre->multiple_goal_and_speed(5,TAB8); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Pousser_module */ -/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ -/****************************************************************************************/ -void Pousser_module(void){ - multipleCentre->multiple_goal_and_speed(5,TAB11); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Initialisation_gauche */ -/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ -/****************************************************************************************/ -void Initialisation_gauche(void){ - //trois_myAX12_2->Set_Secure_Goal(235); - multipleGauche->multiple_goal_and_speed(5,TAB22); - wait(TIME); - multipleGauche->multiple_goal_and_speed(5,TAB21); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_prise_gauche */ -/* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */ -/****************************************************************************************/ -void Preparation_prise_gauche(void){ - //trois_myAX12_2->Set_Secure_Goal(120); - //multiplePorteGauche->multiple_goal_and_speed(5,TAB); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Prendre_module_gauche */ -/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ -/****************************************************************************************/ -void Prendre_module_gauche(void){ - //trois_myAX12_2->Set_Secure_Goal(160); - //multiplePorteGauche->multiple_goal_and_speed(5,TAB); -} - -/***************************************************************************************/ -/* FUNCTION NAME: RangerBrasGauche */ -/* DESCRIPTION : Fonction range le bras gauche */ -/****************************************************************************************/ -void RangerBrasGauche(void){ - multipleGauche->multiple_goal_and_speed(3,TAB21); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_module_gauche */ -/* DESCRIPTION : Fonction qui prepare la tournante */ -/****************************************************************************************/ -void Preparation_module_gauche(void){ - multipleGauche->multiple_goal_and_speed(5,TAB22); - wait(TIME); -} +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){ - //#define POSITION_TOURNER 0xE5 - //#define POSITION_MAX 0xFF - /*int position = POSITION_TOURNER; - TAB23[6] = POSITION_TOURNER;*/ - - //multipleGauche->multiple_goal_and_speed(5,TAB23); - wait(TIME); - /*while((dataPressionGauche == false)&&(position < POSITION_MAX)){ - position++; - TAB23[6] = position; - multipleGauche->multiple_goal_and_speed(5,TAB23); - }*/ - while(dataCouleurGauche() == false){ printf("ici"); moteurDroitPWM(0.2); } moteurDroitPWM(0); } - - -void getPosiotionCentrale(void){ - pc.printf("\n\r * pince avant * \n\r"); - - pc.printf("base : %lf \n\r ", partieBasseCentre->Get_Position() ); - pc.printf("coude : %lf \n\r ", partiePoignetCentre->Get_Position() ); - pc.printf("doigtG: %lf \n\r ", partieMainGaucheCentre->Get_Position() ); - pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() ); - - wait(0.2); -} - -void getPosiotionGauche(void){ - pc.printf("\n\r * bras gauche *\n\r"); - - pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position()); - pc.printf("main : %lf \n\r ",partieMainGauche->Get_Position() ); - pc.printf("porte : %lf \n\r ",partiePorteGauche->Get_Position() ); - - wait(0.2); -} - -void getPosiotionDroite(void){ - pc.printf("\n\r * bras droit *\n\r"); - - pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position()); - pc.printf("main : %lf \n\r ",partieMainDroite->Get_Position() ); - pc.printf("porte : %lf \n\r ",partiePorteDroite->Get_Position() ); - - - wait(0.2); -} - - +/****************************************************************************************/ +/* 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); +} /********************************************************************************************************/ @@ -459,4 +89,487 @@ motDroit.period(T_MOT); motGauche.write(0.0); motDroit.write(0.0); -} \ No newline at end of file +} + +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(200); + + break; + + case AX12_GAUCHE_CROC_FERME : + wait_ms(TIME); + CrocBrasGauchePR->Set_Secure_Goal(240); + + break; + + case AX12_GAUCHE_CROC_INITIALE : + wait_ms(TIME); + CrocBrasGauchePR->Set_Secure_Goal(300); + + 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(106); + + break; + + case AX12_DROIT_CROC_FERME : + wait_ms(TIME); + CrocBrasDroitPR->Set_Secure_Goal(55); + + break; + + case AX12_DROIT_CROC_INITIALE : + wait_ms(TIME); + CrocBrasDroitPR->Set_Secure_Goal(0); + + 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; + + 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); + + while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*95/100)) { + BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ; + wait(TIME*5); + } + + while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*95/100)) { + BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ; + wait(TIME*5); + } +} + +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; + + 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); + + while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*95/100)) { + BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ; + wait(TIME*5); + } + + while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*95/100)) { + BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ; + wait(TIME*5); + } +} +