Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
Diff: AX12/AX12.cpp
- Revision:
- 16:7321fb3bb396
- Parent:
- 15:c2fc239e85df
--- a/AX12/AX12.cpp Thu May 11 12:55:52 2017 +0000 +++ b/AX12/AX12.cpp Fri May 19 17:13:46 2017 +0000 @@ -21,114 +21,10 @@ * THE SOFTWARE. */ -#include "mbed.h" #include "AX12.h" -#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 - #define MAX_TIMEOUT 500 -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 - -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}; - -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}; - -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}; - - - extern Timer t; typedef struct @@ -933,7 +829,7 @@ { #ifdef AX12_DEBUG - printf("\nGetPositionID(%d)",_ID); + printf("\nGetPositionID(%d) \n\r",_ID); #endif char data[2]; @@ -1678,151 +1574,3 @@ return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) } - - - - -////////////////////////////////////////////////////////////////////////////// - -/****************************************************************************************/ -/* 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: Preparation_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); -}