code ax12 petit robot 12/05/2017
Fork of command_AX12_petit_robot_V3 by
main.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-12
- Revision:
- 7:ad4a19e26b84
- Parent:
- 6:c239c7abd7fe
File content as of revision 7:ad4a19e26b84:
#include "mbed.h" #include "AX12.h" #include "actionneurs.h" #include "global.h" #include "ident_crac.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 SIZE_FIFO 25 extern "C" void mbed_reset();//Pour pouvoir reset la carte /****************************************************************************************/ /* 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: 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); /* 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_bras = 0, etat_ax12 = 0, flag = 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; /* 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 declarationAX12(); //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();*/ float position =0; while(true) { //AX12_automate(); canProcessRx(); } } /* 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(); 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 CHECK_AX12: SendRawId(ALIVE_AX12); flag = 1; mbed_reset(); break; case SERVO_AX12_ACTION : etat_ax12 = msgRxBuffer[FIFO_lecture].data[0]; //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; case 0x123: SendRawId(ALIVE_AX12); if(msgRxBuffer[FIFO_lecture].data[0] == 0){ getPosiotionCentrale(); }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){ getPosiotionGauche(); }else{ getPosiotionDroite(); } break; case 0x456: //Demonstration pince centrale switch(msgRxBuffer[FIFO_lecture].data[0]){ case 0: Initialisation_position(); break; case 1: Preparation_prise(); break; case 2: Stockage_haut(); break; case 3: Stockage_bas(); break; case 4: Pousser_module(); break; case 5: Preparation_depot_bas(); break; case 6: Deposer(); break; case 7: Preparation_depot_haut(); break; } wait(1); 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==0x65) 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); } /****************************************************************************************/ /* 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; } }