carte_esclave2019 - carte esclave petit robot

CRAC Team » Code » carte_esclave2019

CRAC Team / Mbed 2 deprecated carte_esclave2019

carte esclave petit robot

Dependencies: mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@2:9e63099cca99, 2019-05-10 (annotated)

Committer:: Artiom
Date:: Fri May 10 09:11:09 2019 +0000
Revision:: 2:9e63099cca99
Parent:: 1:568955af8c2b
Child:: 4:4a79942713fa

Who changed what in which revision?

User	Revision	Line number	New contents of line
Artiom	0:bc74da1c502f	1	#include "mbed.h"
Artiom	0:bc74da1c502f	2	#include "ident_crac.h"
Artiom	0:bc74da1c502f	3	#include "Capteur.h"
Artiom	0:bc74da1c502f	4	#include "Actionneurs.h"
Artiom	0:bc74da1c502f	5	#include "fonctions_herkulex.h"
Artiom	1:568955af8c2b	6	#include "main.h"
Artiom	0:bc74da1c502f	7
Artiom	0:bc74da1c502f	8	#define SIZE_FIFO 50
Artiom	0:bc74da1c502f	9
Artiom	0:bc74da1c502f	10	CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom	0:bc74da1c502f	11
Artiom	0:bc74da1c502f	12
Artiom	0:bc74da1c502f	13	CANMessage msgRxBuffer[SIZE_FIFO];
Artiom	0:bc74da1c502f	14	unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom	0:bc74da1c502f	15	signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom	0:bc74da1c502f	16	unsigned char EtatGameEnd=0;
Artiom	0:bc74da1c502f	17
Artiom	0:bc74da1c502f	18	void canRx_ISR (void);
Artiom	0:bc74da1c502f	19	void SendAck(unsigned short id, unsigned short from);
Artiom	0:bc74da1c502f	20	void SendRawId (unsigned short id);
Artiom	0:bc74da1c502f	21	void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
Artiom	0:bc74da1c502f	22	void canProcessRx(void);
Artiom	0:bc74da1c502f	23
Artiom	0:bc74da1c502f	24	/*********************************************************************************************/
Artiom	0:bc74da1c502f	25	/* FUNCTION NAME: canRx_ISR */
Artiom	0:bc74da1c502f	26	/* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom	0:bc74da1c502f	27	/*********************************************************************************************/
Artiom	0:bc74da1c502f	28	void canRx_ISR (void)
Artiom	0:bc74da1c502f	29	{
Artiom	0:bc74da1c502f	30	if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom	0:bc74da1c502f	31	if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom	0:bc74da1c502f	32
Artiom	0:bc74da1c502f	33	}
Artiom	0:bc74da1c502f	34	FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	35
Artiom	0:bc74da1c502f	36	}
Artiom	0:bc74da1c502f	37
Artiom	0:bc74da1c502f	38	}
Artiom	0:bc74da1c502f	39	/*********************************************************************************************/
Artiom	0:bc74da1c502f	40	/* FUNCTION NAME: SendAck */
Artiom	0:bc74da1c502f	41	/* DESCRIPTION : */
Artiom	0:bc74da1c502f	42	/*********************************************************************************************/
Artiom	0:bc74da1c502f	43	void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale
Artiom	0:bc74da1c502f	44	{
Artiom	0:bc74da1c502f	45	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	46	msgTx.id=from; //waitingAckFrom
Artiom	0:bc74da1c502f	47	msgTx.len=2;
Artiom	0:bc74da1c502f	48	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	49	msgTx.type=CANData;
Artiom	0:bc74da1c502f	50	// from sur 2 octets
Artiom	0:bc74da1c502f	51	msgTx.data[0]=(unsigned char)id; //waitingAckID
Artiom	0:bc74da1c502f	52	msgTx.data[1]=(unsigned char)(id>>8);
Artiom	0:bc74da1c502f	53	can.write(msgTx);
Artiom	0:bc74da1c502f	54	}
Artiom	0:bc74da1c502f	55	/*********************************************************************************************/
Artiom	0:bc74da1c502f	56	/* FUNCTION NAME: SendRawId */
Artiom	0:bc74da1c502f	57	/* DESCRIPTION : */
Artiom	0:bc74da1c502f	58	/*********************************************************************************************/
Artiom	0:bc74da1c502f	59	void SendRawId (unsigned short id)
Artiom	0:bc74da1c502f	60	{
Artiom	0:bc74da1c502f	61	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	62	msgTx.id=id;
Artiom	0:bc74da1c502f	63	msgTx.len=0;
Artiom	0:bc74da1c502f	64	can.write(msgTx); //carte esclave f446re
Artiom	0:bc74da1c502f	65	wait_us(200);
Artiom	0:bc74da1c502f	66	}
Artiom	0:bc74da1c502f	67
Artiom	0:bc74da1c502f	68	/*********************************************************************************************/
Artiom	0:bc74da1c502f	69	/* FUNCTION NAME: GoStraight */
Artiom	0:bc74da1c502f	70	/* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Artiom	0:bc74da1c502f	71	/* recalage : 0 => pas de recalage */
Artiom	0:bc74da1c502f	72	/* 1 => recalage en X */
Artiom	0:bc74da1c502f	73	/* 2 => Recalage en Y */
Artiom	0:bc74da1c502f	74	/* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Artiom	0:bc74da1c502f	75	/* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Artiom	0:bc74da1c502f	76	/* 0 => non */
Artiom	0:bc74da1c502f	77	/* 1 => oui */
Artiom	0:bc74da1c502f	78	/* 2 => dernière instruction de l'enchainement */
Artiom	0:bc74da1c502f	79	/*********************************************************************************************/
Artiom	0:bc74da1c502f	80	void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Artiom	0:bc74da1c502f	81	{
Artiom	0:bc74da1c502f	82	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	83	msgTx.id=ASSERVISSEMENT_RECALAGE;
Artiom	0:bc74da1c502f	84	msgTx.len=6;
Artiom	0:bc74da1c502f	85	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	86	msgTx.type=CANData;
Artiom	0:bc74da1c502f	87	// x sur 2 octets
Artiom	0:bc74da1c502f	88	msgTx.data[0]=(unsigned char)distance;
Artiom	0:bc74da1c502f	89	msgTx.data[1]=(unsigned char)(distance>>8);
Artiom	0:bc74da1c502f	90	//Recalage sur 1 octet
Artiom	0:bc74da1c502f	91	msgTx.data[2]=recalage;
Artiom	0:bc74da1c502f	92	//Valeur du recalage sur 2 octets
Artiom	0:bc74da1c502f	93	msgTx.data[3]=(unsigned char)newValue;
Artiom	0:bc74da1c502f	94	msgTx.data[4]=(unsigned char)(newValue>>8);
Artiom	0:bc74da1c502f	95	//Enchainement sur 1 octet
Artiom	0:bc74da1c502f	96	msgTx.data[5]=isEnchainement;
Artiom	0:bc74da1c502f	97
Artiom	0:bc74da1c502f	98	can.write(msgTx);
Artiom	0:bc74da1c502f	99	//wait_ms(500);
Artiom	0:bc74da1c502f	100	}
Artiom	0:bc74da1c502f	101
Artiom	0:bc74da1c502f	102	int main()
Artiom	0:bc74da1c502f	103	{
Artiom	0:bc74da1c502f	104	can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom	0:bc74da1c502f	105	servo_interrupt_en(); //permettre les interuptions
Artiom	0:bc74da1c502f	106	wait(1);//attente servo boot
Artiom	2:9e63099cca99	107	gabarit_petit_robot();
Artiom	0:bc74da1c502f	108
Artiom	0:bc74da1c502f	109	while(1) {
Artiom	0:bc74da1c502f	110	canProcessRx();
Artiom	0:bc74da1c502f	111
Artiom	2:9e63099cca99	112
Artiom	0:bc74da1c502f	113
Artiom	0:bc74da1c502f	114	if(EtatGameEnd==1) {
Artiom	0:bc74da1c502f	115	while(1);
Artiom	0:bc74da1c502f	116	}
Artiom	0:bc74da1c502f	117
Artiom	0:bc74da1c502f	118	}
Artiom	0:bc74da1c502f	119	}
Artiom	0:bc74da1c502f	120
Artiom	0:bc74da1c502f	121	/****************************************************************************************/
Artiom	0:bc74da1c502f	122	/* FUNCTION NAME: canProcessRx */
Artiom	0:bc74da1c502f	123	/* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom	0:bc74da1c502f	124	/****************************************************************************************/
Artiom	0:bc74da1c502f	125	void canProcessRx(void)
Artiom	0:bc74da1c502f	126	{
Artiom	0:bc74da1c502f	127	static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom	0:bc74da1c502f	128	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	129	FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom	0:bc74da1c502f	130	if(FIFO_occupation<0)
Artiom	0:bc74da1c502f	131	FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom	0:bc74da1c502f	132	if(FIFO_max_occupation<FIFO_occupation)
Artiom	0:bc74da1c502f	133	FIFO_max_occupation=FIFO_occupation;
Artiom	0:bc74da1c502f	134	if(FIFO_occupation!=0) {
Artiom	0:bc74da1c502f	135	int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom	0:bc74da1c502f	136
Artiom	0:bc74da1c502f	137	switch(identifiant) {
Artiom	0:bc74da1c502f	138
Artiom	0:bc74da1c502f	139	case GLOBAL_GAME_END:
Artiom	0:bc74da1c502f	140	EtatGameEnd = 1;
Artiom	0:bc74da1c502f	141	break;
Artiom	0:bc74da1c502f	142
Artiom	0:bc74da1c502f	143	case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom	0:bc74da1c502f	144	char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom	0:bc74da1c502f	145	short distance=lecture_telemetre(numero_telemetre);
Artiom	0:bc74da1c502f	146
Artiom	0:bc74da1c502f	147
Artiom	0:bc74da1c502f	148	msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	149	msgTx.len=2;
Artiom	0:bc74da1c502f	150	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	151	msgTx.type=CANData;
Artiom	0:bc74da1c502f	152	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	153	msgTx.data[0]=(unsigned char)distance;
Artiom	0:bc74da1c502f	154	msgTx.data[1]=(unsigned char)(distance>>8);
Artiom	0:bc74da1c502f	155
Artiom	0:bc74da1c502f	156	can.write(msgTx);
Artiom	0:bc74da1c502f	157	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom	0:bc74da1c502f	158	break;
Artiom	0:bc74da1c502f	159
Artiom	0:bc74da1c502f	160	case DATA_RECALAGE:
Artiom	0:bc74da1c502f	161	short distance1=lecture_telemetre(1);
Artiom	0:bc74da1c502f	162	short distance2=lecture_telemetre(2);
Artiom	0:bc74da1c502f	163	short distance3=lecture_telemetre(3);
Artiom	0:bc74da1c502f	164	short distance4=lecture_telemetre(4);
Artiom	0:bc74da1c502f	165
Artiom	0:bc74da1c502f	166	msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	167	msgTx.len=8;
Artiom	0:bc74da1c502f	168	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	169	msgTx.type=CANData;
Artiom	0:bc74da1c502f	170	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	171	msgTx.data[0]=(unsigned char)distance1;
Artiom	0:bc74da1c502f	172	msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom	0:bc74da1c502f	173	msgTx.data[2]=(unsigned char)distance2;
Artiom	0:bc74da1c502f	174	msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom	0:bc74da1c502f	175	msgTx.data[4]=(unsigned char)distance3;
Artiom	0:bc74da1c502f	176	msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom	0:bc74da1c502f	177	msgTx.data[6]=(unsigned char)distance4;
Artiom	0:bc74da1c502f	178	msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom	0:bc74da1c502f	179	can.write(msgTx);
Artiom	0:bc74da1c502f	180	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom	0:bc74da1c502f	181	break;
Artiom	0:bc74da1c502f	182
Artiom	0:bc74da1c502f	183	#ifdef ROBOT_SMALL
Artiom	2:9e63099cca99	184	case GABARIT_PETIT_ROBOT:
Artiom	2:9e63099cca99	185	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	186	gabarit_petit_robot();
Artiom	2:9e63099cca99	187	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	188	break;
Artiom	0:bc74da1c502f	189
Artiom	0:bc74da1c502f	190	case PRESENTOIR_AVANT:
Artiom	2:9e63099cca99	191	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	0:bc74da1c502f	192	presentoir_avant();
Artiom	2:9e63099cca99	193	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	194	break;
Artiom	0:bc74da1c502f	195
Artiom	0:bc74da1c502f	196	case PRESENTOIR_ARRIERE:
Artiom	2:9e63099cca99	197	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	0:bc74da1c502f	198	presentoir_arriere();
Artiom	2:9e63099cca99	199	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	200	break;
Artiom	0:bc74da1c502f	201
Artiom	0:bc74da1c502f	202	case BALANCE_AVANT:
Artiom	2:9e63099cca99	203	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	0:bc74da1c502f	204	balance_avant();
Artiom	2:9e63099cca99	205	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	206	break;
Artiom	0:bc74da1c502f	207
Artiom	0:bc74da1c502f	208	case BALANCE_ARRIERE:
Artiom	2:9e63099cca99	209	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	0:bc74da1c502f	210	balance_arriere();
Artiom	2:9e63099cca99	211	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	212	break;
Artiom	2:9e63099cca99	213
Artiom	2:9e63099cca99	214	case ACCELERATEUR_AVANT:
Artiom	2:9e63099cca99	215	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	216	accelerateur_avant();
Artiom	2:9e63099cca99	217	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	218	break;
Artiom	2:9e63099cca99	219
Artiom	2:9e63099cca99	220	case ACCELERATEUR_ARRIERE:
Artiom	2:9e63099cca99	221	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	222	accelerateur_arriere();
Artiom	2:9e63099cca99	223	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	224	break;
Artiom	2:9e63099cca99	225	case GOLDENIUM_AVANT:
Artiom	2:9e63099cca99	226	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	227	balance_avant();
Artiom	2:9e63099cca99	228	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	229	break;
Artiom	2:9e63099cca99	230
Artiom	2:9e63099cca99	231	case GOLDENIUM_ARRIERE:
Artiom	2:9e63099cca99	232	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	233	balance_arriere();
Artiom	2:9e63099cca99	234	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	235	break;
Artiom	2:9e63099cca99	236	case SOL_AVANT:
Artiom	2:9e63099cca99	237	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	238	sol_avant();
Artiom	2:9e63099cca99	239	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	240	break;
Artiom	2:9e63099cca99	241
Artiom	2:9e63099cca99	242	case SOL_ARRIERE:
Artiom	2:9e63099cca99	243	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	244	sol_arriere();
Artiom	2:9e63099cca99	245	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	246	break;
Artiom	2:9e63099cca99	247
Artiom	2:9e63099cca99	248	case SOL_AVANT_RELACHE:
Artiom	2:9e63099cca99	249	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	250	sol_avant_relache();
Artiom	2:9e63099cca99	251	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	2:9e63099cca99	252	break;
Artiom	2:9e63099cca99	253
Artiom	2:9e63099cca99	254	case SOL_ARRIERE_RELACHE:
Artiom	2:9e63099cca99	255	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	256	sol_arriere_relache();
Artiom	2:9e63099cca99	257	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	258	break;
Artiom	0:bc74da1c502f	259
Artiom	0:bc74da1c502f	260	#endif
Artiom	0:bc74da1c502f	261	#ifdef ROBOT_BIG
Artiom	0:bc74da1c502f	262	case 3:
Artiom	0:bc74da1c502f	263
Artiom	0:bc74da1c502f	264	break;
Artiom	0:bc74da1c502f	265
Artiom	0:bc74da1c502f	266	case 5:
Artiom	0:bc74da1c502f	267
Artiom	0:bc74da1c502f	268	break;
Artiom	0:bc74da1c502f	269
Artiom	0:bc74da1c502f	270	#endif
Artiom	0:bc74da1c502f	271	}
Artiom	0:bc74da1c502f	272	FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	273
Artiom	0:bc74da1c502f	274	}
Artiom	0:bc74da1c502f	275	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	10 May 2019
Imports:	11
Forks:	0
Commits:	45
Dependents:	0
Dependencies:	5
Followers:	96

main.cpp@2:9e63099cca99, 2019-05-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning