![](/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-10
- Revision:
- 4:4a79942713fa
- Parent:
- 2:9e63099cca99
- Child:
- 5:6e198cdd99ad
File content as of revision 4:4a79942713fa:
#include "mbed.h" #include "ident_crac.h" #include "Capteur.h" #include "Actionneurs.h" #include "fonctions_herkulex.h" #include "main.h" #define SIZE_FIFO 50 CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN 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 fpresentoir_avant=0; char status_pompe=0; void canRx_ISR (void); void SendAck(unsigned short id, unsigned short from); void SendRawId (unsigned short id); void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement); void canProcessRx(void); void automate_ventouse_presentoir_avant(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; } } /*********************************************************************************************/ /* FUNCTION NAME: SendAck */ /* DESCRIPTION : */ /*********************************************************************************************/ void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale { CANMessage msgTx=CANMessage(); msgTx.id=from; //waitingAckFrom msgTx.len=2; msgTx.format=CANStandard; msgTx.type=CANData; // from sur 2 octets msgTx.data[0]=(unsigned char)id; //waitingAckID msgTx.data[1]=(unsigned char)(id>>8); can.write(msgTx); } /*********************************************************************************************/ /* FUNCTION NAME: SendRawId */ /* DESCRIPTION : */ /*********************************************************************************************/ void SendRawId (unsigned short id) { CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=0; can.write(msgTx); //carte esclave f446re wait_us(200); } /*********************************************************************************************/ /* FUNCTION NAME: GoStraight */ /* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */ /* recalage : 0 => pas de recalage */ /* 1 => recalage en X */ /* 2 => Recalage en Y */ /* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */ /* isEnchainement : Indique si il faut executer l'instruction en enchainement */ /* 0 => non */ /* 1 => oui */ /* 2 => dernière instruction de l'enchainement */ /*********************************************************************************************/ void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement) { CANMessage msgTx=CANMessage(); msgTx.id=ASSERVISSEMENT_RECALAGE; msgTx.len=6; msgTx.format=CANStandard; msgTx.type=CANData; // x sur 2 octets msgTx.data[0]=(unsigned char)distance; msgTx.data[1]=(unsigned char)(distance>>8); //Recalage sur 1 octet msgTx.data[2]=recalage; //Valeur du recalage sur 2 octets msgTx.data[3]=(unsigned char)newValue; msgTx.data[4]=(unsigned char)(newValue>>8); //Enchainement sur 1 octet msgTx.data[5]=isEnchainement; can.write(msgTx); //wait_ms(500); } 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 gabarit_petit_robot(); while(1) { canProcessRx(); automate_ventouse_presentoir_avant(); 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 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; #ifdef ROBOT_SMALL //-------------------------------------------------------------------------Actions petit robot---------------------------------------------- case GABARIT_PETIT_ROBOT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); gabarit_petit_robot(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case PRESENTOIR_AVANT: fpresentoir_avant=1; break; case PRESENTOIR_ARRIERE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); presentoir_arriere(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case BALANCE_AVANT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); balance_avant(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case BALANCE_ARRIERE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); balance_arriere(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case ACCELERATEUR_AVANT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); accelerateur_avant(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case ACCELERATEUR_ARRIERE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); accelerateur_arriere(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case GOLDENIUM_AVANT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); balance_avant(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case GOLDENIUM_ARRIERE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); balance_arriere(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case SOL_AVANT: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); sol_avant(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case SOL_ARRIERE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); sol_arriere(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case SOL_AVANT_RELACHE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); sol_avant_relache(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; case SOL_ARRIERE_RELACHE: SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); sol_arriere_relache(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); break; //--------------------------------------------------------------------------ACK carte pompe---------------------------------------------- case HACHEUR_STATUT_VENTOUSES: status_pompe = msgRxBuffer[FIFO_lecture].data[1]; break; case HACHEUR_GET_ATOM_ACK: status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]); break; case HACHEUR_RELEASE_ATOM_ACK : status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]); break; //------------------------------------------------------------------------------------------------------------------------------------------- #endif #ifdef ROBOT_BIG case 3: break; case 5: break; #endif } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } } void automate_ventouse_presentoir_avant (void) { typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat; static type_etat etat = init; switch(etat) { case init: //attente d'initialisation if(fpresentoir_avant) etat=envoi_instruction; break; case envoi_instruction://envoi instruction SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); presentoir_avant(); SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); etat = attente_ack_ventouse; break; case attente_ack_ventouse: if(status_pompe&MASK_PRESENTOIR_AV==MASK_PRESENTOIR_AV) { fpresentoir_avant=0; etat = init; } else { SendRawId(HACHEUR_STATUT_VENTOUSES); } break; } }