CRAC Team
/
ACRAC_carte_esclave_GROS_ROBOT_2019
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed Herkulex_Library_2019 ident_crac actions_Pr
main.cpp
- Committer:
- Artiom
- Date:
- 2019-05-26
- Revision:
- 44:381ecf63e6ab
- Parent:
- 43:7fff7f4d79a1
- Child:
- 45:11614fc23e53
File content as of revision 44:381ecf63e6ab:
#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;
unsigned short distance_recalage,distance_revenir;
unsigned short distance_goldenium;
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();
if(cote) {
convoyeur_gauche_violet();
convoyeur_droit_violet();
} else {
convoyeur_gauche_jaune(); //ok
convoyeur_droit_jaune(); //ok
}
#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 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;
distance_recalage=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
distance_revenir=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
break;
case ACCELERATEUR_ARRIERE:
faccelerateur_arriere=1;
break;
case GOLDENIUM_AVANT:
fgoldenium_avant=1;
distance_goldenium=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
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;
case HACHEUR_ETAT_CONTACTS :
status_contact = msgRxBuffer[FIFO_lecture].data[0];
break;
#endif
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
}
}