carte_esclave_PETIT_ROBOT_2019 - carte esclave Petit Robot

CRAC Team » Code » carte_esclave_PETIT_ROBOT_2019

CRAC Team / Mbed 2 deprecated carte_esclave_PETIT_ROBOT_2019

carte esclave Petit Robot

Dependencies: mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp@27:d9c3af6b14f6, 2019-05-22 (annotated)

Committer:: Artiom
Date:: Wed May 22 21:29:48 2019 +0000
Revision:: 27:d9c3af6b14f6
Parent:: 23:93a427b242ce

Who changed what in which revision?

User	Revision	Line number	New contents of line
Artiom	0:bc74da1c502f	1	#include "mbed.h"
Artiom	1:568955af8c2b	2	#include "main.h"
Artiom	0:bc74da1c502f	3
Artiom	0:bc74da1c502f	4	#define SIZE_FIFO 50
Artiom	0:bc74da1c502f	5
Artiom	0:bc74da1c502f	6	CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom	0:bc74da1c502f	7
kyxstark	22:9e92a2b941ba	8	Serial pc(USBTX,USBRX);
Artiom	0:bc74da1c502f	9
Artiom	0:bc74da1c502f	10	CANMessage msgRxBuffer[SIZE_FIFO];
Artiom	0:bc74da1c502f	11	unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom	0:bc74da1c502f	12	signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom	0:bc74da1c502f	13	unsigned char EtatGameEnd=0;
Artiom	0:bc74da1c502f	14
kyxstark	21:edf6d03fdb20	15
kyxstark	21:edf6d03fdb20	16	int rotation_couroies = 0;
kyxstark	21:edf6d03fdb20	17
kyxstark	21:edf6d03fdb20	18
kyxstark	21:edf6d03fdb20	19	void canProcessRx(void);
Artiom	12:191486ba5118	20
Artiom	12:191486ba5118	21
Artiom	13:9e19048eb065	22
Artiom	4:4a79942713fa	23
kyxstark	21:edf6d03fdb20	24	#ifdef ROBOT_BIG
kyxstark	21:edf6d03fdb20	25	#endif
Artiom	14:b1abd2f6df75	26
Artiom	0:bc74da1c502f	27	/*********************************************************************************************/
Artiom	0:bc74da1c502f	28	/* FUNCTION NAME: canRx_ISR */
Artiom	0:bc74da1c502f	29	/* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom	0:bc74da1c502f	30	/*********************************************************************************************/
Artiom	0:bc74da1c502f	31	void canRx_ISR (void)
Artiom	0:bc74da1c502f	32	{
Artiom	0:bc74da1c502f	33	if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom	0:bc74da1c502f	34	if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom	0:bc74da1c502f	35
Artiom	0:bc74da1c502f	36	}
Artiom	0:bc74da1c502f	37	FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	38
Artiom	0:bc74da1c502f	39	}
Artiom	0:bc74da1c502f	40
Artiom	0:bc74da1c502f	41	}
Artiom	0:bc74da1c502f	42
Artiom	0:bc74da1c502f	43	int main()
Artiom	0:bc74da1c502f	44	{
Artiom	0:bc74da1c502f	45	can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom	0:bc74da1c502f	46	servo_interrupt_en(); //permettre les interuptions
Artiom	11:6c11c081c855	47
Artiom	0:bc74da1c502f	48	wait(1);//attente servo boot
Artiom	27:d9c3af6b14f6	49
Artiom	27:d9c3af6b14f6	50
kyxstark	21:edf6d03fdb20	51	#ifdef ROBOT_SMALL
kyxstark	22:9e92a2b941ba	52	pc.printf("\nPetit robot\n\n");
Artiom	2:9e63099cca99	53	gabarit_petit_robot();
kyxstark	21:edf6d03fdb20	54	#endif
kyxstark	21:edf6d03fdb20	55
kyxstark	21:edf6d03fdb20	56
Artiom	27:d9c3af6b14f6	57
kyxstark	21:edf6d03fdb20	58	#ifdef ROBOT_BIG
kyxstark	22:9e92a2b941ba	59	pc.printf("\nGros robot\n\n");
kyxstark	21:edf6d03fdb20	60	clear(doigt,2);
kyxstark	21:edf6d03fdb20	61	setTorque(doigt, TORQUE_ON,2);
kyxstark	21:edf6d03fdb20	62	positionControl(doigt,384,100,BLED_ON,2);
kyxstark	21:edf6d03fdb20	63	wait(1.0);
kyxstark	21:edf6d03fdb20	64	#endif
kyxstark	21:edf6d03fdb20	65
kyxstark	21:edf6d03fdb20	66
kyxstark	22:9e92a2b941ba	67	pc.printf("\nLAUNCHED");
Artiom	0:bc74da1c502f	68	while(1) {
Artiom	0:bc74da1c502f	69	canProcessRx();
Artiom	10:48c0d1b8aed9	70	f_mesure();//dt35
Artiom	11:6c11c081c855	71
kyxstark	23:93a427b242ce	72	automate_ventouse_presentoir_avant();
kyxstark	23:93a427b242ce	73	automate_ventouse_goldenium_avant();
kyxstark	23:93a427b242ce	74	automate_ventouse_relache_avant();
kyxstark	23:93a427b242ce	75	automate_ventouse_accelerateur_avant();
kyxstark	21:edf6d03fdb20	76
kyxstark	21:edf6d03fdb20	77	#ifdef ROBOT_SMALL
Artiom	5:6e198cdd99ad	78	automate_ventouse_presentoir_arriere();
Artiom	7:553f3f1c2c53	79
Artiom	6:45f9cf44718a	80	automate_ventouse_goldenium_arriere();
Artiom	7:553f3f1c2c53	81
Artiom	6:45f9cf44718a	82	automate_ventouse_sol_avant();
Artiom	7:553f3f1c2c53	83	automate_ventouse_sol_arriere();
Artiom	7:553f3f1c2c53	84
Artiom	12:191486ba5118	85	automate_ventouse_balance_arriere();
Artiom	12:191486ba5118	86
Artiom	12:191486ba5118	87	automate_ventouse_relache_arriere();
Artiom	14:b1abd2f6df75	88
Artiom	13:9e19048eb065	89	automate_ventouse_sol_avant_relache();
Artiom	14:b1abd2f6df75	90	automate_ventouse_sol_arriere_relache();
Artiom	14:b1abd2f6df75	91
Artiom	14:b1abd2f6df75	92	automate_ventouse_accelerateur_arriere();
kyxstark	21:edf6d03fdb20	93	#endif
kyxstark	21:edf6d03fdb20	94
kyxstark	21:edf6d03fdb20	95	#ifdef ROBOT_BIG
kyxstark	21:edf6d03fdb20	96	fifo_couleur();
kyxstark	21:edf6d03fdb20	97	ascenseur();
kyxstark	21:edf6d03fdb20	98
kyxstark	21:edf6d03fdb20	99	#endif
Artiom	13:9e19048eb065	100
Artiom	0:bc74da1c502f	101	if(EtatGameEnd==1) {
Artiom	0:bc74da1c502f	102	while(1);
Artiom	0:bc74da1c502f	103	}
Artiom	0:bc74da1c502f	104
Artiom	0:bc74da1c502f	105	}
Artiom	0:bc74da1c502f	106	}
Artiom	0:bc74da1c502f	107
Artiom	0:bc74da1c502f	108	/****************************************************************************************/
Artiom	0:bc74da1c502f	109	/* FUNCTION NAME: canProcessRx */
Artiom	0:bc74da1c502f	110	/* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom	0:bc74da1c502f	111	/****************************************************************************************/
Artiom	0:bc74da1c502f	112	void canProcessRx(void)
Artiom	0:bc74da1c502f	113	{
Artiom	0:bc74da1c502f	114	static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom	0:bc74da1c502f	115	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	116	FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom	0:bc74da1c502f	117	if(FIFO_occupation<0)
Artiom	0:bc74da1c502f	118	FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom	0:bc74da1c502f	119	if(FIFO_max_occupation<FIFO_occupation)
Artiom	0:bc74da1c502f	120	FIFO_max_occupation=FIFO_occupation;
Artiom	0:bc74da1c502f	121	if(FIFO_occupation!=0) {
Artiom	0:bc74da1c502f	122	int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom	0:bc74da1c502f	123
Artiom	0:bc74da1c502f	124	switch(identifiant) {
Artiom	0:bc74da1c502f	125
Artiom	0:bc74da1c502f	126	case GLOBAL_GAME_END:
Artiom	0:bc74da1c502f	127	EtatGameEnd = 1;
Artiom	0:bc74da1c502f	128	break;
Artiom	0:bc74da1c502f	129
Artiom	0:bc74da1c502f	130	case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom	0:bc74da1c502f	131	char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom	0:bc74da1c502f	132	short distance=lecture_telemetre(numero_telemetre);
Artiom	0:bc74da1c502f	133
Artiom	0:bc74da1c502f	134
Artiom	0:bc74da1c502f	135	msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	136	msgTx.len=2;
Artiom	0:bc74da1c502f	137	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	138	msgTx.type=CANData;
Artiom	0:bc74da1c502f	139	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	140	msgTx.data[0]=(unsigned char)distance;
Artiom	0:bc74da1c502f	141	msgTx.data[1]=(unsigned char)(distance>>8);
Artiom	0:bc74da1c502f	142
Artiom	0:bc74da1c502f	143	can.write(msgTx);
Artiom	0:bc74da1c502f	144	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom	0:bc74da1c502f	145	break;
Artiom	0:bc74da1c502f	146
Artiom	0:bc74da1c502f	147	case DATA_RECALAGE:
Artiom	0:bc74da1c502f	148	short distance1=lecture_telemetre(1);
Artiom	0:bc74da1c502f	149	short distance2=lecture_telemetre(2);
Artiom	0:bc74da1c502f	150	short distance3=lecture_telemetre(3);
Artiom	0:bc74da1c502f	151	short distance4=lecture_telemetre(4);
Artiom	0:bc74da1c502f	152
Artiom	0:bc74da1c502f	153	msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	154	msgTx.len=8;
Artiom	0:bc74da1c502f	155	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	156	msgTx.type=CANData;
Artiom	0:bc74da1c502f	157	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	158	msgTx.data[0]=(unsigned char)distance1;
Artiom	0:bc74da1c502f	159	msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom	0:bc74da1c502f	160	msgTx.data[2]=(unsigned char)distance2;
Artiom	0:bc74da1c502f	161	msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom	0:bc74da1c502f	162	msgTx.data[4]=(unsigned char)distance3;
Artiom	0:bc74da1c502f	163	msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom	0:bc74da1c502f	164	msgTx.data[6]=(unsigned char)distance4;
Artiom	0:bc74da1c502f	165	msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom	0:bc74da1c502f	166	can.write(msgTx);
Artiom	0:bc74da1c502f	167	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom	0:bc74da1c502f	168	break;
Artiom	27:d9c3af6b14f6	169
kyxstark	23:93a427b242ce	170	case PRESENTOIR_AVANT:
kyxstark	23:93a427b242ce	171	fpresentoir_avant=1;
kyxstark	23:93a427b242ce	172	break;
kyxstark	23:93a427b242ce	173
kyxstark	23:93a427b242ce	174	case BALANCE_AVANT:
kyxstark	23:93a427b242ce	175	fbalance_avant=1;
kyxstark	23:93a427b242ce	176	break;
kyxstark	23:93a427b242ce	177
kyxstark	23:93a427b242ce	178	case ACCELERATEUR_AVANT:
kyxstark	23:93a427b242ce	179	faccelerateur_avant=1;
kyxstark	23:93a427b242ce	180	break;
kyxstark	23:93a427b242ce	181
kyxstark	23:93a427b242ce	182	case GOLDENIUM_AVANT:
kyxstark	23:93a427b242ce	183	fgoldenium_avant=1;
kyxstark	23:93a427b242ce	184	break;
kyxstark	23:93a427b242ce	185
kyxstark	23:93a427b242ce	186	case VENTOUSE_AV_CENTRE_BALANCE:
kyxstark	23:93a427b242ce	187	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark	23:93a427b242ce	188	goldenium_avant();
kyxstark	23:93a427b242ce	189	verification();
kyxstark	23:93a427b242ce	190	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark	23:93a427b242ce	191	break;
kyxstark	23:93a427b242ce	192
kyxstark	23:93a427b242ce	193	case AVANT_RELACHE:
kyxstark	23:93a427b242ce	194	favant_relache=1;
kyxstark	23:93a427b242ce	195	break;
kyxstark	23:93a427b242ce	196
kyxstark	23:93a427b242ce	197	//--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
kyxstark	23:93a427b242ce	198
kyxstark	23:93a427b242ce	199
kyxstark	23:93a427b242ce	200	case HACHEUR_STATUT_VENTOUSES:
kyxstark	23:93a427b242ce	201	status_pompe = msgRxBuffer[FIFO_lecture].data[1];
kyxstark	23:93a427b242ce	202	//can.write(CANMessage(0x529, &status_pompe,1));
kyxstark	23:93a427b242ce	203	break;
kyxstark	23:93a427b242ce	204
kyxstark	23:93a427b242ce	205	case HACHEUR_GET_ATOM_ACK:
kyxstark	23:93a427b242ce	206	status_pompe \|= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
kyxstark	23:93a427b242ce	207	//can.write(CANMessage(0x529, &status_pompe,1));
kyxstark	23:93a427b242ce	208	break;
kyxstark	23:93a427b242ce	209
kyxstark	23:93a427b242ce	210	case HACHEUR_RELEASE_ATOM_ACK :
kyxstark	23:93a427b242ce	211	status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
kyxstark	23:93a427b242ce	212	break;
kyxstark	23:93a427b242ce	213
kyxstark	23:93a427b242ce	214	//-------------------------------------------------------------------------------------------------------------------------------------------
kyxstark	23:93a427b242ce	215
Artiom	27:d9c3af6b14f6	216
Artiom	0:bc74da1c502f	217
Artiom	0:bc74da1c502f	218	#ifdef ROBOT_SMALL
Artiom	4:4a79942713fa	219	//-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom	2:9e63099cca99	220	case GABARIT_PETIT_ROBOT:
Artiom	2:9e63099cca99	221	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	222	gabarit_petit_robot();
Artiom	2:9e63099cca99	223	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	224	break;
Artiom	0:bc74da1c502f	225
Artiom	0:bc74da1c502f	226	case PRESENTOIR_ARRIERE:
Artiom	5:6e198cdd99ad	227	fpresentoir_arriere=1;
Artiom	0:bc74da1c502f	228	break;
Artiom	0:bc74da1c502f	229
Artiom	0:bc74da1c502f	230	case BALANCE_ARRIERE:
Artiom	12:191486ba5118	231	fbalance_arriere=1;
Artiom	2:9e63099cca99	232	break;
Artiom	2:9e63099cca99	233
Artiom	2:9e63099cca99	234	case ACCELERATEUR_ARRIERE:
Artiom	14:b1abd2f6df75	235	faccelerateur_arriere=1;
Artiom	2:9e63099cca99	236	break;
Artiom	6:45f9cf44718a	237
Artiom	2:9e63099cca99	238	case GOLDENIUM_ARRIERE:
Artiom	6:45f9cf44718a	239	fgoldenium_arriere=1;
Artiom	2:9e63099cca99	240	break;
Artiom	6:45f9cf44718a	241
Artiom	2:9e63099cca99	242	case SOL_AVANT:
Artiom	6:45f9cf44718a	243	fsol_avant=1;
Artiom	2:9e63099cca99	244	break;
Artiom	2:9e63099cca99	245
Artiom	2:9e63099cca99	246	case SOL_ARRIERE:
Artiom	6:45f9cf44718a	247	fsol_arriere=1;
Artiom	2:9e63099cca99	248	break;
Artiom	2:9e63099cca99	249
Artiom	2:9e63099cca99	250	case SOL_AVANT_RELACHE:
Artiom	7:553f3f1c2c53	251	fsol_avant_relache=1;
Artiom	2:9e63099cca99	252	break;
Artiom	2:9e63099cca99	253
Artiom	2:9e63099cca99	254	case SOL_ARRIERE_RELACHE:
Artiom	7:553f3f1c2c53	255	fsol_arriere_relache=1;
Artiom	0:bc74da1c502f	256	break;
Artiom	12:191486ba5118	257
Artiom	12:191486ba5118	258	case ARRIERE_RELACHE:
Artiom	12:191486ba5118	259	farriere_relache=1;
Artiom	12:191486ba5118	260	break;
Artiom	14:b1abd2f6df75	261	case RECROQUEVILLER:
Artiom	14:b1abd2f6df75	262	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	14:b1abd2f6df75	263	recroqueviller_avant();
Artiom	14:b1abd2f6df75	264	recroqueviller_arriere();
Artiom	14:b1abd2f6df75	265	verification();
Artiom	14:b1abd2f6df75	266	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	14:b1abd2f6df75	267	break;
Artiom	13:9e19048eb065	268
Artiom	20:42f8ec726ac8	269	case VENTOUSE_AR_CENTRE_BALANCE:
Artiom	20:42f8ec726ac8	270	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	20:42f8ec726ac8	271	goldenium_arriere();
Artiom	20:42f8ec726ac8	272	positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur
Artiom	20:42f8ec726ac8	273	verification();
Artiom	20:42f8ec726ac8	274	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	20:42f8ec726ac8	275	break;
Artiom	13:9e19048eb065	276
Artiom	27:d9c3af6b14f6	277	case ACCELERATEUR_INSERTION_AVANT_GAUCHE:
Artiom	27:d9c3af6b14f6	278	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	27:d9c3af6b14f6	279	accelerateur_insertion_avant_gauche();
Artiom	27:d9c3af6b14f6	280	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	27:d9c3af6b14f6	281
Artiom	27:d9c3af6b14f6	282	break;
Artiom	27:d9c3af6b14f6	283
Artiom	27:d9c3af6b14f6	284	case ACCELERATEUR_INSERTION_ARRIERE_GAUCHE:
Artiom	27:d9c3af6b14f6	285	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	27:d9c3af6b14f6	286	accelerateur_insertion_arriere_gauche();
Artiom	27:d9c3af6b14f6	287	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	27:d9c3af6b14f6	288	break;
Artiom	27:d9c3af6b14f6	289
Artiom	0:bc74da1c502f	290
Artiom	0:bc74da1c502f	291	#endif
kyxstark	23:93a427b242ce	292
Artiom	0:bc74da1c502f	293	#ifdef ROBOT_BIG
kyxstark	21:edf6d03fdb20	294	case ASCENSEUR:
kyxstark	22:9e92a2b941ba	295	flag_ascenseur = msgRxBuffer[FIFO_lecture].data[0];
Artiom	0:bc74da1c502f	296	break;
Artiom	0:bc74da1c502f	297
Artiom	0:bc74da1c502f	298	#endif
Artiom	0:bc74da1c502f	299	}
Artiom	0:bc74da1c502f	300	FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	301
Artiom	0:bc74da1c502f	302	}
Artiom	4:4a79942713fa	303
Artiom	4:4a79942713fa	304
Artiom	4:4a79942713fa	305	}
Artiom	4:4a79942713fa	306
Artiom	4:4a79942713fa	307
Artiom	4:4a79942713fa	308
Artiom	11:6c11c081c855	309

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	31 May 2019
Imports:	4
Forks:	0
Commits:	47
Dependents:	0
Dependencies:	5
Followers:	96

main.cpp@27:d9c3af6b14f6, 2019-05-22 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning