mouvement AX12 petit robot version 2
Fork of command_AX12_petit_robot_v2 by
Revision 7:ad4a19e26b84, committed 2017-05-12
- Comitter:
- ClementBreteau
- Date:
- Fri May 12 14:35:09 2017 +0000
- Parent:
- 6:c239c7abd7fe
- Commit message:
- position ax12
Changed in this revision
--- a/AX12/AX12.cpp Thu May 11 14:21:57 2017 +0000 +++ b/AX12/AX12.cpp Fri May 12 14:35:09 2017 +0000 @@ -830,7 +830,7 @@ { #ifdef AX12_DEBUG - printf("\nGetPositionID(%d)",_ID); + printf("\nGetPositionID(%d) \n\r",_ID); #endif char data[2];
--- a/AX12/AX12.h Thu May 11 14:21:57 2017 +0000 +++ b/AX12/AX12.h Fri May 12 14:35:09 2017 +0000 @@ -27,9 +27,9 @@ #include "mbed.h" //#define AX12_WRITE_DEBUG 0 -#define AX12_READ_DEBUG +//#define AX12_READ_DEBUG //#define AX12_TRIGGER_DEBUG 0 -#define AX12_DEBUG 0 +//#define AX12_DEBUG 0 /****** à utiliser pour le debug !! ******/
--- /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; + +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/actionneurs/actionneurs.h Fri May 12 14:35:09 2017 +0000 @@ -0,0 +1,87 @@ + + + /* PROTOTYPES DE FONCTIONS ET POINTEURS */ + +void declarationAX12(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Initialisation_position */ +/* DESCRIPTION : Fonction qui place les bras en position verticale */ +/****************************************************************************************/ +void Initialisation_position(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Preparation_prise */ +/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ +/****************************************************************************************/ +void Preparation_prise(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Stockage_haut */ +/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ +/****************************************************************************************/ +void Stockage_haut(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Stockage_bas */ +/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ +/****************************************************************************************/ +void Stockage_bas(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Deposer */ +/* DESCRIPTION : Fonction qui permet de déposer un module */ +/****************************************************************************************/ +void Deposer(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Preparation_depos_bas */ +/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ +/****************************************************************************************/ +void Preparation_depot_bas(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Preparation_depos_haut */ +/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ +/****************************************************************************************/ +void Preparation_depot_haut(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Pousser_module */ +/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ +/****************************************************************************************/ +void Pousser_module(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Initialisation_gauche */ +/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ +/****************************************************************************************/ +void Initialisation_gauche(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Preparation_prise_gauche */ +/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ +/****************************************************************************************/ +void Preparation_prise_gauche(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Prendre_module_gauche */ +/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ +/****************************************************************************************/ +void Prendre_module_gauche(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Tourner_module_gauche */ +/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ +/****************************************************************************************/ +void Tourner_module_gauche(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Preparatio_module_gauche */ +/* DESCRIPTION : Fonction qui prepare le tournante */ +/****************************************************************************************/ +void Preparation_module_gauche(void); + +void getPosiotionCentrale(void); +void getPosiotionGauche(void); +void getPosiotionDroite(void);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/global.h Fri May 12 14:35:09 2017 +0000 @@ -0,0 +1,6 @@ +/****************************************************************************************/ +/* FUNCTION NAME: SendRawId */ +/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ +/****************************************************************************************/ +void SendRawId (unsigned short id); +
--- a/main.cpp Thu May 11 14:21:57 2017 +0000 +++ b/main.cpp Fri May 12 14:35:09 2017 +0000 @@ -1,5 +1,7 @@ #include "mbed.h" #include "AX12.h" +#include "actionneurs.h" +#include "global.h" #include "ident_crac.h" @@ -13,29 +15,12 @@ #define AX12_POUSSER_MODULE 7 #define AX12_DEFAUT 20 -#define TIME 0.8 -#define SIZE_FIFO 25 - - /* DECLARATION VARIABLES */ -CAN can1(p30,p29); -CANMessage msgRxBuffer[SIZE_FIFO]; -unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN -unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN + +#define SIZE_FIFO 25 + extern "C" void mbed_reset();//Pour pouvoir reset la carte -unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 0; -short vitesse=700; -float angle=0.0; -float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0; -AX12 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale -AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche - - - /* PROTOTYPES DE FONCTIONS ET POINTEURS */ - - - /****************************************************************************************/ /* FUNCTION NAME: canProcessRx */ /* DESCRIPTION : Fonction de traitement des messages CAN */ @@ -65,193 +50,21 @@ /* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */ /****************************************************************************************/ void AX12_automate(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Initialisation_position */ -/* DESCRIPTION : Fonction qui place les bras en position verticale */ -/****************************************************************************************/ -void Initialisation_position(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_prise */ -/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */ -/****************************************************************************************/ -void Preparation_prise(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Stockage_haut */ -/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */ -/****************************************************************************************/ -void Stockage_haut(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Stockage_bas */ -/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */ -/****************************************************************************************/ -void Stockage_bas(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Deposer */ -/* DESCRIPTION : Fonction qui permet de déposer un module */ -/****************************************************************************************/ -void Deposer(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_depos_bas */ -/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */ -/****************************************************************************************/ -void Preparation_depot_bas(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_depos_haut */ -/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */ -/****************************************************************************************/ -void Preparation_depot_haut(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Pousser_module */ -/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */ -/****************************************************************************************/ -void Pousser_module(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Initialisation_gauche */ -/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */ -/****************************************************************************************/ -void Initialisation_gauche(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Preparation_prise_gauche */ -/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ -/****************************************************************************************/ -void Preparation_prise_gauche(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Prendre_module_gauche */ -/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */ -/****************************************************************************************/ -void Prendre_module_gauche(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Tourner_module_gauche */ -/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */ -/****************************************************************************************/ -void Tourner_module_gauche(void); - -/****************************************************************************************/ -/* FUNCTION NAME: Preparatio_module_gauche */ -/* DESCRIPTION : Fonction qui prepare le tournante */ -/****************************************************************************************/ -void Preparation_module_gauche(void); - -Timer t; -Ticker flipper; - -////////////////////// TABLEAU PINCE CENTRALE /////////////////////////// -static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale - 0x10,0x00, 0x02, 0xFF, 0x00, - 0x07,0x00, 0x01, 0xFF, 0x00, - 0x08,0x00, 0x03, 0xFF, 0x00}; + + + /* DECLARATION VARIABLES */ +CAN can1(p30,p29); +CANMessage msgRxBuffer[SIZE_FIFO]; +unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN +unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN -static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise - 0x10,0x50, 0x01, 0xFF, 0x03, - 0x07,0xF4, 0x01, 0xFF, 0x03, - 0x08,0xF4, 0x01, 0xFF, 0x03}; - -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}; +extern "C" void mbed_reset();//Pour pouvoir reset la carte -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,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte) - 0x10,0xF4, 0x01, 0xFF, 0x03, - 0x07,0x00, 0x01, 0xFF, 0x03, - 0x08,0x00, 0x03, 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}; - -////////////////////// 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 */ - - +unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0; +short vitesse=700; +float angle=0.0; +float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0; + /* MAIN */ @@ -260,45 +73,10 @@ can1.frequency(1000000); // fréquence de travail 1Mbit/s can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN - //Pince centrale - un_myAX12 = new AX12(p9, p10, 1, 1000000); - sept_myAX12 = new AX12(p9, p10, 7, 1000000); - huit_myAX12 = new AX12(p9, p10, 8, 1000000); - seize_myAX12 = new AX12(p9, p10, 16, 1000000); - multiple_myAX12 = new AX12(p9,p10,0xFE,1000000); - - //Bras de gauche - un_myAX12_2 = new AX12(p13, p14, 1, 1000000); - deux_myAX12_2 = new AX12(p13, p14, 2, 1000000); - trois_myAX12_2 = new AX12(p13, p14, 3, 1000000); - multiple_myAX12_2 = new AX12(p13,p14,0xFE,1000000); - - //Demonstration pince centrale - /* - Initialisation_position(); - wait(1); - Preparation_prise(); - wait(1); - Stockage_haut(); - wait(1); - Preparation_prise(); - wait(1); - Stockage_bas(); - wait(1); - Pousser_module(); - wait(1); - Preparation_depot_bas(); - wait(1); - Deposer(); - wait(1); - Preparation_depot_haut(); - wait(1); - Deposer(); - wait(1); - */ + declarationAX12(); //Demonstration bras gauche - Initialisation_gauche(); + /*Initialisation_gauche(); wait(1); Preparation_prise_gauche(); wait(1); @@ -314,10 +92,12 @@ wait(1); Preparation_module_gauche(); wait(1); - Tourner_module_gauche(); - + Tourner_module_gauche();*/ + float position =0; while(true) { //AX12_automate(); + canProcessRx(); + } } @@ -348,6 +128,7 @@ case CHECK_AX12: SendRawId(ALIVE_AX12); flag = 1; + mbed_reset(); break; case SERVO_AX12_ACTION : @@ -359,6 +140,47 @@ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0]; can1.write(msgTx); break; + case 0x123: + SendRawId(ALIVE_AX12); + if(msgRxBuffer[FIFO_lecture].data[0] == 0){ + getPosiotionCentrale(); + }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){ + getPosiotionGauche(); + }else{ + getPosiotionDroite(); + } + break; + case 0x456: + //Demonstration pince centrale + switch(msgRxBuffer[FIFO_lecture].data[0]){ + case 0: + Initialisation_position(); + break; + case 1: + Preparation_prise(); + break; + case 2: + Stockage_haut(); + break; + case 3: + Stockage_bas(); + break; + case 4: + Pousser_module(); + break; + case 5: + Preparation_depot_bas(); + break; + case 6: + Deposer(); + break; + case 7: + Preparation_depot_haut(); + break; + + } + wait(1); + break; } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; @@ -372,7 +194,7 @@ void canRx_ISR (void) { if (can1.read(msgRxBuffer[FIFO_ecriture])) { - if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); + if(msgRxBuffer[FIFO_ecriture].id==0x65) mbed_reset(); else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; } } @@ -388,163 +210,8 @@ msgTx.len=0; can1.write(msgTx); } - void Init_AX12() // Initialisation des différents paramétres -{ - un_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) - sept_myAX12-> Set_Goal_speed(vitesse); - huit_myAX12-> Set_Goal_speed(vitesse); - seize_myAX12-> Set_Goal_speed(vitesse); - - un_myAX12-> Set_Mode(0); - sept_myAX12-> Set_Mode(0); - huit_myAX12-> Set_Mode(0); - seize_myAX12-> Set_Mode(0); -} -/****************************************************************************************/ -/* FUNCTION NAME: Initialisation_position */ -/* DESCRIPTION : Fonction qui place les bras en position verticale */ -/****************************************************************************************/ -void Initialisation_position(void){ - multiple_myAX12->multiple_goal_and_speed(4,TAB12); - wait(TIME); - multiple_myAX12->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){ - multiple_myAX12->multiple_goal_and_speed(4,TAB12); - wait(TIME); - action_precedente = 1; - } - multiple_myAX12->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){ - multiple_myAX12->multiple_goal_and_speed(4,TAB3); - wait(TIME); - multiple_myAX12->multiple_goal_and_speed(4,TAB4); - wait(TIME); - multiple_myAX12->multiple_goal_and_speed(4,TAB5); - wait(TIME); - multiple_myAX12->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){ - multiple_myAX12->multiple_goal_and_speed(4,TAB3); - wait(TIME); - multiple_myAX12->multiple_goal_and_speed(4,TAB7); - wait(TIME); -} - -/****************************************************************************************/ -/* FUNCTION NAME: Deposer */ -/* DESCRIPTION : Fonction qui permet de déposer le module */ -/****************************************************************************************/ -void Deposer(void){ - multiple_myAX12->multiple_goal_and_speed(4,TAB9); - wait(TIME/5); - multiple_myAX12->multiple_goal_and_speed(4,TAB8); - wait(TIME/5); - multiple_myAX12->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){ - multiple_myAX12->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){ - multiple_myAX12->multiple_goal_and_speed(4,TAB6); - wait(TIME); - multiple_myAX12->multiple_goal_and_speed(4,TAB5); - wait(TIME); - multiple_myAX12->multiple_goal_and_speed(4,TAB10); - wait(TIME); - multiple_myAX12->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){ - multiple_myAX12->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); - multiple_myAX12_2->multiple_goal_and_speed(4,TAB22); - wait(TIME); - multiple_myAX12_2->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){ - multiple_myAX12_2->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){ - multiple_myAX12_2->multiple_goal_and_speed(4,TAB23); - wait(TIME); -} - /****************************************************************************************/ /* FUNCTION NAME: Fin_action */