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