ACRAC_carte_esclave_GROS_ROBOT_2019 - Carte esclave gros robot

CRAC Team » Code » ACRAC_carte_esclave_GROS_ROBOT_2019

CRAC Team / Mbed 2 deprecated ACRAC_carte_esclave_GROS_ROBOT_2019

Carte esclave gros robot

Dependencies: mbed Herkulex_Library_2019 ident_crac actions_Pr

main.cpp@31:29500874c1cc, 2019-05-23 (annotated)

Committer:: Artiom
Date:: Thu May 23 07:53:43 2019 +0000
Revision:: 31:29500874c1cc
Parent:: 30:d034f469359e
Child:: 32:e9947815c4d7

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

main.cpp@31:29500874c1cc, 2019-05-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning