code ax12 petit robot 12/05/2017
Fork of command_AX12_petit_robot_V3 by
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; + +}