CRAC Team
homologation gros robot et test avec les ack de la carte a tout faire
Fork of
CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT
by 
CRAC Team
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