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@39:124575240d09, 2019-05-23 (annotated)

Committer:: kyxstark
Date:: Thu May 23 18:42:51 2019 +0000
Revision:: 39:124575240d09
Parent:: 38:9295a9721acb
Child:: 41:a71d06f292ac

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

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@39:124575240d09, 2019-05-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning