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@20:42f8ec726ac8, 2019-05-22 (annotated)

Committer:: Artiom
Date:: Wed May 22 08:25:51 2019 +0000
Revision:: 20:42f8ec726ac8
Parent:: 19:511ff570c2f5
Child:: 21:edf6d03fdb20

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	20:42f8ec726ac8	325	case VENTOUSE_AV_CENTRE_BALANCE:
Artiom	20:42f8ec726ac8	326	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	20:42f8ec726ac8	327	goldenium_avant();
Artiom	20:42f8ec726ac8	328	positionControl(AV_poigne_D, 270,1,BLED_ON,1);//actionneur
Artiom	20:42f8ec726ac8	329	verification();
Artiom	20:42f8ec726ac8	330	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	20:42f8ec726ac8	331	break;
Artiom	20:42f8ec726ac8	332
Artiom	20:42f8ec726ac8	333	case VENTOUSE_AR_CENTRE_BALANCE:
Artiom	20:42f8ec726ac8	334	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	20:42f8ec726ac8	335	goldenium_arriere();
Artiom	20:42f8ec726ac8	336	positionControl(AR_poigne_D, 200,1,BLED_ON,3);//actionneur
Artiom	20:42f8ec726ac8	337	verification();
Artiom	20:42f8ec726ac8	338	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	20:42f8ec726ac8	339	break;
Artiom	13:9e19048eb065	340
Artiom	4:4a79942713fa	341	//--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
Artiom	4:4a79942713fa	342
Artiom	4:4a79942713fa	343
Artiom	4:4a79942713fa	344	case HACHEUR_STATUT_VENTOUSES:
Artiom	4:4a79942713fa	345	status_pompe = msgRxBuffer[FIFO_lecture].data[1];
Artiom	5:6e198cdd99ad	346	//can.write(CANMessage(0x529, &status_pompe,1));
Artiom	4:4a79942713fa	347	break;
Artiom	4:4a79942713fa	348
Artiom	4:4a79942713fa	349	case HACHEUR_GET_ATOM_ACK:
Artiom	4:4a79942713fa	350	status_pompe \|= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom	5:6e198cdd99ad	351	//can.write(CANMessage(0x529, &status_pompe,1));
Artiom	4:4a79942713fa	352	break;
Artiom	4:4a79942713fa	353
Artiom	4:4a79942713fa	354	case HACHEUR_RELEASE_ATOM_ACK :
Artiom	4:4a79942713fa	355	status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
Artiom	4:4a79942713fa	356	break;
Artiom	13:9e19048eb065	357
Artiom	13:9e19048eb065	358
Artiom	4:4a79942713fa	359	//-------------------------------------------------------------------------------------------------------------------------------------------
Artiom	4:4a79942713fa	360
Artiom	0:bc74da1c502f	361
Artiom	0:bc74da1c502f	362	#endif
Artiom	0:bc74da1c502f	363	#ifdef ROBOT_BIG
kyxstark	18:0ea0d887966f	364	case COUROIES:
kyxstark	18:0ea0d887966f	365	rotation_couroies = msgRxBuffer[FIFO_lecture].data[0];
Artiom	0:bc74da1c502f	366	break;
Artiom	0:bc74da1c502f	367
Artiom	0:bc74da1c502f	368	case 5:
Artiom	0:bc74da1c502f	369
Artiom	0:bc74da1c502f	370	break;
Artiom	0:bc74da1c502f	371
Artiom	0:bc74da1c502f	372	#endif
Artiom	0:bc74da1c502f	373	}
Artiom	0:bc74da1c502f	374	FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Artiom	0:bc74da1c502f	375
Artiom	0:bc74da1c502f	376	}
Artiom	4:4a79942713fa	377
Artiom	4:4a79942713fa	378
Artiom	4:4a79942713fa	379	}
Artiom	4:4a79942713fa	380
Artiom	4:4a79942713fa	381	void automate_ventouse_presentoir_avant (void)
Artiom	4:4a79942713fa	382	{
Artiom	4:4a79942713fa	383
Artiom	4:4a79942713fa	384	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	4:4a79942713fa	385	static type_etat etat = init;
Artiom	4:4a79942713fa	386
Artiom	4:4a79942713fa	387	switch(etat) {
Artiom	4:4a79942713fa	388	case init: //attente d'initialisation
Artiom	4:4a79942713fa	389	if(fpresentoir_avant)
Artiom	4:4a79942713fa	390	etat=envoi_instruction;
Artiom	4:4a79942713fa	391	break;
Artiom	4:4a79942713fa	392
Artiom	4:4a79942713fa	393	case envoi_instruction://envoi instruction
Artiom	4:4a79942713fa	394	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	4:4a79942713fa	395	presentoir_avant();
Artiom	20:42f8ec726ac8	396	can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV));
Artiom	5:6e198cdd99ad	397	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	4:4a79942713fa	398	etat = attente_ack_ventouse;
Artiom	4:4a79942713fa	399	break;
Artiom	4:4a79942713fa	400
Artiom	4:4a79942713fa	401	case attente_ack_ventouse:
Artiom	5:6e198cdd99ad	402	if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
Artiom	4:4a79942713fa	403	fpresentoir_avant=0;
Artiom	5:6e198cdd99ad	404	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	4:4a79942713fa	405	etat = init;
Artiom	5:6e198cdd99ad	406	}
Artiom	5:6e198cdd99ad	407	break;
Artiom	5:6e198cdd99ad	408
Artiom	5:6e198cdd99ad	409	}
Artiom	5:6e198cdd99ad	410	}
Artiom	5:6e198cdd99ad	411	void automate_ventouse_presentoir_arriere (void)
Artiom	5:6e198cdd99ad	412	{
Artiom	5:6e198cdd99ad	413
Artiom	5:6e198cdd99ad	414	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	5:6e198cdd99ad	415	static type_etat etat = init;
Artiom	5:6e198cdd99ad	416
Artiom	5:6e198cdd99ad	417	switch(etat) {
Artiom	5:6e198cdd99ad	418	case init: //attente d'initialisation
Artiom	5:6e198cdd99ad	419	if(fpresentoir_arriere)
Artiom	5:6e198cdd99ad	420	etat=envoi_instruction;
Artiom	5:6e198cdd99ad	421	break;
Artiom	5:6e198cdd99ad	422
Artiom	5:6e198cdd99ad	423	case envoi_instruction://envoi instruction
Artiom	5:6e198cdd99ad	424	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	5:6e198cdd99ad	425	presentoir_arriere();
Artiom	20:42f8ec726ac8	426	can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AR));
Artiom	5:6e198cdd99ad	427	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	5:6e198cdd99ad	428	etat = attente_ack_ventouse;
Artiom	5:6e198cdd99ad	429	break;
Artiom	5:6e198cdd99ad	430
Artiom	5:6e198cdd99ad	431	case attente_ack_ventouse:
Artiom	5:6e198cdd99ad	432	if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom	5:6e198cdd99ad	433	fpresentoir_arriere=0;
Artiom	5:6e198cdd99ad	434	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	5:6e198cdd99ad	435	etat = init;
Artiom	4:4a79942713fa	436	}
Artiom	4:4a79942713fa	437	break;
Artiom	4:4a79942713fa	438
Artiom	4:4a79942713fa	439	}
Artiom	6:45f9cf44718a	440	}
Artiom	6:45f9cf44718a	441	void automate_ventouse_goldenium_avant (void)
Artiom	6:45f9cf44718a	442	{
Artiom	4:4a79942713fa	443
Artiom	6:45f9cf44718a	444	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	445	static type_etat etat = init;
Artiom	6:45f9cf44718a	446
Artiom	6:45f9cf44718a	447	switch(etat) {
Artiom	6:45f9cf44718a	448	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	449	if(fgoldenium_avant)
Artiom	6:45f9cf44718a	450	etat=envoi_instruction;
Artiom	6:45f9cf44718a	451	break;
Artiom	6:45f9cf44718a	452
Artiom	6:45f9cf44718a	453	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	454	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	455	goldenium_avant();
Artiom	14:b1abd2f6df75	456	char pompe=AV_CENTRE;
Artiom	14:b1abd2f6df75	457	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	20:42f8ec726ac8	458	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	459	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	460	break;
Artiom	6:45f9cf44718a	461
Artiom	20:42f8ec726ac8	462	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	463	if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom	6:45f9cf44718a	464	fgoldenium_avant=0;
Artiom	6:45f9cf44718a	465	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	466	etat = init;
Artiom	6:45f9cf44718a	467	}
Artiom	6:45f9cf44718a	468	break;
Artiom	6:45f9cf44718a	469
Artiom	6:45f9cf44718a	470	}
Artiom	6:45f9cf44718a	471	}
Artiom	6:45f9cf44718a	472
Artiom	6:45f9cf44718a	473	void automate_ventouse_goldenium_arriere (void)
Artiom	6:45f9cf44718a	474	{
Artiom	6:45f9cf44718a	475
Artiom	6:45f9cf44718a	476	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	477	static type_etat etat = init;
Artiom	6:45f9cf44718a	478
Artiom	6:45f9cf44718a	479	switch(etat) {
Artiom	6:45f9cf44718a	480	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	481	if(fgoldenium_arriere)
Artiom	6:45f9cf44718a	482	etat=envoi_instruction;
Artiom	6:45f9cf44718a	483	break;
Artiom	6:45f9cf44718a	484
Artiom	6:45f9cf44718a	485	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	486	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	487	goldenium_arriere();
Artiom	14:b1abd2f6df75	488	char pompe=AR_CENTRE;
Artiom	14:b1abd2f6df75	489	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	20:42f8ec726ac8	490	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	491	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	492	break;
Artiom	6:45f9cf44718a	493
Artiom	20:42f8ec726ac8	494	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	495	if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom	6:45f9cf44718a	496	fgoldenium_arriere=0;
Artiom	6:45f9cf44718a	497	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	498	etat = init;
Artiom	6:45f9cf44718a	499	}
Artiom	6:45f9cf44718a	500	break;
Artiom	6:45f9cf44718a	501
Artiom	6:45f9cf44718a	502	}
Artiom	6:45f9cf44718a	503
Artiom	6:45f9cf44718a	504
Artiom	6:45f9cf44718a	505	}
Artiom	6:45f9cf44718a	506
Artiom	6:45f9cf44718a	507	void automate_ventouse_sol_avant (void)
Artiom	6:45f9cf44718a	508	{
Artiom	6:45f9cf44718a	509
Artiom	6:45f9cf44718a	510	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	511	static type_etat etat = init;
Artiom	6:45f9cf44718a	512
Artiom	6:45f9cf44718a	513	switch(etat) {
Artiom	6:45f9cf44718a	514	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	515	if(fsol_avant)
Artiom	6:45f9cf44718a	516	etat=envoi_instruction;
Artiom	6:45f9cf44718a	517	break;
Artiom	6:45f9cf44718a	518
Artiom	6:45f9cf44718a	519	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	520	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	521	sol_avant_baisser();
Artiom	13:9e19048eb065	522	char pompe=AV_BAS;
Artiom	13:9e19048eb065	523	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	6:45f9cf44718a	524	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	525	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	526	break;
Artiom	6:45f9cf44718a	527
Artiom	6:45f9cf44718a	528	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	529	if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom	6:45f9cf44718a	530	sol_avant_remonter();
Artiom	6:45f9cf44718a	531	fsol_avant=0;
Artiom	6:45f9cf44718a	532	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	533	etat = init;
Artiom	6:45f9cf44718a	534	}
Artiom	6:45f9cf44718a	535	break;
Artiom	6:45f9cf44718a	536
Artiom	6:45f9cf44718a	537	}
Artiom	6:45f9cf44718a	538
Artiom	6:45f9cf44718a	539
Artiom	6:45f9cf44718a	540	}
Artiom	6:45f9cf44718a	541	void automate_ventouse_sol_arriere (void)
Artiom	6:45f9cf44718a	542	{
Artiom	6:45f9cf44718a	543
Artiom	6:45f9cf44718a	544	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	6:45f9cf44718a	545	static type_etat etat = init;
Artiom	6:45f9cf44718a	546
Artiom	6:45f9cf44718a	547	switch(etat) {
Artiom	6:45f9cf44718a	548	case init: //attente d'initialisation
Artiom	6:45f9cf44718a	549	if(fsol_arriere)
Artiom	6:45f9cf44718a	550	etat=envoi_instruction;
Artiom	6:45f9cf44718a	551	break;
Artiom	6:45f9cf44718a	552
Artiom	6:45f9cf44718a	553	case envoi_instruction://envoi instruction
Artiom	6:45f9cf44718a	554	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	6:45f9cf44718a	555	sol_arriere_baisser();
Artiom	13:9e19048eb065	556	char pompe=AR_BAS;
Artiom	13:9e19048eb065	557	can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom	6:45f9cf44718a	558	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	6:45f9cf44718a	559	etat = attente_ack_ventouse;
Artiom	6:45f9cf44718a	560	break;
Artiom	6:45f9cf44718a	561
Artiom	6:45f9cf44718a	562	case attente_ack_ventouse:
Artiom	6:45f9cf44718a	563	if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom	6:45f9cf44718a	564	sol_arriere_remonter();
Artiom	6:45f9cf44718a	565	fsol_arriere=0;
Artiom	6:45f9cf44718a	566	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	6:45f9cf44718a	567	etat = init;
Artiom	6:45f9cf44718a	568	}
Artiom	6:45f9cf44718a	569	break;
Artiom	6:45f9cf44718a	570
Artiom	6:45f9cf44718a	571	}
Artiom	4:4a79942713fa	572
Artiom	4:4a79942713fa	573
Artiom	11:6c11c081c855	574	}
Artiom	11:6c11c081c855	575	void automate_ventouse_balance_avant (void)
Artiom	11:6c11c081c855	576	{
Artiom	11:6c11c081c855	577	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	11:6c11c081c855	578	static type_etat etat = init;
Artiom	11:6c11c081c855	579
Artiom	11:6c11c081c855	580	switch(etat) {
Artiom	11:6c11c081c855	581	case init: //attente d'initialisation
Artiom	11:6c11c081c855	582	if(fbalance_avant)
Artiom	11:6c11c081c855	583	etat=envoi_instruction;
Artiom	11:6c11c081c855	584	break;
Artiom	11:6c11c081c855	585
Artiom	11:6c11c081c855	586	case envoi_instruction://envoi instruction
Artiom	11:6c11c081c855	587	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	15:119fc452dcaf	588	presentoir_avant();
Artiom	20:42f8ec726ac8	589	verification();
Artiom	12:191486ba5118	590	balance_avant_placement();
Artiom	12:191486ba5118	591	Rotate(200);
Artiom	12:191486ba5118	592	wait(0.5);
Artiom	11:6c11c081c855	593	balance_avant();
Artiom	12:191486ba5118	594	Rotate(-200);
Artiom	12:191486ba5118	595	wait(0.5);
Artiom	20:42f8ec726ac8	596	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	11:6c11c081c855	597	etat = attente_ack_ventouse;
Artiom	11:6c11c081c855	598	break;
Artiom	11:6c11c081c855	599
Artiom	11:6c11c081c855	600	case attente_ack_ventouse:
Artiom	12:191486ba5118	601	char pompe=AV_DROIT;
Artiom	12:191486ba5118	602	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	11:6c11c081c855	603	if((status_pompe&MASK_AV_DROIT)== 0) {
Artiom	11:6c11c081c855	604	fbalance_avant=0;
Artiom	12:191486ba5118	605	presentoir_arriere();
Artiom	12:191486ba5118	606	presentoir_avant();
Artiom	12:191486ba5118	607	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	12:191486ba5118	608	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	12:191486ba5118	609	etat = init;
Artiom	12:191486ba5118	610	}
Artiom	12:191486ba5118	611	break;
Artiom	12:191486ba5118	612
Artiom	12:191486ba5118	613	}
Artiom	12:191486ba5118	614
Artiom	12:191486ba5118	615
Artiom	12:191486ba5118	616	}
Artiom	12:191486ba5118	617	void automate_ventouse_balance_arriere (void)
Artiom	12:191486ba5118	618	{
Artiom	12:191486ba5118	619	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	12:191486ba5118	620	static type_etat etat = init;
Artiom	12:191486ba5118	621
Artiom	12:191486ba5118	622	switch(etat) {
Artiom	12:191486ba5118	623	case init: //attente d'initialisation
Artiom	12:191486ba5118	624	if(fbalance_arriere)
Artiom	12:191486ba5118	625	etat=envoi_instruction;
Artiom	12:191486ba5118	626	break;
Artiom	12:191486ba5118	627
Artiom	12:191486ba5118	628	case envoi_instruction://envoi instruction
Artiom	12:191486ba5118	629	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	15:119fc452dcaf	630	presentoir_arriere();
Artiom	20:42f8ec726ac8	631	verification();
Artiom	12:191486ba5118	632	balance_arriere_placement();
Artiom	12:191486ba5118	633	Rotate(200);
Artiom	12:191486ba5118	634	wait(0.5);
Artiom	12:191486ba5118	635	balance_arriere();
Artiom	12:191486ba5118	636	Rotate(-200);
Artiom	12:191486ba5118	637	wait(0.5);
Artiom	16:2a66cfe37262	638	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	12:191486ba5118	639	etat = attente_ack_ventouse;
Artiom	12:191486ba5118	640	break;
Artiom	12:191486ba5118	641
Artiom	12:191486ba5118	642	case attente_ack_ventouse:
Artiom	12:191486ba5118	643	char pompe=AR_DROIT;
Artiom	12:191486ba5118	644	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	12:191486ba5118	645	if((status_pompe&MASK_AR_DROIT)== 0) {
Artiom	12:191486ba5118	646	fbalance_arriere=0;
Artiom	12:191486ba5118	647	presentoir_arriere();
Artiom	12:191486ba5118	648	presentoir_avant();
Artiom	12:191486ba5118	649	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	12:191486ba5118	650	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	12:191486ba5118	651	etat = init;
Artiom	12:191486ba5118	652	}
Artiom	12:191486ba5118	653	break;
Artiom	12:191486ba5118	654
Artiom	12:191486ba5118	655	}
Artiom	12:191486ba5118	656
Artiom	12:191486ba5118	657
Artiom	12:191486ba5118	658	}
Artiom	13:9e19048eb065	659	void automate_ventouse_relache_avant (void)
Artiom	13:9e19048eb065	660	{
Artiom	13:9e19048eb065	661	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	662	static type_etat etat = init;
Artiom	13:9e19048eb065	663
Artiom	13:9e19048eb065	664	switch(etat) {
Artiom	13:9e19048eb065	665	case init: //attente d'initialisation
Artiom	13:9e19048eb065	666	if(favant_relache)
Artiom	13:9e19048eb065	667	etat=envoi_instruction;
Artiom	13:9e19048eb065	668	break;
Artiom	13:9e19048eb065	669
Artiom	13:9e19048eb065	670	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	671	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	16:2a66cfe37262	672	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	673	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	674	break;
Artiom	13:9e19048eb065	675
Artiom	13:9e19048eb065	676	case attente_ack_ventouse:
Artiom	13:9e19048eb065	677	SendRawId(HACHEUR_RELEASE_AV);
Artiom	13:9e19048eb065	678	if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
Artiom	13:9e19048eb065	679	favant_relache=0;
Artiom	13:9e19048eb065	680	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	681	etat = init;
Artiom	13:9e19048eb065	682	}
Artiom	13:9e19048eb065	683	break;
Artiom	13:9e19048eb065	684
Artiom	13:9e19048eb065	685	}
Artiom	13:9e19048eb065	686
Artiom	13:9e19048eb065	687
Artiom	13:9e19048eb065	688	}
Artiom	12:191486ba5118	689	void automate_ventouse_relache_arriere (void)
Artiom	12:191486ba5118	690	{
Artiom	12:191486ba5118	691	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	12:191486ba5118	692	static type_etat etat = init;
Artiom	12:191486ba5118	693
Artiom	12:191486ba5118	694	switch(etat) {
Artiom	12:191486ba5118	695	case init: //attente d'initialisation
Artiom	12:191486ba5118	696	if(farriere_relache)
Artiom	12:191486ba5118	697	etat=envoi_instruction;
Artiom	12:191486ba5118	698	break;
Artiom	12:191486ba5118	699
Artiom	12:191486ba5118	700	case envoi_instruction://envoi instruction
Artiom	12:191486ba5118	701	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	16:2a66cfe37262	702	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	12:191486ba5118	703	etat = attente_ack_ventouse;
Artiom	12:191486ba5118	704	break;
Artiom	12:191486ba5118	705
Artiom	12:191486ba5118	706	case attente_ack_ventouse:
Artiom	20:42f8ec726ac8	707	SendRawId(HACHEUR_RELEASE_AR);
Artiom	12:191486ba5118	708	if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
Artiom	12:191486ba5118	709	farriere_relache=0;
Artiom	11:6c11c081c855	710	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	11:6c11c081c855	711	etat = init;
Artiom	11:6c11c081c855	712	}
Artiom	11:6c11c081c855	713	break;
Artiom	11:6c11c081c855	714	}
Artiom	11:6c11c081c855	715
Artiom	13:9e19048eb065	716	}
Artiom	13:9e19048eb065	717	void automate_ventouse_sol_avant_relache (void)
Artiom	13:9e19048eb065	718	{
Artiom	13:9e19048eb065	719	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	720	static type_etat etat = init;
Artiom	13:9e19048eb065	721
Artiom	13:9e19048eb065	722	switch(etat) {
Artiom	13:9e19048eb065	723	case init: //attente d'initialisation
Artiom	13:9e19048eb065	724	if(fsol_avant_relache)
Artiom	13:9e19048eb065	725	etat=envoi_instruction;
Artiom	13:9e19048eb065	726	break;
Artiom	13:9e19048eb065	727
Artiom	13:9e19048eb065	728	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	729	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	13:9e19048eb065	730	positionControl(AV_sol,520,1,BLED_ON,4);//baisser
Artiom	13:9e19048eb065	731	verification();
Artiom	16:2a66cfe37262	732	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	733	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	734	break;
Artiom	13:9e19048eb065	735
Artiom	13:9e19048eb065	736	case attente_ack_ventouse:
Artiom	13:9e19048eb065	737	char pompe=AV_BAS;
Artiom	13:9e19048eb065	738	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	13:9e19048eb065	739	if((status_pompe&MASK_SOL_AV)== 0) {
Artiom	13:9e19048eb065	740	sol_avant_remonter();
Artiom	13:9e19048eb065	741	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	742	fsol_avant_relache=0;
Artiom	13:9e19048eb065	743	etat = init;
Artiom	13:9e19048eb065	744	}
Artiom	13:9e19048eb065	745	break;
Artiom	13:9e19048eb065	746
Artiom	13:9e19048eb065	747	}
Artiom	13:9e19048eb065	748
Artiom	13:9e19048eb065	749
Artiom	13:9e19048eb065	750	}
Artiom	13:9e19048eb065	751
Artiom	13:9e19048eb065	752	void automate_ventouse_sol_arriere_relache (void)
Artiom	13:9e19048eb065	753	{
Artiom	13:9e19048eb065	754	typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom	13:9e19048eb065	755	static type_etat etat = init;
Artiom	13:9e19048eb065	756
Artiom	13:9e19048eb065	757	switch(etat) {
Artiom	13:9e19048eb065	758	case init: //attente d'initialisation
Artiom	13:9e19048eb065	759	if(fsol_arriere_relache)
Artiom	13:9e19048eb065	760	etat=envoi_instruction;
Artiom	13:9e19048eb065	761	break;
Artiom	13:9e19048eb065	762
Artiom	13:9e19048eb065	763	case envoi_instruction://envoi instruction
Artiom	13:9e19048eb065	764	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	13:9e19048eb065	765	positionControl(AR_sol,480,1,BLED_ON,4);//baisser
Artiom	13:9e19048eb065	766	verification();
Artiom	20:42f8ec726ac8	767	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	13:9e19048eb065	768	etat = attente_ack_ventouse;
Artiom	13:9e19048eb065	769	break;
Artiom	13:9e19048eb065	770
Artiom	13:9e19048eb065	771	case attente_ack_ventouse:
Artiom	13:9e19048eb065	772	char pompe=AR_BAS;
Artiom	13:9e19048eb065	773	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	13:9e19048eb065	774	if((status_pompe&MASK_SOL_AR)== 0) {
Artiom	13:9e19048eb065	775	sol_arriere_remonter();
Artiom	13:9e19048eb065	776	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	13:9e19048eb065	777	fsol_arriere_relache=0;
Artiom	13:9e19048eb065	778	etat = init;
Artiom	13:9e19048eb065	779	}
Artiom	13:9e19048eb065	780	break;
Artiom	13:9e19048eb065	781
Artiom	13:9e19048eb065	782	}
Artiom	13:9e19048eb065	783
Artiom	13:9e19048eb065	784
Artiom	14:b1abd2f6df75	785	}
Artiom	14:b1abd2f6df75	786	void automate_ventouse_accelerateur_avant (void)
Artiom	14:b1abd2f6df75	787	{
Artiom	16:2a66cfe37262	788	typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom	14:b1abd2f6df75	789	static type_etat etat = init;
Artiom	14:b1abd2f6df75	790
Artiom	14:b1abd2f6df75	791	switch(etat) {
Artiom	14:b1abd2f6df75	792	case init: //attente d'initialisation
Artiom	14:b1abd2f6df75	793	if(faccelerateur_avant)
Artiom	14:b1abd2f6df75	794	etat=envoi_instruction;
Artiom	14:b1abd2f6df75	795	break;
Artiom	14:b1abd2f6df75	796
Artiom	14:b1abd2f6df75	797	case envoi_instruction://envoi instruction
Artiom	14:b1abd2f6df75	798	SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom	14:b1abd2f6df75	799	accelerateur_avant();
Artiom	14:b1abd2f6df75	800	GoStraight(200,1,35,0);//35epaisseur accelerateur
Artiom	16:2a66cfe37262	801	SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom	16:2a66cfe37262	802	etat = attente_ack_ventouse_droite_gauche;
Artiom	14:b1abd2f6df75	803	break;
Artiom	14:b1abd2f6df75	804
Artiom	16:2a66cfe37262	805	case attente_ack_ventouse_droite_gauche:
Artiom	14:b1abd2f6df75	806	char pompe=AV_DROIT;
Artiom	14:b1abd2f6df75	807	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	14:b1abd2f6df75	808	pompe=AV_GAUCHE;
Artiom	14:b1abd2f6df75	809	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	16:2a66cfe37262	810	if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom	16:2a66cfe37262	811	GoStraight(-20,0,0,0);
Artiom	16:2a66cfe37262	812	wait(1);
Artiom	16:2a66cfe37262	813	presentoir_avant();
Artiom	16:2a66cfe37262	814	etat = attente_ack_ventouse_centre;
Artiom	16:2a66cfe37262	815	}
Artiom	16:2a66cfe37262	816	case attente_ack_ventouse_centre:
Artiom	20:42f8ec726ac8	817	pompe=AV_CENTRE;
Artiom	20:42f8ec726ac8	818	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom	14:b1abd2f6df75	819	if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom	14:b1abd2f6df75	820	faccelerateur_avant=0;
Artiom	14:b1abd2f6df75	821	presentoir_avant();
Artiom	14:b1abd2f6df75	822	can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom	14:b1abd2f6df75	823	SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom	14:b1abd2f6df75	824	etat = init;
Artiom	14:b1abd2f6df75	825	}
Artiom	20:42f8ec726ac8	826
Artiom	14:b1abd2f6df75	827	break;
Artiom	14:b1abd2f6df75	828
Artiom	14:b1abd2f6df75	829	}
Artiom	14:b1abd2f6df75	830
Artiom	14:b1abd2f6df75	831
Artiom	14:b1abd2f6df75	832	}
Artiom	14:b1abd2f6df75	833
Artiom	14:b1abd2f6df75	834
Artiom	14:b1abd2f6df75	835	void automate_ventouse_accelerateur_arriere (void)
Artiom	14:b1abd2f6df75	836	{
Artiom	14:b1abd2f6df75	837
Artiom	14:b1abd2f6df75	838	}
Artiom	14:b1abd2f6df75	839
Artiom	14:b1abd2f6df75	840

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@20:42f8ec726ac8, 2019-05-22 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning