code ax12 petit robot 12/05/2017
Fork of command_AX12_petit_robot_V3 by
Diff: main.cpp
- Revision:
- 4:888b32bc5b63
- Parent:
- 3:6cbd7d2c78f4
- Child:
- 6:c239c7abd7fe
diff -r 6cbd7d2c78f4 -r 888b32bc5b63 main.cpp --- 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