AX12 gros robot
Fork of command_AX12_gros_robot by
Diff: main.cpp
- Revision:
- 2:99b1cb0d9f5e
- Parent:
- 1:b3ff77670606
- Child:
- 3:6cbd7d2c78f4
--- a/main.cpp Tue Mar 21 14:46:54 2017 +0000 +++ b/main.cpp Tue Apr 18 16:02:40 2017 +0000 @@ -1,42 +1,139 @@ #include "mbed.h" #include "AX12.h" #include "cmsis.h" +#include "ident_crac.h" +#define SIZE_FIFO 14 +#define TIME 0.8 + /* 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 + +extern "C" void mbed_reset();//Pour pouvoir reset la carte + +unsigned char action = 0, choix = 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, *six_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; + + + /* PROTOTYPES DE FONCTIONS ET POINTEURS */ + + + +/****************************************************************************************/ +/* FUNCTION NAME: canProcessRx */ +/* DESCRIPTION : Fonction de traitement des messages CAN */ +/****************************************************************************************/ +void canProcessRx(void); + +/****************************************************************************************/ +/* FUNCTION NAME: canRx_ISR */ +/* DESCRIPTION : Interruption en réception sur le CAN */ +/****************************************************************************************/ +void canRx_ISR (void); + +/****************************************************************************************/ +/* FUNCTION NAME: SendRawId */ +/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ +/****************************************************************************************/ +void SendRawId (unsigned short id); + +/****************************************************************************************/ +/* FUNCTION NAME: prendre_modules */ +/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */ +/****************************************************************************************/ +void Prendre_modules(unsigned char choix); + +/****************************************************************************************/ +/* FUNCTION NAME: Poser_modules */ +/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */ +/****************************************************************************************/ +void Poser_modules(unsigned char choix); + +/****************************************************************************************/ +/* FUNCTION NAME: Funny_action */ +/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */ +/****************************************************************************************/ +void Funny_Action(void); + +/****************************************************************************************/ +/* FUNCTION NAME: Tourner_modules */ +/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */ +/****************************************************************************************/ +void Tourner_modules(void); + Timer t; Ticker flipper; -static char TAB1[25]= {0x06,0x0A, 0x01, 0xFF, 0x00, - 0x12,0xBC, 0x02, 0xFF, 0x00, - 0x04,0xE9, 0x00, 0xFF, 0x00, - 0x07,0x4D, 0x01, 0xFF, 0x00, - 0x0F,0x90, 0x01, 0xFF, 0x00}; + +///////////TABLEAU PINCE DROITE /////////////////////////// +static char TAB1[25]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte + 0x04,0x00, 0x02, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; -static char TAB2[25]= {0x06,0x0A, 0x01, 0xFF, 0x00, - 0x12,0x25, 0x02, 0xFF, 0x00, - 0x04,0x15, 0x02, 0xFF, 0x00, - 0x07,0x4D, 0x01, 0xFF, 0x00, - 0x0F,0x90, 0x01, 0xFF, 0x00}; +static char TAB2[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte + 0x04,0xC0, 0x00, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; + +static char TAB3[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee + 0x04,0x15, 0x02, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; + +static char TAB4[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement + 0x04,0x30, 0x01, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; + +static char TAB5[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte + 0x04,0xC0, 0x00, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; + +static char TAB6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin + 0x04,0x4D, 0x01, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; + -static char TAB3[25]= {0x06,0x0A, 0x01, 0xFF, 0x00, - 0x12,0x2C, 0x01, 0xFF, 0x00, - 0x04,0x15, 0x02, 0xFF, 0x00, - 0x07,0x4D, 0x01, 0xFF, 0x00, - 0x0F,0x90, 0x01, 0xFF, 0x00}; +//////////////////TABLEAU PINCE GAUCHE ///////////////////////////// +static char TAB1125]= {0x12,0xA0, 0x00, 0xFF, 0x03, ///en l'air pince ouverte + 0x04,0x00, 0x02, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; + +static char TAB12[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte + 0x04,0xC0, 0x00, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; -static char TAB4[25]= {0x06,0x0A, 0x01, 0xFF, 0x00, - 0x12,0x2C, 0x01, 0xFF, 0x00, - 0x04,0x4D, 0x01, 0xFF, 0x00, - 0x07,0x4D, 0x01, 0xFF, 0x00, - 0x0F,0x90, 0x01, 0xFF, 0x00}; +static char TAB13[25]= {0x12,0xE0, 0x00, 0xFF, 0x03, ///en haut pince fermee + 0x04,0x15, 0x02, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; + +static char TAB14[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air petit mouvement + 0x04,0x30, 0x01, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; -static char TAB5[25]= {0x06,0x0A, 0x01, 0xFF, 0x00, - 0x12,0x2C, 0x01, 0xFF, 0x00, - 0x04,0x4D, 0x01, 0xFF, 0x00, - 0x07,0x6E, 0x01, 0xFF, 0x00, - 0x0F,0x90, 0x01, 0xFF, 0x00}; +static char TAB15[25]= {0x12,0xFF, 0x02, 0xFF, 0x03, ///en bas pince ouverte + 0x04,0xC0, 0x00, 0xFF, 0x03, + 0x07,0xC0, 0x00, 0xFF, 0x03, + 0x0F,0x05, 0x02, 0xFF, 0x03}; + +static char TAB1 6[25]= {0x12,0x00, 0x01, 0xFF, 0x03, ///en l'air fin + 0x04,0x4D, 0x01, 0xFF, 0x03, + 0x07,0x4D, 0x01, 0xFF, 0x03, + 0x0F,0x90, 0x01, 0xFF, 0x03}; - - + /* ANGLE */ /* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02 @@ -61,126 +158,223 @@ 6 = 0x06 | 15 = 0x0F 7 = 0x07 | 16 = 0x10 8 = 0x08 | 17 = 0x11 */ - - /*static char Tab1[5]={ 0x09, - 0x5E, - 0x01, - 0x70, - 0x01};*/ - - /*static char Tab2[5]={ 0x05, - 0x96, - 0x01, - 0x85, - 0x05};*/ -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; - - - /* DECLARATION PROTOTYPES & POINTEURS */ - -short vitesse=700; // Vitesse angulaire (0-1023) -AX12 *dixhuit_myAX12, *quatre_myAX12,*six_myAX12, *sept_myAX12, *quinze_myAX12, *multiple_AX12; -void Init_AX12(void); -void Lecture(void); -void Flip(void); - - - /* FONCTIONS */ + + /* MAIN */ -void Init_AX12() // Initialisation des différents paramétres +int main() { - - dixhuit_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) - quatre_myAX12-> Set_Goal_speed(vitesse); + 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; + + /* + deux_myAX12 = new AX12(p9, p10, 2, 1000000); //main tourneuse + quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); // main tourneuse + */ + + trois_myAX12 = new AX12(p9, p10, 3, 1000000); //pince droite + treize_myAX12 = new AX12(p9, p10, 13, 1000000); //pince droite + quatorze_myAX12 = new AX12(p9, p10, 14, 1000000); //pince droite + huit_myAX12 = new AX12(p9, p10, 8, 1000000); //pince droite + + quatre_myAX12 = new AX12(p13, p14, 4, 1000000); //pince gauche + sept_myAX12 = new AX12(p13, p14, 7, 1000000); //pince gauche + quinze_myAX12 = new AX12(p13, p14, 15, 1000000); //pince gauche + dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); //pince gauche + + six_myAX12 = new AX12(p28, p27, 6, 1000000); //funny action + + multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); + multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); + + + while(true) { + canProcessRx();//Traitement des trames CAN en attente + + if (action == 0){ + choix = 1; + Prendre_modules(choix); + choix = 2; + Prendre_modules(choix); + wait(2); + /* + choix = 1; + Poser_modules(choix); + choix = 2; + Poser_modules(choix); + wait(2); + + TrameTx.id = 0x092; + TrameTx.len = 1; + TrameTx.data[0] = 0; + can1.write(TrameTx); + */ + + action = 2; + } + } +} + + /* FONCTIONS */ + +/****************************************************************************************/ +/* FUNCTION NAME: canProcessRx */ +/* DESCRIPTION : Fonction de traitement des messages CAN */ +/****************************************************************************************/ +void canProcessRx(void) +{ + static signed char FIFO_occupation=0,FIFO_max_occupation=0; + CANMessage msgTx=CANMessage(); + 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; + break; + + case CHECK_ACTIONNEURS: + SendRawId (ALIVE_ACTIONNEURS); + break; + + } + + FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; + } +} + +/****************************************************************************************/ +/* FUNCTION NAME: canRx_ISR */ +/* DESCRIPTION : Interruption en réception sur le CAN */ +/****************************************************************************************/ +void canRx_ISR (void) +{ + if (can1.read(msgRxBuffer[FIFO_ecriture])) { + if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset(); + else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; + } +} + +/****************************************************************************************/ +/* FUNCTION NAME: SendRawId */ +/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */ +/****************************************************************************************/ +void SendRawId (unsigned short id) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=id; + msgTx.len=0; + can1.write(msgTx); +} + void Init_AX12() // Initialisation des différents paramétres +{ + deux_myAX12-> Set_Goal_speed(vitesse); // vitesse (0-1023) + quatre_myAX12-> Set_Goal_speed(vitesse); six_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); + huit_myAX12-> Set_Goal_speed(vitesse); + trois_myAX12-> Set_Goal_speed(vitesse); + quatorze_myAX12-> Set_Goal_speed(vitesse); + treize_myAX12-> Set_Goal_speed(vitesse); - dixhuit_myAX12-> Set_Mode(0); // Mode Position(0) Mode Continu(1) + deux_myAX12-> Set_Mode(0); quatre_myAX12-> Set_Mode(0); six_myAX12-> Set_Mode(0); sept_myAX12-> Set_Mode(0); + quatorze_myAX12-> Set_Mode(0); quinze_myAX12-> Set_Mode(0); -} - -/*void Lecture() -{ - angle= socle_myAX12-> Get_Position(); - test_socle = socle_myAX12 -> read_and_test(angle,TAB); - - angle= bas_myAX12-> Get_Position(); - test_bas = bas_myAX12-> read_and_test(angle,TAB); - - angle= milieu_myAX12-> Get_Position(); - test_milieu = milieu_myAX12-> read_and_test(angle,TAB); - - angle= haut_myAX12-> Get_Position(); - test_haut = haut_myAX12-> read_and_test(angle,TAB); - - angle= ventouse_myAX12-> Get_Position(); - test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB); - - valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse; + 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); +} + +/****************************************************************************************/ +/* FUNCTION NAME: prendre_modules */ +/* DESCRIPTION : Fonction pour prendre des modules lunaires avec la pince */ +/****************************************************************************************/ +void Prendre_modules(unsigned char choix){ + if (choix == 1){ + //multiple_myAX12->multiple_goal_and_speed(4,TAB1); + //wait(TIME); + multiple_myAX12->multiple_goal_and_speed(4,TAB2); + wait(TIME); + multiple_myAX12->multiple_goal_and_speed(4,TAB5); + wait(TIME); + 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(5,TAB6); + wait(TIME); + } - #ifdef AX12_DEBUG - printf("\ntest_socle = %f",test_socle); - #endif - #ifdef AX12_DEBUG - printf("\ntest_bas = %f",test_bas); - #endif - #ifdef AX12_DEBUG - printf("\ntest_milieu = %f",test_milieu); - #endif - #ifdef AX12_DEBUG - printf("\ntest_haut = %f",test_haut); - #endif - #ifdef AX12_DEBUG - printf("\ntest_ventouse = %f",test_ventouse); - #endif - #ifdef AX12_DEBUG - printf("\nvaleur_test = %f",valeur_test); - #endif -}*/ - - /* PROGRAMME PRINCIPAL*/ + else if (choix == 2){ + //multiple2_myAX12->multiple_goal_and_speed(4,TAB11); + //wait(TIME); + multiple2_myAX12->multiple_goal_and_speed(4,TAB12); + wait(TIME); + multiple2_myAX12->multiple_goal_and_speed(4,TAB15); + wait(TIME); + multiple2_myAX12->multiple_goal_and_speed(4,TAB13); + wait(TIME); + multiple2_myAX12->multiple_goal_and_speed(4,TAB14); + wait(TIME); + multiple2_myAX12->multiple_goal_and_speed(5,TAB16); + wait(TIME); + } + + SendRawId(0x123); +} -int main() -{ - // flipper.attach(&Flip, 2.0); - dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000); - quatre_myAX12 = new AX12(p13, p14, 4, 1000000); - six_myAX12 = new AX12(p13, p14, 6, 1000000); - sept_myAX12 = new AX12(p13, p14, 7, 1000000); - quinze_myAX12 = new AX12(p13, p14, 15, 1000000); - multiple_AX12 = new AX12(p13,p14,0xFE,1000000); // Création objet : data pin 9&10, ID Broadcast, baud - - multiple_AX12->multiple_goal_and_speed(5,TAB1); - wait(2); - multiple_AX12->multiple_goal_and_speed(5,TAB2); - wait(2); - multiple_AX12->multiple_goal_and_speed(5,TAB3); - wait(2); - multiple_AX12->multiple_goal_and_speed(5,TAB4); - wait(2); - multiple_AX12->multiple_goal_and_speed(5,TAB5); - - - //six_myAX12 -> Set_Secure_Goal(200); - /*quatorze_myAX12 -> Set_Secure_Goal(260); - gauche_myAX12 -> Set_Secure_Goal(280); - quatorze_myAX12 -> Set_Secure_Goal(300); //avoir ax12 14 a 90°*/ - - /* while(valeur_test<4) - { - Lecture(); - }*/ - - - - +/****************************************************************************************/ +/* FUNCTION NAME: Poser_modules */ +/* DESCRIPTION : Fonction qui permet de déposer des modules lunaires */ +/****************************************************************************************/ +void Poser_modules(unsigned char choix){ + multiple_myAX12->multiple_goal_and_speed(4,TAB4); + wait(2); + multiple_myAX12->multiple_goal_and_speed(4,TAB3); + wait(2); + multiple_myAX12->multiple_goal_and_speed(4,TAB2); + wait(2); + multiple_myAX12->multiple_goal_and_speed(4,TAB1); +} + +/****************************************************************************************/ +/* FUNCTION NAME: Funny_action */ +/* DESCRIPTION : Fonction qui permet d'effectuer la Funny Action en fin de match */ +/****************************************************************************************/ +void Funny_Action(void){ + six_myAX12 -> Set_Secure_Goal(150); + wait(0.5); + six_myAX12 -> Set_Secure_Goal(60); + wait(0.5); + six_myAX12 -> Set_Secure_Goal(150); +} + +/****************************************************************************************/ +/* FUNCTION NAME: Tourner_modules */ +/* DESCRIPTION : Fonction qui permet de deployer la main tourneuse */ +/****************************************************************************************/ +void Tourner_modules(void){ + multiple2_myAX12 -> multiple_goal_and_speed(2,TAB11); + wait(1.5); + multiple2_myAX12 -> multiple_goal_and_speed(2,TAB12); +} -} +