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@16:2a66cfe37262, 2019-05-21 (annotated)

Committer:: Artiom
Date:: Tue May 21 15:27:07 2019 +0000
Revision:: 16:2a66cfe37262
Parent:: 15:119fc452dcaf
Child:: 17:b8ffd3593d0e
Child:: 18:0ea0d887966f

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	9:9833e788942b	7	#include "dt.h"
Artiom	0:bc74da1c502f	8
Artiom	0:bc74da1c502f	9	#define SIZE_FIFO 50
Artiom	0:bc74da1c502f	10
Artiom	0:bc74da1c502f	11	CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN
Artiom	0:bc74da1c502f	12
Artiom	0:bc74da1c502f	13
Artiom	0:bc74da1c502f	14	CANMessage msgRxBuffer[SIZE_FIFO];
Artiom	0:bc74da1c502f	15	unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Artiom	0:bc74da1c502f	16	signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Artiom	0:bc74da1c502f	17	unsigned char EtatGameEnd=0;
Artiom	0:bc74da1c502f	18
Artiom	6:45f9cf44718a	19	char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom	14:b1abd2f6df75	20	char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom	6:45f9cf44718a	21	char fsol_avant=0,fsol_arriere=0;
Artiom	7:553f3f1c2c53	22	char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom	12:191486ba5118	23	char fbalance_avant=0,fbalance_arriere=0;
Artiom	12:191486ba5118	24	char favant_relache=0,farriere_relache=0;
Artiom	14:b1abd2f6df75	25	char faccelerateur_avant=0,faccelerateur_arriere=0;;
Artiom	12:191486ba5118	26
Artiom	12:191486ba5118	27
Artiom	13:9e19048eb065	28
Artiom	4:4a79942713fa	29	char status_pompe=0;
Artiom	4:4a79942713fa	30
Artiom	0:bc74da1c502f	31	void canRx_ISR (void);
Artiom	0:bc74da1c502f	32	void SendAck(unsigned short id, unsigned short from);
Artiom	0:bc74da1c502f	33	void SendRawId (unsigned short id);
Artiom	0:bc74da1c502f	34	void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
Artiom	0:bc74da1c502f	35	void canProcessRx(void);
Artiom	6:45f9cf44718a	36
Artiom	4:4a79942713fa	37	void automate_ventouse_presentoir_avant(void);
Artiom	5:6e198cdd99ad	38	void automate_ventouse_presentoir_arriere (void);
Artiom	0:bc74da1c502f	39
Artiom	6:45f9cf44718a	40	void automate_ventouse_goldenium_avant (void);
Artiom	6:45f9cf44718a	41	void automate_ventouse_goldenium_arriere (void);
Artiom	6:45f9cf44718a	42
Artiom	6:45f9cf44718a	43	void automate_ventouse_sol_avant (void);
Artiom	6:45f9cf44718a	44	void automate_ventouse_sol_arriere(void);
Artiom	6:45f9cf44718a	45
Artiom	7:553f3f1c2c53	46	void automate_ventouse_sol_avant_relache(void);
Artiom	7:553f3f1c2c53	47	void automate_ventouse_sol_arriere_relache(void);
Artiom	11:6c11c081c855	48
Artiom	11:6c11c081c855	49	void automate_ventouse_balance_avant (void);
Artiom	12:191486ba5118	50	void automate_ventouse_balance_arriere(void);
Artiom	12:191486ba5118	51
Artiom	13:9e19048eb065	52	void automate_ventouse_relache_avant(void);
Artiom	13:9e19048eb065	53	void automate_ventouse_relache_arriere (void);
Artiom	12:191486ba5118	54
Artiom	14:b1abd2f6df75	55	void automate_ventouse_accelerateur_avant(void);
Artiom	14:b1abd2f6df75	56	void automate_ventouse_accelerateur_arriere(void);
Artiom	14:b1abd2f6df75	57
Artiom	14:b1abd2f6df75	58
Artiom	0:bc74da1c502f	59	/*********************************************************************************************/
Artiom	0:bc74da1c502f	60	/* FUNCTION NAME: canRx_ISR */
Artiom	0:bc74da1c502f	61	/* DESCRIPTION : lit les messages sur le can et les stocke dans la FIFO */
Artiom	0:bc74da1c502f	62	/*********************************************************************************************/
Artiom	0:bc74da1c502f	63	void canRx_ISR (void)
Artiom	0:bc74da1c502f	64	{
Artiom	0:bc74da1c502f	65	if (can.read(msgRxBuffer[FIFO_ecriture])) {
Artiom	0:bc74da1c502f	66	if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {
Artiom	0:bc74da1c502f	67
Artiom	0:bc74da1c502f	68	}
Artiom	0:bc74da1c502f	69	FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	70
Artiom	0:bc74da1c502f	71	}
Artiom	0:bc74da1c502f	72
Artiom	0:bc74da1c502f	73	}
Artiom	0:bc74da1c502f	74	/*********************************************************************************************/
Artiom	0:bc74da1c502f	75	/* FUNCTION NAME: SendAck */
Artiom	0:bc74da1c502f	76	/* DESCRIPTION : */
Artiom	0:bc74da1c502f	77	/*********************************************************************************************/
Artiom	0:bc74da1c502f	78	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	79	{
Artiom	0:bc74da1c502f	80	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	81	msgTx.id=from; //waitingAckFrom
Artiom	0:bc74da1c502f	82	msgTx.len=2;
Artiom	0:bc74da1c502f	83	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	84	msgTx.type=CANData;
Artiom	0:bc74da1c502f	85	// from sur 2 octets
Artiom	0:bc74da1c502f	86	msgTx.data[0]=(unsigned char)id; //waitingAckID
Artiom	0:bc74da1c502f	87	msgTx.data[1]=(unsigned char)(id>>8);
Artiom	0:bc74da1c502f	88	can.write(msgTx);
Artiom	0:bc74da1c502f	89	}
Artiom	0:bc74da1c502f	90	/*********************************************************************************************/
Artiom	0:bc74da1c502f	91	/* FUNCTION NAME: SendRawId */
Artiom	0:bc74da1c502f	92	/* DESCRIPTION : */
Artiom	0:bc74da1c502f	93	/*********************************************************************************************/
Artiom	0:bc74da1c502f	94	void SendRawId (unsigned short id)
Artiom	0:bc74da1c502f	95	{
Artiom	0:bc74da1c502f	96	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	97	msgTx.id=id;
Artiom	0:bc74da1c502f	98	msgTx.len=0;
Artiom	0:bc74da1c502f	99	can.write(msgTx); //carte esclave f446re
Artiom	0:bc74da1c502f	100	wait_us(200);
Artiom	0:bc74da1c502f	101	}
Artiom	0:bc74da1c502f	102
Artiom	0:bc74da1c502f	103	/*********************************************************************************************/
Artiom	0:bc74da1c502f	104	/* FUNCTION NAME: GoStraight */
Artiom	0:bc74da1c502f	105	/* DESCRIPTION : Transmission CAN correspondant à une ligne droite, avec ou sans recalage */
Artiom	0:bc74da1c502f	106	/* recalage : 0 => pas de recalage */
Artiom	0:bc74da1c502f	107	/* 1 => recalage en X */
Artiom	0:bc74da1c502f	108	/* 2 => Recalage en Y */
Artiom	0:bc74da1c502f	109	/* newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo */
Artiom	0:bc74da1c502f	110	/* isEnchainement : Indique si il faut executer l'instruction en enchainement */
Artiom	0:bc74da1c502f	111	/* 0 => non */
Artiom	0:bc74da1c502f	112	/* 1 => oui */
Artiom	0:bc74da1c502f	113	/* 2 => dernière instruction de l'enchainement */
Artiom	0:bc74da1c502f	114	/*********************************************************************************************/
Artiom	0:bc74da1c502f	115	void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
Artiom	0:bc74da1c502f	116	{
Artiom	0:bc74da1c502f	117	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	118	msgTx.id=ASSERVISSEMENT_RECALAGE;
Artiom	0:bc74da1c502f	119	msgTx.len=6;
Artiom	0:bc74da1c502f	120	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	121	msgTx.type=CANData;
Artiom	0:bc74da1c502f	122	// x sur 2 octets
Artiom	0:bc74da1c502f	123	msgTx.data[0]=(unsigned char)distance;
Artiom	0:bc74da1c502f	124	msgTx.data[1]=(unsigned char)(distance>>8);
Artiom	0:bc74da1c502f	125	//Recalage sur 1 octet
Artiom	0:bc74da1c502f	126	msgTx.data[2]=recalage;
Artiom	0:bc74da1c502f	127	//Valeur du recalage sur 2 octets
Artiom	0:bc74da1c502f	128	msgTx.data[3]=(unsigned char)newValue;
Artiom	0:bc74da1c502f	129	msgTx.data[4]=(unsigned char)(newValue>>8);
Artiom	0:bc74da1c502f	130	//Enchainement sur 1 octet
Artiom	0:bc74da1c502f	131	msgTx.data[5]=isEnchainement;
Artiom	0:bc74da1c502f	132
Artiom	0:bc74da1c502f	133	can.write(msgTx);
Artiom	0:bc74da1c502f	134	//wait_ms(500);
Artiom	0:bc74da1c502f	135	}
Artiom	12:191486ba5118	136	/****************************************************************************************/
Artiom	12:191486ba5118	137	/* FUNCTION NAME: Rotate */
Artiom	12:191486ba5118	138	/* DESCRIPTION : Transmission CAN correspondant à une rotation */
Artiom	12:191486ba5118	139	/****************************************************************************************/
Artiom	12:191486ba5118	140	void Rotate (signed short angle)
Artiom	12:191486ba5118	141	{
Artiom	12:191486ba5118	142	CANMessage msgTx=CANMessage();
Artiom	12:191486ba5118	143	msgTx.id=ASSERVISSEMENT_ROTATION; // Tx rotation autour du centre du robot
Artiom	12:191486ba5118	144	msgTx.len=2;
Artiom	12:191486ba5118	145	msgTx.format=CANStandard;
Artiom	12:191486ba5118	146	msgTx.type=CANData;
Artiom	12:191486ba5118	147	// Angle signé sur 2 octets
Artiom	12:191486ba5118	148	msgTx.data[0]=(unsigned char)angle;
Artiom	12:191486ba5118	149	msgTx.data[1]=(unsigned char)(angle>>8);
Artiom	12:191486ba5118	150
Artiom	12:191486ba5118	151	can.write(msgTx);
Artiom	12:191486ba5118	152	}
Artiom	0:bc74da1c502f	153
Artiom	0:bc74da1c502f	154	int main()
Artiom	0:bc74da1c502f	155	{
Artiom	0:bc74da1c502f	156	can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
Artiom	0:bc74da1c502f	157	servo_interrupt_en(); //permettre les interuptions
Artiom	11:6c11c081c855	158
Artiom	0:bc74da1c502f	159	wait(1);//attente servo boot
Artiom	2:9e63099cca99	160	gabarit_petit_robot();
Artiom	0:bc74da1c502f	161	while(1) {
Artiom	0:bc74da1c502f	162	canProcessRx();
Artiom	10:48c0d1b8aed9	163	f_mesure();//dt35
Artiom	11:6c11c081c855	164
Artiom	4:4a79942713fa	165	automate_ventouse_presentoir_avant();
Artiom	5:6e198cdd99ad	166	automate_ventouse_presentoir_arriere();
Artiom	7:553f3f1c2c53	167
Artiom	6:45f9cf44718a	168	automate_ventouse_goldenium_avant();
Artiom	6:45f9cf44718a	169	automate_ventouse_goldenium_arriere();
Artiom	7:553f3f1c2c53	170
Artiom	6:45f9cf44718a	171	automate_ventouse_sol_avant();
Artiom	7:553f3f1c2c53	172	automate_ventouse_sol_arriere();
Artiom	7:553f3f1c2c53	173
Artiom	11:6c11c081c855	174	automate_ventouse_balance_avant();
Artiom	12:191486ba5118	175	automate_ventouse_balance_arriere();
Artiom	12:191486ba5118	176
Artiom	13:9e19048eb065	177	automate_ventouse_relache_avant();
Artiom	12:191486ba5118	178	automate_ventouse_relache_arriere();
Artiom	14:b1abd2f6df75	179
Artiom	13:9e19048eb065	180	automate_ventouse_sol_avant_relache();
Artiom	14:b1abd2f6df75	181	automate_ventouse_sol_arriere_relache();
Artiom	14:b1abd2f6df75	182
Artiom	14:b1abd2f6df75	183	automate_ventouse_accelerateur_avant();
Artiom	14:b1abd2f6df75	184	automate_ventouse_accelerateur_arriere();
Artiom	13:9e19048eb065	185
Artiom	0:bc74da1c502f	186	if(EtatGameEnd==1) {
Artiom	0:bc74da1c502f	187	while(1);
Artiom	0:bc74da1c502f	188	}
Artiom	0:bc74da1c502f	189
Artiom	0:bc74da1c502f	190	}
Artiom	0:bc74da1c502f	191	}
Artiom	0:bc74da1c502f	192
Artiom	0:bc74da1c502f	193	/****************************************************************************************/
Artiom	0:bc74da1c502f	194	/* FUNCTION NAME: canProcessRx */
Artiom	0:bc74da1c502f	195	/* DESCRIPTION : Fonction de traitement des messages CAN */
Artiom	0:bc74da1c502f	196	/****************************************************************************************/
Artiom	0:bc74da1c502f	197	void canProcessRx(void)
Artiom	0:bc74da1c502f	198	{
Artiom	0:bc74da1c502f	199	static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Artiom	0:bc74da1c502f	200	CANMessage msgTx=CANMessage();
Artiom	0:bc74da1c502f	201	FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom	0:bc74da1c502f	202	if(FIFO_occupation<0)
Artiom	0:bc74da1c502f	203	FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom	0:bc74da1c502f	204	if(FIFO_max_occupation<FIFO_occupation)
Artiom	0:bc74da1c502f	205	FIFO_max_occupation=FIFO_occupation;
Artiom	0:bc74da1c502f	206	if(FIFO_occupation!=0) {
Artiom	0:bc74da1c502f	207	int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom	0:bc74da1c502f	208
Artiom	0:bc74da1c502f	209	switch(identifiant) {
Artiom	0:bc74da1c502f	210
Artiom	0:bc74da1c502f	211	case GLOBAL_GAME_END:
Artiom	0:bc74da1c502f	212	EtatGameEnd = 1;
Artiom	0:bc74da1c502f	213	break;
Artiom	0:bc74da1c502f	214
Artiom	0:bc74da1c502f	215	case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
Artiom	0:bc74da1c502f	216	char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
Artiom	0:bc74da1c502f	217	short distance=lecture_telemetre(numero_telemetre);
Artiom	0:bc74da1c502f	218
Artiom	0:bc74da1c502f	219
Artiom	0:bc74da1c502f	220	msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	221	msgTx.len=2;
Artiom	0:bc74da1c502f	222	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	223	msgTx.type=CANData;
Artiom	0:bc74da1c502f	224	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	225	msgTx.data[0]=(unsigned char)distance;
Artiom	0:bc74da1c502f	226	msgTx.data[1]=(unsigned char)(distance>>8);
Artiom	0:bc74da1c502f	227
Artiom	0:bc74da1c502f	228	can.write(msgTx);
Artiom	0:bc74da1c502f	229	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
Artiom	0:bc74da1c502f	230	break;
Artiom	0:bc74da1c502f	231
Artiom	0:bc74da1c502f	232	case DATA_RECALAGE:
Artiom	0:bc74da1c502f	233	short distance1=lecture_telemetre(1);
Artiom	0:bc74da1c502f	234	short distance2=lecture_telemetre(2);
Artiom	0:bc74da1c502f	235	short distance3=lecture_telemetre(3);
Artiom	0:bc74da1c502f	236	short distance4=lecture_telemetre(4);
Artiom	0:bc74da1c502f	237
Artiom	0:bc74da1c502f	238	msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
Artiom	0:bc74da1c502f	239	msgTx.len=8;
Artiom	0:bc74da1c502f	240	msgTx.format=CANStandard;
Artiom	0:bc74da1c502f	241	msgTx.type=CANData;
Artiom	0:bc74da1c502f	242	// Rayon sur 2 octets
Artiom	0:bc74da1c502f	243	msgTx.data[0]=(unsigned char)distance1;
Artiom	0:bc74da1c502f	244	msgTx.data[1]=(unsigned char)(distance1>>8);
Artiom	0:bc74da1c502f	245	msgTx.data[2]=(unsigned char)distance2;
Artiom	0:bc74da1c502f	246	msgTx.data[3]=(unsigned char)(distance2>>8);
Artiom	0:bc74da1c502f	247	msgTx.data[4]=(unsigned char)distance3;
Artiom	0:bc74da1c502f	248	msgTx.data[5]=(unsigned char)(distance3>>8);
Artiom	0:bc74da1c502f	249	msgTx.data[6]=(unsigned char)distance4;
Artiom	0:bc74da1c502f	250	msgTx.data[7]=(unsigned char)(distance4>>8);
Artiom	0:bc74da1c502f	251	can.write(msgTx);
Artiom	0:bc74da1c502f	252	SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
Artiom	0:bc74da1c502f	253	break;
Artiom	0:bc74da1c502f	254
Artiom	0:bc74da1c502f	255	#ifdef ROBOT_SMALL
Artiom	4:4a79942713fa	256	//-------------------------------------------------------------------------Actions petit robot----------------------------------------------
Artiom	2:9e63099cca99	257	case GABARIT_PETIT_ROBOT:
Artiom	2:9e63099cca99	258	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	2:9e63099cca99	259	gabarit_petit_robot();
Artiom	2:9e63099cca99	260	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	0:bc74da1c502f	261	break;
Artiom	0:bc74da1c502f	262
Artiom	0:bc74da1c502f	263	case PRESENTOIR_AVANT:
Artiom	4:4a79942713fa	264	fpresentoir_avant=1;
Artiom	0:bc74da1c502f	265	break;
Artiom	0:bc74da1c502f	266
Artiom	0:bc74da1c502f	267	case PRESENTOIR_ARRIERE:
Artiom	5:6e198cdd99ad	268	fpresentoir_arriere=1;
Artiom	0:bc74da1c502f	269	break;
Artiom	0:bc74da1c502f	270
Artiom	0:bc74da1c502f	271	case BALANCE_AVANT:
Artiom	11:6c11c081c855	272	fbalance_avant=1;
Artiom	0:bc74da1c502f	273	break;
Artiom	0:bc74da1c502f	274
Artiom	0:bc74da1c502f	275	case BALANCE_ARRIERE:
Artiom	12:191486ba5118	276	fbalance_arriere=1;
Artiom	2:9e63099cca99	277	break;
Artiom	2:9e63099cca99	278
Artiom	2:9e63099cca99	279	case ACCELERATEUR_AVANT:
Artiom	14:b1abd2f6df75	280	faccelerateur_avant=1;
Artiom	2:9e63099cca99	281	break;
Artiom	2:9e63099cca99	282
Artiom	2:9e63099cca99	283	case ACCELERATEUR_ARRIERE:
Artiom	14:b1abd2f6df75	284	faccelerateur_arriere=1;
Artiom	2:9e63099cca99	285	break;
Artiom	6:45f9cf44718a	286
Artiom	2:9e63099cca99	287	case GOLDENIUM_AVANT:
Artiom	6:45f9cf44718a	288	fgoldenium_avant=1;
Artiom	2:9e63099cca99	289	break;
Artiom	2:9e63099cca99	290
Artiom	2:9e63099cca99	291	case GOLDENIUM_ARRIERE:
Artiom	6:45f9cf44718a	292	fgoldenium_arriere=1;
Artiom	2:9e63099cca99	293	break;
Artiom	6:45f9cf44718a	294
Artiom	2:9e63099cca99	295	case SOL_AVANT:
Artiom	6:45f9cf44718a	296	fsol_avant=1;
Artiom	2:9e63099cca99	297	break;
Artiom	2:9e63099cca99	298
Artiom	2:9e63099cca99	299	case SOL_ARRIERE:
Artiom	6:45f9cf44718a	300	fsol_arriere=1;
Artiom	2:9e63099cca99	301	break;
Artiom	2:9e63099cca99	302
Artiom	2:9e63099cca99	303	case SOL_AVANT_RELACHE:
Artiom	7:553f3f1c2c53	304	fsol_avant_relache=1;
Artiom	2:9e63099cca99	305	break;
Artiom	2:9e63099cca99	306
Artiom	2:9e63099cca99	307	case SOL_ARRIERE_RELACHE:
Artiom	7:553f3f1c2c53	308	fsol_arriere_relache=1;
Artiom	0:bc74da1c502f	309	break;
Artiom	12:191486ba5118	310	case AVANT_RELACHE:
Artiom	12:191486ba5118	311	favant_relache=1;
Artiom	12:191486ba5118	312	break;
Artiom	12:191486ba5118	313
Artiom	12:191486ba5118	314	case ARRIERE_RELACHE:
Artiom	12:191486ba5118	315	farriere_relache=1;
Artiom	12:191486ba5118	316	break;
Artiom	14:b1abd2f6df75	317	case RECROQUEVILLER:
Artiom	14:b1abd2f6df75	318	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	14:b1abd2f6df75	319	recroqueviller_avant();
Artiom	14:b1abd2f6df75	320	recroqueviller_arriere();
Artiom	14:b1abd2f6df75	321	verification();
Artiom	14:b1abd2f6df75	322	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	14:b1abd2f6df75	323	break;
Artiom	13:9e19048eb065	324
Artiom	13:9e19048eb065	325
Artiom	4:4a79942713fa	326	//--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom	4:4a79942713fa	327
Artiom	4:4a79942713fa	328
Artiom	4:4a79942713fa	329	case HACHEUR_STATUT_VENTOUSES:
Artiom	4:4a79942713fa	330	status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom	5:6e198cdd99ad	331	//can.write(CANMessage(0x529, &status_pompe,1));
Artiom	4:4a79942713fa	332	break;
Artiom	4:4a79942713fa	333
Artiom	4:4a79942713fa	334	case HACHEUR_GET_ATOM_ACK:
Artiom	4:4a79942713fa	335	status_pompe \|= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom	5:6e198cdd99ad	336	//can.write(CANMessage(0x529, &status_pompe,1));
Artiom	4:4a79942713fa	337	break;
Artiom	4:4a79942713fa	338
Artiom	4:4a79942713fa	339	case HACHEUR_RELEASE_ATOM_ACK :
Artiom	4:4a79942713fa	340	status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom	4:4a79942713fa	341	break;
Artiom	13:9e19048eb065	342
Artiom	13:9e19048eb065	343
Artiom	4:4a79942713fa	344	//-------------------------------------------------------------------------------------------------------------------------------------------
Artiom	4:4a79942713fa	345
Artiom	0:bc74da1c502f	346
Artiom	0:bc74da1c502f	347	#endif
Artiom	0:bc74da1c502f	348	#ifdef ROBOT_BIG
Artiom	0:bc74da1c502f	349	case 3:
Artiom	0:bc74da1c502f	350
Artiom	0:bc74da1c502f	351	break;
Artiom	0:bc74da1c502f	352
Artiom	0:bc74da1c502f	353	case 5:
Artiom	0:bc74da1c502f	354
Artiom	0:bc74da1c502f	355	break;
Artiom	0:bc74da1c502f	356
Artiom	0:bc74da1c502f	357	#endif
Artiom	0:bc74da1c502f	358	}
Artiom	0:bc74da1c502f	359	FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	360
Artiom	0:bc74da1c502f	361	}
Artiom	4:4a79942713fa	362
Artiom	4:4a79942713fa	363
Artiom	4:4a79942713fa	364	}
Artiom	4:4a79942713fa	365
Artiom	4:4a79942713fa	366	void automate_ventouse_presentoir_avant (void)
Artiom	4:4a79942713fa	367	{
Artiom	4:4a79942713fa	368
Artiom	4:4a79942713fa	369	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	4:4a79942713fa	370	static type_etat etat = init;
Artiom	4:4a79942713fa	371
Artiom	4:4a79942713fa	372	switch(etat) {
Artiom	4:4a79942713fa	373	case init: //attente d'initialisation
Artiom	4:4a79942713fa	374	if(fpresentoir_avant)
Artiom	4:4a79942713fa	375	etat=envoi_instruction;
Artiom	4:4a79942713fa	376	break;
Artiom	4:4a79942713fa	377
Artiom	4:4a79942713fa	378	case envoi_instruction://envoi instruction
Artiom	4:4a79942713fa	379	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	4:4a79942713fa	380	presentoir_avant();
Artiom	5:6e198cdd99ad	381	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	4:4a79942713fa	382	etat = attente_ack_ventouse;
Artiom	4:4a79942713fa	383	break;
Artiom	4:4a79942713fa	384
Artiom	4:4a79942713fa	385	case attente_ack_ventouse:
Artiom	5:6e198cdd99ad	386	if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
Artiom	4:4a79942713fa	387	fpresentoir_avant=0;
Artiom	5:6e198cdd99ad	388	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	4:4a79942713fa	389	etat = init;
Artiom	5:6e198cdd99ad	390	}
Artiom	5:6e198cdd99ad	391	break;
Artiom	5:6e198cdd99ad	392
Artiom	5:6e198cdd99ad	393	}
Artiom	5:6e198cdd99ad	394	}
Artiom	5:6e198cdd99ad	395	void automate_ventouse_presentoir_arriere (void)
Artiom	5:6e198cdd99ad	396	{
Artiom	5:6e198cdd99ad	397
Artiom	5:6e198cdd99ad	398	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	5:6e198cdd99ad	399	static type_etat etat = init;
Artiom	5:6e198cdd99ad	400
Artiom	5:6e198cdd99ad	401	switch(etat) {
Artiom	5:6e198cdd99ad	402	case init: //attente d'initialisation
Artiom	5:6e198cdd99ad	403	if(fpresentoir_arriere)
Artiom	5:6e198cdd99ad	404	etat=envoi_instruction;
Artiom	5:6e198cdd99ad	405	break;
Artiom	5:6e198cdd99ad	406
Artiom	5:6e198cdd99ad	407	case envoi_instruction://envoi instruction
Artiom	5:6e198cdd99ad	408	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	5:6e198cdd99ad	409	presentoir_arriere();
Artiom	5:6e198cdd99ad	410	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	5:6e198cdd99ad	411	etat = attente_ack_ventouse;
Artiom	5:6e198cdd99ad	412	break;
Artiom	5:6e198cdd99ad	413
Artiom	5:6e198cdd99ad	414	case attente_ack_ventouse:
Artiom	5:6e198cdd99ad	415	if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom	5:6e198cdd99ad	416	fpresentoir_arriere=0;
Artiom	5:6e198cdd99ad	417	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	5:6e198cdd99ad	418	etat = init;
Artiom	4:4a79942713fa	419	}
Artiom	4:4a79942713fa	420	break;
Artiom	4:4a79942713fa	421
Artiom	4:4a79942713fa	422	}
Artiom	6:45f9cf44718a	423	}
Artiom	6:45f9cf44718a	424	void automate_ventouse_goldenium_avant (void)
Artiom	6:45f9cf44718a	425	{
Artiom	4:4a79942713fa	426
Artiom	6:45f9cf44718a	427	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	428	static type_etat etat = init;
Artiom	6:45f9cf44718a	429
Artiom	6:45f9cf44718a	430	switch(etat) {
Artiom	6:45f9cf44718a	431	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	432	if(fgoldenium_avant)
Artiom	6:45f9cf44718a	433	etat=envoi_instruction;
Artiom	6:45f9cf44718a	434	break;
Artiom	6:45f9cf44718a	435
Artiom	6:45f9cf44718a	436	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	437	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	438	goldenium_avant();
Artiom	14:b1abd2f6df75	439	char pompe=AV_CENTRE;
Artiom	14:b1abd2f6df75	440	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	16:2a66cfe37262	441	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	442	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	443	break;
Artiom	6:45f9cf44718a	444
Artiom	16:2a66cfe37262	445	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	446	if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom	6:45f9cf44718a	447	fgoldenium_avant=0;
Artiom	6:45f9cf44718a	448	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	449	etat = init;
Artiom	6:45f9cf44718a	450	}
Artiom	6:45f9cf44718a	451	break;
Artiom	6:45f9cf44718a	452
Artiom	6:45f9cf44718a	453	}
Artiom	6:45f9cf44718a	454	}
Artiom	6:45f9cf44718a	455
Artiom	6:45f9cf44718a	456	void automate_ventouse_goldenium_arriere (void)
Artiom	6:45f9cf44718a	457	{
Artiom	6:45f9cf44718a	458
Artiom	6:45f9cf44718a	459	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	460	static type_etat etat = init;
Artiom	6:45f9cf44718a	461
Artiom	6:45f9cf44718a	462	switch(etat) {
Artiom	6:45f9cf44718a	463	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	464	if(fgoldenium_arriere)
Artiom	6:45f9cf44718a	465	etat=envoi_instruction;
Artiom	6:45f9cf44718a	466	break;
Artiom	6:45f9cf44718a	467
Artiom	6:45f9cf44718a	468	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	469	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	470	goldenium_arriere();
Artiom	14:b1abd2f6df75	471	char pompe=AR_CENTRE;
Artiom	14:b1abd2f6df75	472	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	16:2a66cfe37262	473	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	474	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	475	break;
Artiom	6:45f9cf44718a	476
Artiom	16:2a66cfe37262	477	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	478	if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom	6:45f9cf44718a	479	fgoldenium_arriere=0;
Artiom	6:45f9cf44718a	480	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	481	etat = init;
Artiom	6:45f9cf44718a	482	}
Artiom	6:45f9cf44718a	483	break;
Artiom	6:45f9cf44718a	484
Artiom	6:45f9cf44718a	485	}
Artiom	6:45f9cf44718a	486
Artiom	6:45f9cf44718a	487
Artiom	6:45f9cf44718a	488	}
Artiom	6:45f9cf44718a	489
Artiom	6:45f9cf44718a	490	void automate_ventouse_sol_avant (void)
Artiom	6:45f9cf44718a	491	{
Artiom	6:45f9cf44718a	492
Artiom	6:45f9cf44718a	493	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	494	static type_etat etat = init;
Artiom	6:45f9cf44718a	495
Artiom	6:45f9cf44718a	496	switch(etat) {
Artiom	6:45f9cf44718a	497	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	498	if(fsol_avant)
Artiom	6:45f9cf44718a	499	etat=envoi_instruction;
Artiom	6:45f9cf44718a	500	break;
Artiom	6:45f9cf44718a	501
Artiom	6:45f9cf44718a	502	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	503	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	504	sol_avant_baisser();
Artiom	13:9e19048eb065	505	char pompe=AV_BAS;
Artiom	13:9e19048eb065	506	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	6:45f9cf44718a	507	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	508	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	509	break;
Artiom	6:45f9cf44718a	510
Artiom	6:45f9cf44718a	511	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	512	if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom	6:45f9cf44718a	513	sol_avant_remonter();
Artiom	6:45f9cf44718a	514	fsol_avant=0;
Artiom	6:45f9cf44718a	515	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	516	etat = init;
Artiom	6:45f9cf44718a	517	}
Artiom	6:45f9cf44718a	518	break;
Artiom	6:45f9cf44718a	519
Artiom	6:45f9cf44718a	520	}
Artiom	6:45f9cf44718a	521
Artiom	6:45f9cf44718a	522
Artiom	6:45f9cf44718a	523	}
Artiom	6:45f9cf44718a	524	void automate_ventouse_sol_arriere (void)
Artiom	6:45f9cf44718a	525	{
Artiom	6:45f9cf44718a	526
Artiom	6:45f9cf44718a	527	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	528	static type_etat etat = init;
Artiom	6:45f9cf44718a	529
Artiom	6:45f9cf44718a	530	switch(etat) {
Artiom	6:45f9cf44718a	531	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	532	if(fsol_arriere)
Artiom	6:45f9cf44718a	533	etat=envoi_instruction;
Artiom	6:45f9cf44718a	534	break;
Artiom	6:45f9cf44718a	535
Artiom	6:45f9cf44718a	536	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	537	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	538	sol_arriere_baisser();
Artiom	13:9e19048eb065	539	char pompe=AR_BAS;
Artiom	13:9e19048eb065	540	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	6:45f9cf44718a	541	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	542	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	543	break;
Artiom	6:45f9cf44718a	544
Artiom	6:45f9cf44718a	545	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	546	if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom	6:45f9cf44718a	547	sol_arriere_remonter();
Artiom	6:45f9cf44718a	548	fsol_arriere=0;
Artiom	6:45f9cf44718a	549	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	550	etat = init;
Artiom	6:45f9cf44718a	551	}
Artiom	6:45f9cf44718a	552	break;
Artiom	6:45f9cf44718a	553
Artiom	6:45f9cf44718a	554	}
Artiom	4:4a79942713fa	555
Artiom	4:4a79942713fa	556
Artiom	11:6c11c081c855	557	}
Artiom	11:6c11c081c855	558	void automate_ventouse_balance_avant (void)
Artiom	11:6c11c081c855	559	{
Artiom	11:6c11c081c855	560	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	11:6c11c081c855	561	static type_etat etat = init;
Artiom	11:6c11c081c855	562
Artiom	11:6c11c081c855	563	switch(etat) {
Artiom	11:6c11c081c855	564	case init: //attente d'initialisation
Artiom	11:6c11c081c855	565	if(fbalance_avant)
Artiom	11:6c11c081c855	566	etat=envoi_instruction;
Artiom	11:6c11c081c855	567	break;
Artiom	11:6c11c081c855	568
Artiom	11:6c11c081c855	569	case envoi_instruction://envoi instruction
Artiom	11:6c11c081c855	570	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	15:119fc452dcaf	571	presentoir_avant();
Artiom	12:191486ba5118	572	balance_avant_placement();
Artiom	12:191486ba5118	573	Rotate(200);
Artiom	12:191486ba5118	574	wait(0.5);
Artiom	11:6c11c081c855	575	balance_avant();
Artiom	12:191486ba5118	576	Rotate(-200);
Artiom	12:191486ba5118	577	wait(0.5);
Artiom	16:2a66cfe37262	578	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	11:6c11c081c855	579	etat = attente_ack_ventouse;
Artiom	11:6c11c081c855	580	break;
Artiom	11:6c11c081c855	581
Artiom	11:6c11c081c855	582	case attente_ack_ventouse:
Artiom	12:191486ba5118	583	char pompe=AV_DROIT;
Artiom	12:191486ba5118	584	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	11:6c11c081c855	585	if((status_pompe&MASK_AV_DROIT)== 0) {
Artiom	11:6c11c081c855	586	fbalance_avant=0;
Artiom	12:191486ba5118	587	presentoir_arriere();
Artiom	12:191486ba5118	588	presentoir_avant();
Artiom	12:191486ba5118	589	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	12:191486ba5118	590	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	12:191486ba5118	591	etat = init;
Artiom	12:191486ba5118	592	}
Artiom	12:191486ba5118	593	break;
Artiom	12:191486ba5118	594
Artiom	12:191486ba5118	595	}
Artiom	12:191486ba5118	596
Artiom	12:191486ba5118	597
Artiom	12:191486ba5118	598	}
Artiom	12:191486ba5118	599	void automate_ventouse_balance_arriere (void)
Artiom	12:191486ba5118	600	{
Artiom	12:191486ba5118	601	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	12:191486ba5118	602	static type_etat etat = init;
Artiom	12:191486ba5118	603
Artiom	12:191486ba5118	604	switch(etat) {
Artiom	12:191486ba5118	605	case init: //attente d'initialisation
Artiom	12:191486ba5118	606	if(fbalance_arriere)
Artiom	12:191486ba5118	607	etat=envoi_instruction;
Artiom	12:191486ba5118	608	break;
Artiom	12:191486ba5118	609
Artiom	12:191486ba5118	610	case envoi_instruction://envoi instruction
Artiom	12:191486ba5118	611	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	15:119fc452dcaf	612	presentoir_arriere();
Artiom	12:191486ba5118	613	balance_arriere_placement();
Artiom	12:191486ba5118	614	Rotate(200);
Artiom	12:191486ba5118	615	wait(0.5);
Artiom	12:191486ba5118	616	balance_arriere();
Artiom	12:191486ba5118	617	Rotate(-200);
Artiom	12:191486ba5118	618	wait(0.5);
Artiom	16:2a66cfe37262	619	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	12:191486ba5118	620	etat = attente_ack_ventouse;
Artiom	12:191486ba5118	621	break;
Artiom	12:191486ba5118	622
Artiom	12:191486ba5118	623	case attente_ack_ventouse:
Artiom	12:191486ba5118	624	char pompe=AR_DROIT;
Artiom	12:191486ba5118	625	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	12:191486ba5118	626	if((status_pompe&MASK_AR_DROIT)== 0) {
Artiom	12:191486ba5118	627	fbalance_arriere=0;
Artiom	12:191486ba5118	628	presentoir_arriere();
Artiom	12:191486ba5118	629	presentoir_avant();
Artiom	12:191486ba5118	630	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	12:191486ba5118	631	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	12:191486ba5118	632	etat = init;
Artiom	12:191486ba5118	633	}
Artiom	12:191486ba5118	634	break;
Artiom	12:191486ba5118	635
Artiom	12:191486ba5118	636	}
Artiom	12:191486ba5118	637
Artiom	12:191486ba5118	638
Artiom	12:191486ba5118	639	}
Artiom	13:9e19048eb065	640	void automate_ventouse_relache_avant (void)
Artiom	13:9e19048eb065	641	{
Artiom	13:9e19048eb065	642	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	643	static type_etat etat = init;
Artiom	13:9e19048eb065	644
Artiom	13:9e19048eb065	645	switch(etat) {
Artiom	13:9e19048eb065	646	case init: //attente d'initialisation
Artiom	13:9e19048eb065	647	if(favant_relache)
Artiom	13:9e19048eb065	648	etat=envoi_instruction;
Artiom	13:9e19048eb065	649	break;
Artiom	13:9e19048eb065	650
Artiom	13:9e19048eb065	651	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	652	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	16:2a66cfe37262	653	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	654	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	655	break;
Artiom	13:9e19048eb065	656
Artiom	13:9e19048eb065	657	case attente_ack_ventouse:
Artiom	13:9e19048eb065	658	SendRawId(HACHEUR_RELEASE_AV);
Artiom	13:9e19048eb065	659	if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
Artiom	13:9e19048eb065	660	favant_relache=0;
Artiom	13:9e19048eb065	661	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	662	etat = init;
Artiom	13:9e19048eb065	663	}
Artiom	13:9e19048eb065	664	break;
Artiom	13:9e19048eb065	665
Artiom	13:9e19048eb065	666	}
Artiom	13:9e19048eb065	667
Artiom	13:9e19048eb065	668
Artiom	13:9e19048eb065	669	}
Artiom	12:191486ba5118	670	void automate_ventouse_relache_arriere (void)
Artiom	12:191486ba5118	671	{
Artiom	12:191486ba5118	672	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	12:191486ba5118	673	static type_etat etat = init;
Artiom	12:191486ba5118	674
Artiom	12:191486ba5118	675	switch(etat) {
Artiom	12:191486ba5118	676	case init: //attente d'initialisation
Artiom	12:191486ba5118	677	if(farriere_relache)
Artiom	12:191486ba5118	678	etat=envoi_instruction;
Artiom	12:191486ba5118	679	break;
Artiom	12:191486ba5118	680
Artiom	12:191486ba5118	681	case envoi_instruction://envoi instruction
Artiom	12:191486ba5118	682	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	16:2a66cfe37262	683	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	12:191486ba5118	684	etat = attente_ack_ventouse;
Artiom	12:191486ba5118	685	break;
Artiom	12:191486ba5118	686
Artiom	12:191486ba5118	687	case attente_ack_ventouse:
Artiom	16:2a66cfe37262	688	SendRawId(HACHEUR_RELEASE_AR);
Artiom	12:191486ba5118	689	if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
Artiom	12:191486ba5118	690	farriere_relache=0;
Artiom	11:6c11c081c855	691	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	11:6c11c081c855	692	etat = init;
Artiom	11:6c11c081c855	693	}
Artiom	11:6c11c081c855	694	break;
Artiom	11:6c11c081c855	695	}
Artiom	11:6c11c081c855	696
Artiom	13:9e19048eb065	697	}
Artiom	13:9e19048eb065	698	void automate_ventouse_sol_avant_relache (void)
Artiom	13:9e19048eb065	699	{
Artiom	13:9e19048eb065	700	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	701	static type_etat etat = init;
Artiom	13:9e19048eb065	702
Artiom	13:9e19048eb065	703	switch(etat) {
Artiom	13:9e19048eb065	704	case init: //attente d'initialisation
Artiom	13:9e19048eb065	705	if(fsol_avant_relache)
Artiom	13:9e19048eb065	706	etat=envoi_instruction;
Artiom	13:9e19048eb065	707	break;
Artiom	13:9e19048eb065	708
Artiom	13:9e19048eb065	709	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	710	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	13:9e19048eb065	711	positionControl(AV_sol,520,1,BLED_ON,4);//baisser
Artiom	13:9e19048eb065	712	verification();
Artiom	16:2a66cfe37262	713	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	714	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	715	break;
Artiom	13:9e19048eb065	716
Artiom	13:9e19048eb065	717	case attente_ack_ventouse:
Artiom	13:9e19048eb065	718	char pompe=AV_BAS;
Artiom	13:9e19048eb065	719	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	13:9e19048eb065	720	if((status_pompe&MASK_SOL_AV)== 0) {
Artiom	13:9e19048eb065	721	sol_avant_remonter();
Artiom	13:9e19048eb065	722	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	723	fsol_avant_relache=0;
Artiom	13:9e19048eb065	724	etat = init;
Artiom	13:9e19048eb065	725	}
Artiom	13:9e19048eb065	726	break;
Artiom	13:9e19048eb065	727
Artiom	13:9e19048eb065	728	}
Artiom	13:9e19048eb065	729
Artiom	13:9e19048eb065	730
Artiom	13:9e19048eb065	731	}
Artiom	13:9e19048eb065	732
Artiom	13:9e19048eb065	733	void automate_ventouse_sol_arriere_relache (void)
Artiom	13:9e19048eb065	734	{
Artiom	13:9e19048eb065	735	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	736	static type_etat etat = init;
Artiom	13:9e19048eb065	737
Artiom	13:9e19048eb065	738	switch(etat) {
Artiom	13:9e19048eb065	739	case init: //attente d'initialisation
Artiom	13:9e19048eb065	740	if(fsol_arriere_relache)
Artiom	13:9e19048eb065	741	etat=envoi_instruction;
Artiom	13:9e19048eb065	742	break;
Artiom	13:9e19048eb065	743
Artiom	13:9e19048eb065	744	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	745	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	13:9e19048eb065	746	positionControl(AR_sol,480,1,BLED_ON,4);//baisser
Artiom	13:9e19048eb065	747	verification();
Artiom	16:2a66cfe37262	748	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	749	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	750	break;
Artiom	13:9e19048eb065	751
Artiom	13:9e19048eb065	752	case attente_ack_ventouse:
Artiom	13:9e19048eb065	753	char pompe=AR_BAS;
Artiom	13:9e19048eb065	754	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	13:9e19048eb065	755	if((status_pompe&MASK_SOL_AR)== 0) {
Artiom	13:9e19048eb065	756	sol_arriere_remonter();
Artiom	13:9e19048eb065	757	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	758	fsol_arriere_relache=0;
Artiom	13:9e19048eb065	759	etat = init;
Artiom	13:9e19048eb065	760	}
Artiom	13:9e19048eb065	761	break;
Artiom	13:9e19048eb065	762
Artiom	13:9e19048eb065	763	}
Artiom	13:9e19048eb065	764
Artiom	13:9e19048eb065	765
Artiom	14:b1abd2f6df75	766	}
Artiom	14:b1abd2f6df75	767	void automate_ventouse_accelerateur_avant (void)
Artiom	14:b1abd2f6df75	768	{
Artiom	16:2a66cfe37262	769	typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom	14:b1abd2f6df75	770	static type_etat etat = init;
Artiom	14:b1abd2f6df75	771
Artiom	14:b1abd2f6df75	772	switch(etat) {
Artiom	14:b1abd2f6df75	773	case init: //attente d'initialisation
Artiom	14:b1abd2f6df75	774	if(faccelerateur_avant)
Artiom	14:b1abd2f6df75	775	etat=envoi_instruction;
Artiom	14:b1abd2f6df75	776	break;
Artiom	14:b1abd2f6df75	777
Artiom	14:b1abd2f6df75	778	case envoi_instruction://envoi instruction
Artiom	14:b1abd2f6df75	779	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	14:b1abd2f6df75	780	accelerateur_avant();
Artiom	14:b1abd2f6df75	781	GoStraight(200,1,35,0);//35epaisseur accelerateur
Artiom	16:2a66cfe37262	782	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	16:2a66cfe37262	783	etat = attente_ack_ventouse_droite_gauche;
Artiom	14:b1abd2f6df75	784	break;
Artiom	14:b1abd2f6df75	785
Artiom	16:2a66cfe37262	786	case attente_ack_ventouse_droite_gauche:
Artiom	14:b1abd2f6df75	787	char pompe=AV_DROIT;
Artiom	14:b1abd2f6df75	788	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	14:b1abd2f6df75	789	pompe=AV_GAUCHE;
Artiom	14:b1abd2f6df75	790	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	16:2a66cfe37262	791	if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom	16:2a66cfe37262	792	GoStraight(-20,0,0,0);
Artiom	16:2a66cfe37262	793	wait(1);
Artiom	16:2a66cfe37262	794	presentoir_avant();
Artiom	16:2a66cfe37262	795	etat = attente_ack_ventouse_centre;
Artiom	16:2a66cfe37262	796	}
Artiom	16:2a66cfe37262	797	case attente_ack_ventouse_centre:
Artiom	16:2a66cfe37262	798	pompe=AV_CENTRE;
Artiom	16:2a66cfe37262	799	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	14:b1abd2f6df75	800	if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom	14:b1abd2f6df75	801	faccelerateur_avant=0;
Artiom	14:b1abd2f6df75	802	presentoir_avant();
Artiom	14:b1abd2f6df75	803	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	14:b1abd2f6df75	804	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	14:b1abd2f6df75	805	etat = init;
Artiom	14:b1abd2f6df75	806	}
Artiom	16:2a66cfe37262	807
Artiom	14:b1abd2f6df75	808	break;
Artiom	14:b1abd2f6df75	809
Artiom	14:b1abd2f6df75	810	}
Artiom	14:b1abd2f6df75	811
Artiom	14:b1abd2f6df75	812
Artiom	14:b1abd2f6df75	813	}
Artiom	14:b1abd2f6df75	814
Artiom	14:b1abd2f6df75	815
Artiom	14:b1abd2f6df75	816	void automate_ventouse_accelerateur_arriere (void)
Artiom	14:b1abd2f6df75	817	{
Artiom	14:b1abd2f6df75	818
Artiom	14:b1abd2f6df75	819
Artiom	14:b1abd2f6df75	820	}
Artiom	14:b1abd2f6df75	821
Artiom	14:b1abd2f6df75	822

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@16:2a66cfe37262, 2019-05-21 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning