code petit robot pour homologation
Fork of CRAC-Strat_2017_V2 by
Diff: peripheriques/actionneurs.cpp
- Revision:
- 16:7321fb3bb396
- Child:
- 18:cc5fec34ed9c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/peripheriques/actionneurs.cpp Fri May 19 17:13:46 2017 +0000 @@ -0,0 +1,462 @@ +#include "peripheriques.h" +/* contient les fonctiones qui servent à utiliser les AX12 et les moteurs sur le petit robot*/ + +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}; + +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); + + 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); +} + +/****************************************************************************************/ +/* 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); +} + +/****************************************************************************************/ +/* 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: moteurGauchePWM */ +/* DESCRIPTION : bouge le moteur gauche */ +/*********************************************************************************************************/ +void moteurGauchePWM(float pwm){ + motGauche.write(pwm); +} + +/*********************************************************************************************************/ +/* FUNCTION NAME: moteurDroitPWM */ +/* DESCRIPTION : bouge le moteur gauche */ +/*********************************************************************************************************/ +void moteurDroitPWM(float pwm){ + motDroit.write(pwm); +} + +/*********************************************************************************************************/ +/* FUNCTION NAME: initMoteurs */ +/* DESCRIPTION : init les moteurs des mains du petit robot */ +/*********************************************************************************************************/ +void initMoteurs(void){ + motGauche.period(T_MOT); + motDroit.period(T_MOT); + motGauche.write(0.0); + motDroit.write(0.0); +} \ No newline at end of file