mouvement AX12 petit robot version 2
Fork of command_AX12_petit_robot_v2 by
actionneurs/actionneurs.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-12
- Revision:
- 7:ad4a19e26b84
File content as of revision 7:ad4a19e26b84:
#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; }