mouvement AX12 petit robot version 2
Fork of command_AX12_petit_robot_v2 by
main.cpp
- Committer:
- SquirrelGod
- Date:
- 2017-04-18
- Revision:
- 2:99b1cb0d9f5e
- Parent:
- 1:b3ff77670606
- Child:
- 3:6cbd7d2c78f4
File content as of revision 2:99b1cb0d9f5e:
#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; ///////////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]= {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}; //////////////////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 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 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 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 */ /* MAIN */ int main() { 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); 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); 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); } 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); } /****************************************************************************************/ /* 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); }