CRAC Team
code petit robot pour homologation
Fork of
CRAC-Strat_2017_V2
by 
CRAC Team
Diff: peripheriques/actionneurs.cpp
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 19:b4b91258c275
diff -r d1594579eec6 -r cc5fec34ed9c peripheriques/actionneurs.cpp
--- 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);
+ }
+}
+