CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
Diff: actionneurs/actionneurs.cpp
- Revision:
- 7:ad4a19e26b84
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/actionneurs/actionneurs.cpp Fri May 12 14:35:09 2017 +0000
@@ -0,0 +1,376 @@
+#include"AX12.h"
+#include"mbed.h"
+#include "global.h"
+
+#define TIME 0.8
+#define VITESSE 700
+DigitalOut led(LED1);
+
+// AX12 partie centrale du petit robot
+AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral;
+AX12 *partieDoigtCentre, *multipleDoigtCentre;
+// AX12 partie gauche du robot
+AX12 *partieBasseGauche, *partieMainGauche, *multipleBrasGauche;
+AX12 *partiePorteGauche, *multiplePorteGauche;
+// AX12 partie droite du robot
+AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite;
+AX12 *partiePorteDroite, *multiplePorteDroite;
+
+Serial pc(USBTX, USBRX);
+
+Timer t;
+Ticker flipper;
+unsigned char action_precedente = 0;
+////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
+static char TAB1[25]= {0x01,0x15, 0x02, 0xFF, 0x00, ///Position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x00,
+ 0x07,0x90, 0x01, 0xB1, 0x00,
+ 0x08,0x58, 0x02, 0x79, 0x00};
+
+static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x02, ///Preparation prise
+ 0x10,0x50, 0x01, 0xFF, 0x02,
+ 0x07,0xF4, 0x01, 0xFF, 0x02,
+ 0x08,0xF4, 0x01, 0xFF, 0x02};
+
+static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0xF4, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB6[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0x4d, 0x01, 0xFF, 0x03,
+ 0x08,0x9a, 0x02, 0xFF, 0x03};
+
+static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air)
+ 0x10,0xB0, 0x00, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0x00, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xA0, 0x02, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+static char TAB14[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x10,0x42, 0x00, 0xFF, 0x03,
+ 0x07,0x4d, 0x01, 0xFF, 0x03,
+ 0x08,0x9a, 0x02, 0xFF, 0x03};
+
+static char TAB15[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0x42, 0x00, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB16[25]= {0x01,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x10,0x42, 0x00, 0xFF, 0x00,
+ 0x07,0x2c, 0x01, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
+static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale
+ 0x02,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner
+ 0x02,0x40, 0x03, 0xFF, 0x03};
+
+static char TAB23[25]= {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
+ //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre, *multipleBrasCentral;
+ partieBasseCentre = new AX12(p9, p10, 1, 1000000);
+ partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000);
+ partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000);
+ partiePoignetCentre = new AX12(p9, p10, 16, 1000000);
+ multipleBrasCentral = new AX12(p9,p10,0xFE,1000000);
+
+ //Bras de gauche
+ partieBasseGauche = new AX12(p13, p14, 1, 1000000);
+ partieMainGauche = new AX12(p13, p14, 2, 1000000);
+ multipleBrasGauche = new AX12(p13,p14,0xFE,1000000);
+ //Porte gauche
+ partiePorteGauche = new AX12(p13, p14, 3, 1000000);
+ multiplePorteGauche = new AX12(p13, p14, 0xFE, 1000000);
+
+ //Bras de droite
+ partieBasseDroite = new AX12(p28, p27, 1, 1000000);
+ partieMainDroite = new AX12(p28, p27, 2, 1000000);
+ multipleBrasDroite = new AX12(p28,p27,0xFE,1000000);
+ //Porte droite
+ partiePorteDroite = new AX12(p28, p27, 3, 1000000);
+ multiplePorteDroite = new AX12(p28, p27, 0xFE, 1000000);
+ }
+
+
+void Init_AX12() // Initialisation des différents paramétres
+{
+ partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023)
+ partieMainGauche-> Set_Goal_speed(VITESSE);
+ partieMainDroite-> Set_Goal_speed(VITESSE);
+ partiePoignetCentre-> Set_Goal_speed(VITESSE);
+
+ partieBasseCentre-> Set_Mode(0);
+ partieMainGauche-> Set_Mode(0);
+ partieMainDroite-> Set_Mode(0);
+ partiePoignetCentre-> Set_Mode(0);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB1);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(void){
+ /*if (action_precedente == 0){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ action_precedente = 1;
+ }*/
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB2);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(void){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB4);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
+/****************************************************************************************/
+void Stockage_bas(void){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB7);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer le module */
+/****************************************************************************************/
+void Deposer(void){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB9);
+ wait(TIME/5);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB8);
+ wait(TIME/5);
+ //multipleBrasCentral->multiple_goal_and_speed(4,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){
+ multipleBrasCentral->multiple_goal_and_speed(4,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){
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB14);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB16);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB15);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,TAB10);
+ wait(TIME);
+ multipleBrasCentral->multiple_goal_and_speed(4,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){
+ multipleBrasCentral->multiple_goal_and_speed(4,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);
+ multipleBrasGauche->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+ multipleBrasGauche->multiple_goal_and_speed(4,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);
+}
+
+/****************************************************************************************/
+/* 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);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparatio_module_gauche */
+/* DESCRIPTION : Fonction qui prepare le tournante */
+/****************************************************************************************/
+void Preparation_module_gauche(void){
+ multipleBrasGauche->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_module_gauche */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
+/****************************************************************************************/
+void Tourner_module_gauche(void){
+ multipleBrasGauche->multiple_goal_and_speed(4,TAB23);
+ wait(TIME);
+}
+
+void getPosiotionCentrale(void){
+ //AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre,
+ led = 1;
+ 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);
+ led = 0;
+
+}
+
+void getPosiotionGauche(void){
+ //AX12 *partieBasseGauche, *partieMainGauche
+ led = 1;
+ 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);
+ led = 0;
+
+}
+
+void getPosiotionDroite(void){
+ //AX12 *partieBasseDroite, *partieMainDroite, *multipleBrasDroite;
+ led = 1;
+ 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);
+ led = 0;
+
+}