![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
carte esclave Petit Robot
Dependencies: mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr
main.cpp
- Committer:
- Artiom
- Date:
- 2019-05-25
- Revision:
- 43:7fff7f4d79a1
- Parent:
- 42:a7300d563596
- Child:
- 44:381ecf63e6ab
File content as of revision 43:7fff7f4d79a1:
#include "main.h" #define SIZE_FIFO 50 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN Serial pc(USBTX,USBRX); CANMessage msgRxBuffer[SIZE_FIFO]; unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN unsigned char EtatGameEnd=0; char cote=0; void canProcessRx(void); /*********************************************************************************************/ /* FUNCTION NAME: canRx_ISR */ /* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */ /*********************************************************************************************/ void canRx_ISR (void) { if (can.read(msgRxBuffer[FIFO_ecriture])) { if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) { } FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO; } } int main() { can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN servo_interrupt_en(); //permettre les interuptions wait(1);//attente servo boot #ifdef ROBOT_SMALL pc.printf("\nPetit robot\n\n"); deverouillage_torque(); #endif #ifdef ROBOT_BIG pc.printf("\nGros robot\n\n"); deverouillage_torque(); wait(0.5); #endif gabarit_robot(); pc.printf("\nLAUNCHED"); while(1) { /*setTorque(doigt,TORQUE_FREE,2); pc.printf("pos : %d\n", getPos(doigt,2)); wait_ms(10.0);*/ canProcessRx(); f_mesure();//dt35 #ifdef ROBOT_SMALL automate_ventouse_presentoir_arriere(); automate_ventouse_presentoir_avant(); automate_ventouse_goldenium_arriere(); automate_ventouse_goldenium_avant(); automate_ventouse_sol_avant(); automate_ventouse_sol_arriere(); automate_ventouse_balance_avant(); automate_ventouse_balance_arriere(); automate_ventouse_relache_arriere(); automate_ventouse_relache_avant(); automate_ventouse_sol_avant_relache(); automate_ventouse_sol_arriere_relache(); automate_ventouse_accelerateur_arriere(); automate_ventouse_accelerateur_avant(); #endif #ifdef ROBOT_BIG automate_ventouse_presentoir_avant(); automate_ventouse_goldenium_avant(); automate_ventouse_relache_avant(); automate_ventouse_accelerateur_avant(); fifo_couleur(); ascenseur(); convoyeur_gauche(); convoyeur_droit(); #endif if(EtatGameEnd==1) { while(1); } } } /****************************************************************************************/ /* 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) { int identifiant=msgRxBuffer[FIFO_lecture].id; switch(identifiant) { case CHOICE_COLOR: cote = msgRxBuffer[FIFO_lecture].data[0]; /*SendMsgCan(0x788, (unsigned char*)&cote, 1); if(cote) printf("\nCote violet : %d",cote); else printf("\nCote jaune : %d", cote);*/ break; case GLOBAL_GAME_END: EtatGameEnd = 1; break; case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0]; short distance=lecture_telemetre(numero_telemetre); msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1 msgTx.len=2; msgTx.format=CANStandard; msgTx.type=CANData; // Rayon sur 2 octets msgTx.data[0]=(unsigned char)distance; msgTx.data[1]=(unsigned char)(distance>>8); can.write(msgTx); SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA); break; case DATA_RECALAGE: short distance1=lecture_telemetre(1); short distance2=lecture_telemetre(2); short distance3=lecture_telemetre(3); short distance4=lecture_telemetre(4); msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1 msgTx.len=8; msgTx.format=CANStandard; msgTx.type=CANData; // Rayon sur 2 octets msgTx.data[0]=(unsigned char)distance1; msgTx.data[1]=(unsigned char)(distance1>>8); msgTx.data[2]=(unsigned char)distance2; msgTx.data[3]=(unsigned char)(distance2>>8); msgTx.data[4]=(unsigned char)distance3; msgTx.data[5]=(unsigned char)(distance3>>8); msgTx.data[6]=(unsigned char)distance4; msgTx.data[7]=(unsigned char)(distance4>>8); can.write(msgTx); SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE); break; case DATA_TELEMETRE_LOGIQUE: msgTx.id=RECEPTION_TELEMETRE_LOGIQUE; // tx Valeur Telemetre1 msgTx.len=4; msgTx.format=CANStandard; msgTx.type=CANData; msgTx.data[0]=(unsigned char)DT1_interrupt_Ex; msgTx.data[1]=(unsigned char)DT2_interrupt_Ex; msgTx.data[2]=(unsigned char)DT3_interrupt_Ex; msgTx.data[3]=(unsigned char)DT4_interrupt_Ex; can.write(msgTx); SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_TELEMETRE_LOGIQUE); break; //--------------------------------------------------------------------------ACK carte pompe---------------------------------------------- case HACHEUR_STATUT_VENTOUSES: status_pompe = msgRxBuffer[FIFO_lecture].data[1]; //can.write(CANMessage(0x529, &status_pompe,1)); break; case HACHEUR_GET_ATOM_ACK: status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]); //can.write(CANMessage(0x529, &status_pompe,1)); break; case HACHEUR_RELEASE_ATOM_ACK : status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]); break; case HACHEUR_ETAT_CONTACTS : status_contact = msgRxBuffer[FIFO_lecture].data[0]; break; //------------------------------------------------------------------------------------------------------------------------------------------- case PRESENTOIR_AVANT: fpresentoir_avant=1; break; case GABARIT_ROBOT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); gabarit_robot(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case AVANT_RELACHE: favant_relache=1; break; #ifdef ROBOT_SMALL //-------------------------------------------------------------------------Actions petit robot---------------------------------------------- case PRESENTOIR_ARRIERE: fpresentoir_arriere=1; break; case BALANCE_AVANT: fbalance_avant=1; break; case BALANCE_ARRIERE: fbalance_arriere=1; break; case ACCELERATEUR_AVANT: faccelerateur_avant=1; break; case ACCELERATEUR_ARRIERE: faccelerateur_arriere=1; break; case GOLDENIUM_AVANT: fgoldenium_avant=1; break; case GOLDENIUM_ARRIERE: fgoldenium_arriere=1; break; case SOL_AVANT: fsol_avant=1; break; case SOL_ARRIERE: fsol_arriere=1; break; case SOL_AVANT_RELACHE: fsol_avant_relache=1; break; case SOL_ARRIERE_RELACHE: fsol_arriere_relache=1; break; case ARRIERE_RELACHE: farriere_relache=1; break; case RECROQUEVILLER: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); recroqueviller_avant(); recroqueviller_arriere(); verification(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case VENTOUSE_AV_CENTRE_BALANCE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); goldenium_avant(); verification(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case VENTOUSE_AR_CENTRE_BALANCE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); goldenium_arriere(); positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur verification(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case ACCELERATEUR_INSERTION_AVANT_GAUCHE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); accelerateur_insertion_avant_gauche(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case ACCELERATEUR_INSERTION_ARRIERE_GAUCHE : SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); accelerateur_insertion_arriere_gauche(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; #endif #ifdef ROBOT_BIG case ASCENSEUR: flag_ascenseur = msgRxBuffer[FIFO_lecture].data[0]; break; #endif } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } }