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