CRAC Team
code ax12 petit robot 12/05/2017
Fork of
command_AX12_petit_robot_V3
by 
CRAC Team
Diff: main.cpp
- Revision:
- 4:888b32bc5b63
- Parent:
- 3:6cbd7d2c78f4
- Child:
- 6:c239c7abd7fe
--- a/main.cpp Wed Apr 19 15:53:45 2017 +0000
+++ b/main.cpp Tue May 02 12:06:47 2017 +0000
@@ -3,8 +3,18 @@
#include "cmsis.h"
#include "ident_crac.h"
-#define SIZE_FIFO 14
+#define AX12_INITIALISATION 0
+#define AX12_PREPARATION_PRISE 1
+#define AX12_STOCKAGE_HAUT 2
+#define AX12_STOCKAGE_BAS 3
+#define AX12_DEPOSER 4
+#define AX12_PREPARATION_DEPOT_BAS 5
+#define AX12_PREPARATION_DEPOT_HAUT 6
+#define AX12_POUSSER_MODULE 7
+#define AX12_DEFAUT 20
+
#define TIME 0.8
+#define SIZE_FIFO 25
/* DECLARATION VARIABLES */
CAN can1(p30,p29);
@@ -14,17 +24,18 @@
extern "C" void mbed_reset();//Pour pouvoir reset la carte
-unsigned char action = 0, choix = 0;
+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 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12;
+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 */
@@ -44,6 +55,18 @@
void SendRawId (unsigned short id);
/****************************************************************************************/
+/* FUNCTION NAME: Fin_action */
+/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
+/****************************************************************************************/
+void Fin_action(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Automate_ax12 */
+/* 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 */
/****************************************************************************************/
@@ -53,160 +76,155 @@
/* FUNCTION NAME: Preparation_prise */
/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
/****************************************************************************************/
-void Preparation_prise(unsigned char choix);
+void Preparation_prise(void);
/****************************************************************************************/
/* FUNCTION NAME: Stockage_haut */
/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
/****************************************************************************************/
-void Stockage_haut(unsigned char choix);
+void Stockage_haut(void);
/****************************************************************************************/
/* FUNCTION NAME: Stockage_bas */
/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
/****************************************************************************************/
-void Stockage_bas(unsigned char choix);
+void Stockage_bas(void);
/****************************************************************************************/
/* FUNCTION NAME: Deposer */
/* DESCRIPTION : Fonction qui permet de déposer un module */
/****************************************************************************************/
-void Deposer(unsigned char choix);
+void Deposer(void);
/****************************************************************************************/
/* FUNCTION NAME: Preparation_depos_bas */
/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
/****************************************************************************************/
-void Preparation_depos_bas(unsigned char choix);
+void Preparation_depot_bas(void);
/****************************************************************************************/
/* FUNCTION NAME: Preparation_depos_haut */
/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
/****************************************************************************************/
-void Preparation_depos_haut(unsigned char choix);
+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(unsigned char choix);
+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 DROITE ///////////////////////////
-static char TAB1[25]= {0x12,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
- 0x04,0xF4, 0x01, 0xFF, 0x00,
- 0x07,0x50, 0x00, 0xFF, 0x00,
- 0x0F,0xB0, 0x03, 0xFF, 0x00};
+////////////////////// 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};
-static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
- 0x04,0xE9, 0x00, 0xFF, 0x03,
+static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise
+ 0x10,0x50, 0x01, 0xFF, 0x03,
0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x0F,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB3[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
- 0x04,0xE9, 0x00, 0xFF, 0x03,
- 0x07,0xB5, 0x00, 0xFF, 0x03,
- 0x0F,0x39, 0x03, 0xFF, 0x03};
-
-static char TAB4[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
- 0x04,0x00, 0x02, 0xFF, 0x03,
- 0x07,0xB5, 0x00, 0xFF, 0x03,
- 0x0F,0x39, 0x03, 0xFF, 0x03};
-
-static char TAB5[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
- 0x04,0x60, 0x01, 0xFF, 0x00,
- 0x07,0xB5, 0x00, 0xFF, 0x00,
- 0x0F,0x39, 0x03, 0xFF, 0x00};
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
-static char TAB6[25]= {0x12,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x04,0x50, 0x01, 0xFF, 0x03,
- 0x07,0x0A, 0x01, 0xFF, 0x03,
- 0x0F,0xDD, 0x02, 0xFF, 0x03};
-
-static char TAB7[25]= {0x12,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
- 0x04,0xDE, 0x03, 0xFF, 0x03,
- 0x07,0xB5, 0x00, 0xFF, 0x03,
- 0x0F,0x39, 0x03, 0xFF, 0x03};
-
-static char TAB8[25]= {0x12,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
- 0x04,0x00, 0x03, 0xFF, 0x00,
- 0x07,0xB5, 0x00, 0xFF, 0x00,
- 0x0F,0x39, 0x03, 0xFF, 0x00};
+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 TAB9[25]= {0x12,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
- 0x04,0x00, 0x03, 0xFF, 0x03,
- 0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x0F,0xF4, 0x01, 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 TAB10[25]= {0x12,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
- 0x04,0x00, 0x02, 0xFF, 0x00,
- 0x07,0xB5, 0x00, 0xFF, 0x00,
- 0x0F,0x39, 0x03, 0xFF, 0x00};
+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 TAB11[25]= {0x12,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
- 0x04,0x00, 0x03, 0xFF, 0x00,
- 0x07,0xB5, 0x00, 0xFF, 0x00,
- 0x0F,0x39, 0x03, 0xFF, 0x00};
-
+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};
-//////////////////TABLEAU PINCE GAUCHE /////////////////////////////
-static char TAB21[25]= {0x08,0xDA, 0x00, 0xFF, 0x00, ///Position initiale
- 0x03,0xF4, 0x01, 0xFF, 0x00,
- 0x0D,0x50, 0x00, 0xFF, 0x00,
- 0x0E,0xB0, 0x03, 0xFF, 0x00};
-
-static char TAB22[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Preparation prise
- 0x03,0xE9, 0x00, 0xFF, 0x03,
- 0x0D,0xF4, 0x01, 0xFF, 0x03,
- 0x0E,0xF4, 0x01, 0xFF, 0x03};
+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 TAB23[25]= {0x08,0xFF, 0x02, 0xFF, 0x03, ///Stockage haut/bas (attraper module)
- 0x03,0xE9, 0x00, 0xFF, 0x03,
- 0x0D,0xB5, 0x00, 0xFF, 0x03,
- 0x0E,0x39, 0x03, 0xFF, 0x03};
-
-static char TAB24[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (bras en l'air)
- 0x03,0x00, 0x02, 0xFF, 0x03,
- 0x0D,0xB5, 0x00, 0xFF, 0x03,
- 0x0E,0x39, 0x03, 0xFF, 0x03};
+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 TAB25[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///Stockage haut (module sur tige)
- 0x03,0x60, 0x01, 0xFF, 0x00,
- 0x0D,0xB5, 0x00, 0xFF, 0x00,
- 0x0E,0x39, 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 TAB26[25]= {0x08,0x35, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x03,0x50, 0x01, 0xFF, 0x03,
- 0x0D,0x0A, 0x01, 0xFF, 0x03,
- 0x0E,0xDD, 0x02, 0xFF, 0x03};
-
-static char TAB27[25]= {0x08,0xA6, 0x00, 0xFF, 0x03, ///Stockage bas (bras en l'air)
- 0x03,0xDE, 0x03, 0xFF, 0x03,
- 0x0D,0xB5, 0x00, 0xFF, 0x03,
- 0x0E,0x39, 0x03, 0xFF, 0x03};
-
-static char TAB28[25]= {0x08,0xF4, 0x01, 0xFF, 0x00, ///Deposer bas (descendre bras)
- 0x03,0x00, 0x03, 0xFF, 0x00,
- 0x0D,0xB5, 0x00, 0xFF, 0x00,
- 0x0E,0x39, 0x03, 0xFF, 0x00};
+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 TAB29[25]= {0x08,0xF4, 0x01, 0xFF, 0x03, ///Deposer bas (pince ouverte)
- 0x03,0x00, 0x03, 0xFF, 0x03,
- 0x0D,0xF4, 0x01, 0xFF, 0x03,
- 0x0E,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 TAB30[25]= {0x08,0x35, 0x01, 0xFF, 0x00, ///deposer bas (preparation module haut)
- 0x03,0x00, 0x02, 0xFF, 0x00,
- 0x0D,0xB5, 0x00, 0xFF, 0x00,
- 0x0E,0x39, 0x03, 0xFF, 0x00};
-
-static char TAB31[25]= {0x08,0xB0, 0x01, 0xFF, 0x00, ///Pousser module (bras en bas)
- 0x03,0x00, 0x03, 0xFF, 0x00,
- 0x0D,0xB5, 0x00, 0xFF, 0x00,
- 0x0E,0x39, 0x03, 0xFF, 0x00};
-
+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 */
@@ -241,41 +259,65 @@
{
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
-
- CANMessage TrameTx = CANMessage();
- TrameTx.format=CANStandard;
- TrameTx.type=CANData;
-
- trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince gauche
- treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince gauche
- quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince gauche
- huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince gauche
-
- quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince droite
- sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince droite
- quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince droite
- dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince droite
-
- multiple_myAX12 = new AX12(p13,p14,0xFE,1000000);
- multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000);
- //Initialisation de la position du bras
+ //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);
+ */
+
+ //Demonstration bras gauche
+ Initialisation_gauche();
+ wait(1);
+ Preparation_prise_gauche();
+ wait(1);
+ Prendre_module_gauche();
+ wait(1);
+ Preparation_module_gauche();
+ wait(1);
+ Tourner_module_gauche();
+ wait(1);
+ Preparation_module_gauche();
+ wait(1);
+ Tourner_module_gauche();
+ wait(1);
+ Preparation_module_gauche();
+ wait(1);
+ Tourner_module_gauche();
+
while(true) {
- canProcessRx();//Traitement des trames CAN en attente
-
- if (action == 0){
-
- /*
- TrameTx.id = 0x092;
- TrameTx.len = 1;
- TrameTx.data[0] = 0;
- can1.write(TrameTx);
- */
-
- action = 2;
- }
+ //AX12_automate();
}
}
@@ -288,23 +330,35 @@
void canProcessRx(void)
{
static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+
CANMessage msgTx=CANMessage();
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+
if(FIFO_occupation<0)
FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+
if(FIFO_max_occupation<FIFO_occupation)
FIFO_max_occupation=FIFO_occupation;
+
if(FIFO_occupation!=0) {
switch(msgRxBuffer[FIFO_lecture].id) {
- case SERVO_AX12_PROCESS :
- action = 1;
+ case CHECK_AX12:
+ SendRawId(ALIVE_AX12);
+ flag = 1;
break;
+
+ case SERVO_AX12_ACTION :
+ etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
- case CHECK_ACTIONNEURS:
- SendRawId (ALIVE_ACTIONNEURS);
+ //ACK de reception des actions a effectuer
+ msgTx.id = SERVO_AX12_ACK;
+ msgTx.len = 1;
+ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+ can1.write(msgTx);
break;
-
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
@@ -336,27 +390,15 @@
}
void Init_AX12() // Initialisation des différents paramétres
{
- deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
- quatre_myAX12-> Set_Goal_speed(vitesse);
- sept_myAX12-> Set_Goal_speed(vitesse);
- quatorze_myAX12-> Set_Goal_speed(vitesse);
- quinze_myAX12-> Set_Goal_speed(vitesse);
- dixhuit_myAX12-> Set_Goal_speed(vitesse);
+ un_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023)
+ sept_myAX12-> Set_Goal_speed(vitesse);
huit_myAX12-> Set_Goal_speed(vitesse);
- trois_myAX12-> Set_Goal_speed(vitesse);
- quatorze_myAX12-> Set_Goal_speed(vitesse);
- treize_myAX12-> Set_Goal_speed(vitesse);
+ seize_myAX12-> Set_Goal_speed(vitesse);
- deux_myAX12-> Set_Mode(0);
- quatre_myAX12-> Set_Mode(0);
+ un_myAX12-> Set_Mode(0);
sept_myAX12-> Set_Mode(0);
- quatorze_myAX12-> Set_Mode(0);
- quinze_myAX12-> Set_Mode(0);
- dixhuit_myAX12-> Set_Mode(0);
- huit_myAX12-> Set_Mode(0);
- trois_myAX12-> Set_Mode(0);
- quatorze_myAX12-> Set_Mode(0);
- treize_myAX12-> Set_Mode(0);
+ huit_myAX12-> Set_Mode(0);
+ seize_myAX12-> Set_Mode(0);
}
@@ -365,151 +407,220 @@
/* 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);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
- wait(TIME);
+ wait(TIME);
}
/****************************************************************************************/
/* FUNCTION NAME: Preparation_prise */
/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
/****************************************************************************************/
-void Preparation_prise(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+void Preparation_prise(void){
+ if (action_precedente == 0){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB12);
wait(TIME);
+ action_precedente = 1;
}
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
- wait(TIME);
- }
+ 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(unsigned char choix){
- if (choix == 1){
- 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);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
- wait(TIME);
- }
+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(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB3);
- wait(TIME);
- multiple_myAX12->multiple_goal_and_speed(4,TAB7);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
- wait(TIME);
- }
+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(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB9);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
- wait(TIME);
- }
+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_depos_bas */
+/* FUNCTION NAME: Preparation_depot_bas */
/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
/****************************************************************************************/
-void Preparation_depos_bas(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB8);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
- wait(TIME);
- }
+void Preparation_depot_bas(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
}
/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_haut */
+/* FUNCTION NAME: Preparation_depot_haut */
/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
/****************************************************************************************/
-void Preparation_depos_haut(unsigned char choix){
- if (choix == 1){
- 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);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
- wait(TIME);
- multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
- wait(TIME);
- }
+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(unsigned char choix){
- if (choix == 1){
- multiple_myAX12->multiple_goal_and_speed(4,TAB11);
- wait(TIME);
- }
-
- else if (choix == 2){
- multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
- wait(TIME);
- }
+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 */
+/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
+/****************************************************************************************/
+void Fin_action(void){
+ CANMessage msgTx=CANMessage();
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+
+ msgTx.id = SERVO_AX12_END;
+ msgTx.len = 1;
+ msgTx.data[0] = AX12_PREPARATION_PRISE;
+ can1.write(msgTx);
+}
+/****************************************************************************************/
+/* FUNCTION NAME: Automate_ax12 */
+/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
+/****************************************************************************************/
+void AX12_automate(void){
+ switch(etat_ax12){
+ case AX12_INITIALISATION :
+ if (flag == 1){
+ Initialisation_position();
+ flag = 2;
+ }
+
+ break;
+
+ case AX12_PREPARATION_PRISE :
+ Preparation_prise();
+ if (action == 0){
+ Fin_action();
+ action ++;
+ }
+ break;
+
+ case AX12_STOCKAGE_HAUT :
+ Stockage_haut();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_STOCKAGE_BAS :
+ Stockage_bas();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_DEPOSER :
+ Deposer();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_PREPARATION_DEPOT_BAS :
+ Preparation_depot_bas();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_PREPARATION_DEPOT_HAUT :
+ Preparation_depot_haut();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_POUSSER_MODULE :
+ Pousser_module();
+ etat_ax12 = AX12_DEFAUT;
+ Fin_action();
+ break;
+
+ case AX12_DEFAUT :
+ action = 0;
+ break;
+ }
+}
\ No newline at end of file