actions_Gr - librairie actions gros robot carte esclave

CRAC Team » Code » actions_Gr

librairie actions gros robot carte esclave

Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019

actions_Gr.cpp@1:af508f84a079, 2019-05-23 (annotated)

Committer:: kyxstark
Date:: Thu May 23 11:44:30 2019 +0000
Revision:: 1:af508f84a079
Parent:: 0:f900f4ea9dfd
Child:: 2:33583329d6c8

Who changed what in which revision?

User	Revision	Line number	New contents of line
Artiom	0:f900f4ea9dfd	1	#include "actions_Gr.h"
Artiom	0:f900f4ea9dfd	2
Artiom	0:f900f4ea9dfd	3
Artiom	0:f900f4ea9dfd	4	#ifdef ROBOT_BIG
Artiom	0:f900f4ea9dfd	5	#define MASK_PRESENTOIR_AV 0x07
Artiom	0:f900f4ea9dfd	6	#define MASK_PRESENTOIR_AR 0x70
Artiom	0:f900f4ea9dfd	7
Artiom	0:f900f4ea9dfd	8	#define MASK_GOLDENIUM_AV 0x02
Artiom	0:f900f4ea9dfd	9	#define MASK_GOLDENIUM_AR 0x20
Artiom	0:f900f4ea9dfd	10
Artiom	0:f900f4ea9dfd	11	#define MASK_SOL_AV 0x08
Artiom	0:f900f4ea9dfd	12	#define MASK_SOL_AR 0x80
Artiom	0:f900f4ea9dfd	13
Artiom	0:f900f4ea9dfd	14	#define MASK_AV_DROIT 0x01
Artiom	0:f900f4ea9dfd	15	#define MASK_AR_DROIT 0x10
Artiom	0:f900f4ea9dfd	16
Artiom	0:f900f4ea9dfd	17	#define MASK_AV_DROIT_GAUCHE 0x05
Artiom	0:f900f4ea9dfd	18	#define MASK_AR_DROIT_GAUCHE 0x50
Artiom	0:f900f4ea9dfd	19
Artiom	0:f900f4ea9dfd	20
Artiom	0:f900f4ea9dfd	21	char status_pompe=0;
Artiom	0:f900f4ea9dfd	22	bool flag_ascenseur = 0;
Artiom	0:f900f4ea9dfd	23
Artiom	0:f900f4ea9dfd	24	char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom	0:f900f4ea9dfd	25	char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom	0:f900f4ea9dfd	26	char fsol_avant=0,fsol_arriere=0;
Artiom	0:f900f4ea9dfd	27	char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom	0:f900f4ea9dfd	28	char fbalance_avant=0,fbalance_arriere=0;
Artiom	0:f900f4ea9dfd	29	char favant_relache=0,farriere_relache=0;
Artiom	0:f900f4ea9dfd	30	char faccelerateur_avant=0,faccelerateur_arriere=0;
Artiom	0:f900f4ea9dfd	31
Artiom	0:f900f4ea9dfd	32
Artiom	0:f900f4ea9dfd	33	DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
Artiom	0:f900f4ea9dfd	34	DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
Artiom	0:f900f4ea9dfd	35	DigitalIn presence_droit(PB_5);
Artiom	0:f900f4ea9dfd	36	DigitalIn presence_gauche(PB_4);
Artiom	0:f900f4ea9dfd	37
Artiom	0:f900f4ea9dfd	38
Artiom	0:f900f4ea9dfd	39	char buffer_couleur_bas[SIZE_FIFO];
Artiom	0:f900f4ea9dfd	40	unsigned char FIFO_couleur_ecriture=0;
Artiom	0:f900f4ea9dfd	41	signed char FIFO_couleur_lecture=0;
Artiom	0:f900f4ea9dfd	42
Artiom	0:f900f4ea9dfd	43
Artiom	0:f900f4ea9dfd	44
Artiom	0:f900f4ea9dfd	45
Artiom	0:f900f4ea9dfd	46
Artiom	0:f900f4ea9dfd	47	void gabarit_gros_robot(void)
Artiom	0:f900f4ea9dfd	48	{
Artiom	0:f900f4ea9dfd	49	uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom	0:f900f4ea9dfd	50
Artiom	0:f900f4ea9dfd	51	uint16_t pos_av_centre[2] = {550,600};//470,350
Artiom	0:f900f4ea9dfd	52
Artiom	0:f900f4ea9dfd	53
Artiom	0:f900f4ea9dfd	54	int speed=100;
Artiom	0:f900f4ea9dfd	55
Artiom	0:f900f4ea9dfd	56	deverouillage_torque();
Artiom	0:f900f4ea9dfd	57	//positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom	0:f900f4ea9dfd	58
Artiom	0:f900f4ea9dfd	59	}
Artiom	0:f900f4ea9dfd	60
Artiom	0:f900f4ea9dfd	61
Artiom	0:f900f4ea9dfd	62	void fifo_couleur(void)
Artiom	0:f900f4ea9dfd	63	{
Artiom	0:f900f4ea9dfd	64
Artiom	0:f900f4ea9dfd	65	typedef enum {n_atome, atome, tmp} type_etat ;
Artiom	0:f900f4ea9dfd	66	static type_etat etat = n_atome;
Artiom	0:f900f4ea9dfd	67
Artiom	0:f900f4ea9dfd	68	int etat_cap = !couleur_bas[0] + !couleur_bas[1]2 + !couleur_bas[2]2;
Artiom	0:f900f4ea9dfd	69	switch(etat) {
Artiom	0:f900f4ea9dfd	70	case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom	0:f900f4ea9dfd	71	if(etat_cap) {
Artiom	0:f900f4ea9dfd	72	buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom	0:f900f4ea9dfd	73	flag_ascenseur = 1;
Artiom	0:f900f4ea9dfd	74	if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
Artiom	0:f900f4ea9dfd	75	if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
Artiom	0:f900f4ea9dfd	76	positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu
Artiom	0:f900f4ea9dfd	77	} else {
Artiom	0:f900f4ea9dfd	78	positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert
Artiom	0:f900f4ea9dfd	79	}
Artiom	0:f900f4ea9dfd	80	}
Artiom	0:f900f4ea9dfd	81	//pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom	0:f900f4ea9dfd	82	FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom	0:f900f4ea9dfd	83	etat = tmp;
Artiom	0:f900f4ea9dfd	84	}
Artiom	0:f900f4ea9dfd	85	break;
Artiom	0:f900f4ea9dfd	86
Artiom	0:f900f4ea9dfd	87	case atome : //on stocke dans la fifo la couleur de l'atome
Artiom	0:f900f4ea9dfd	88	buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom	0:f900f4ea9dfd	89	flag_ascenseur = 1;
Artiom	0:f900f4ea9dfd	90	/*if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
Artiom	0:f900f4ea9dfd	91	positionControl(doigt,384,1,BLED_ON,2); //position herkulex stockage bleu
Artiom	0:f900f4ea9dfd	92	} else {
Artiom	0:f900f4ea9dfd	93	positionControl(doigt,640,1,GLED_ON,2); //position herkulex stockage rouge/vert
Artiom	0:f900f4ea9dfd	94	}*/
Artiom	0:f900f4ea9dfd	95	//pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom	0:f900f4ea9dfd	96	FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom	0:f900f4ea9dfd	97	etat = tmp;
Artiom	0:f900f4ea9dfd	98	break;
Artiom	0:f900f4ea9dfd	99
Artiom	0:f900f4ea9dfd	100	case tmp : //on attend que l'atome traité soit totalement passé
Artiom	0:f900f4ea9dfd	101	if(!etat_cap) {
Artiom	0:f900f4ea9dfd	102	etat = n_atome;
Artiom	0:f900f4ea9dfd	103	}
Artiom	0:f900f4ea9dfd	104	break;
Artiom	0:f900f4ea9dfd	105
Artiom	0:f900f4ea9dfd	106	}
Artiom	0:f900f4ea9dfd	107
Artiom	0:f900f4ea9dfd	108	}
Artiom	0:f900f4ea9dfd	109
Artiom	0:f900f4ea9dfd	110
Artiom	0:f900f4ea9dfd	111	void ascenseur(void)
Artiom	0:f900f4ea9dfd	112	{
Artiom	0:f900f4ea9dfd	113	typedef enum {init, atome, tmp} type_etat;
Artiom	0:f900f4ea9dfd	114	static type_etat etat = init;
Artiom	0:f900f4ea9dfd	115
Artiom	0:f900f4ea9dfd	116	int etat_cap = !couleur_haut[0] + !couleur_haut[1]2 + !couleur_haut[2]2;
Artiom	0:f900f4ea9dfd	117	static int flag_sortie = 0;
Artiom	0:f900f4ea9dfd	118	switch(etat) {
Artiom	0:f900f4ea9dfd	119	case init :
Artiom	0:f900f4ea9dfd	120	//on attend le premier atome et place le herkulex en fonction
Artiom	0:f900f4ea9dfd	121	if(flag_ascenseur) {
Artiom	0:f900f4ea9dfd	122	/*if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {
Artiom	0:f900f4ea9dfd	123	positionControl(doigt,384,1,BLED_ON,2); //position herkulex stockage bleu
Artiom	0:f900f4ea9dfd	124	} else {
Artiom	0:f900f4ea9dfd	125	positionControl(doigt,640,1,GLED_ON,2); //position herkulex stockage rouge/vert
Artiom	0:f900f4ea9dfd	126	}*/
Artiom	0:f900f4ea9dfd	127	etat = atome;
Artiom	0:f900f4ea9dfd	128	}
Artiom	0:f900f4ea9dfd	129	break;
Artiom	0:f900f4ea9dfd	130
Artiom	0:f900f4ea9dfd	131	case atome :
Artiom	0:f900f4ea9dfd	132	//on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
Artiom	0:f900f4ea9dfd	133	if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
Artiom	0:f900f4ea9dfd	134	if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) {
Artiom	0:f900f4ea9dfd	135	/*if(flag_sortie) {
Artiom	0:f900f4ea9dfd	136	if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
Artiom	0:f900f4ea9dfd	137	positionControl(doigt,384,10,BLED_ON,2);
Artiom	0:f900f4ea9dfd	138	} else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
Artiom	0:f900f4ea9dfd	139	positionControl(doigt,640,10,GLED_ON,2);
Artiom	0:f900f4ea9dfd	140	}
Artiom	0:f900f4ea9dfd	141	flag_sortie = 0;
Artiom	0:f900f4ea9dfd	142	}
Artiom	0:f900f4ea9dfd	143	pc.printf("--fifo lecture %d\n", buffer_couleur_bas[FIFO_couleur_lecture]);
Artiom	0:f900f4ea9dfd	144	//on change la position du herkulex si le prochain atome est différent
Artiom	0:f900f4ea9dfd	145	if(buffer_couleur_bas[FIFO_couleur_lecture] != buffer_couleur_bas[FIFO_couleur_lecture+1] ) {
Artiom	0:f900f4ea9dfd	146	flag_sortie = 1;
Artiom	0:f900f4ea9dfd	147	}*/
Artiom	0:f900f4ea9dfd	148	if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
Artiom	0:f900f4ea9dfd	149	positionControl(doigt,384,10,BLED_ON,2);
Artiom	0:f900f4ea9dfd	150	} else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
Artiom	0:f900f4ea9dfd	151	positionControl(doigt,640,10,GLED_ON,2);
Artiom	0:f900f4ea9dfd	152	}
Artiom	0:f900f4ea9dfd	153	etat = tmp;
Artiom	0:f900f4ea9dfd	154	} else if(etat_cap != 0) {
Artiom	0:f900f4ea9dfd	155	//pc.printf("--fifo lecture %d --- vu : %d\n", buffer_couleur_bas[FIFO_couleur_lecture], etat_cap);
Artiom	0:f900f4ea9dfd	156	etat = tmp;
Artiom	0:f900f4ea9dfd	157	}
Artiom	0:f900f4ea9dfd	158	}
Artiom	0:f900f4ea9dfd	159	break;
Artiom	0:f900f4ea9dfd	160
Artiom	0:f900f4ea9dfd	161	case tmp :
Artiom	0:f900f4ea9dfd	162	//on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom	0:f900f4ea9dfd	163	if(etat_cap == 0) {
Artiom	0:f900f4ea9dfd	164	//pc.printf("\t\tfifo + 1\n");
Artiom	0:f900f4ea9dfd	165	FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
Artiom	0:f900f4ea9dfd	166	etat = atome;
Artiom	0:f900f4ea9dfd	167	}
Artiom	0:f900f4ea9dfd	168	break;
Artiom	0:f900f4ea9dfd	169	}
Artiom	0:f900f4ea9dfd	170	}
kyxstark	1:af508f84a079	171
Artiom	0:f900f4ea9dfd	172	void presentoir_avant(void)
Artiom	0:f900f4ea9dfd	173	{
Artiom	0:f900f4ea9dfd	174	}
Artiom	0:f900f4ea9dfd	175
Artiom	0:f900f4ea9dfd	176
Artiom	0:f900f4ea9dfd	177	void automate_ventouse_presentoir_avant(void)
Artiom	0:f900f4ea9dfd	178	{
Artiom	0:f900f4ea9dfd	179	}
Artiom	0:f900f4ea9dfd	180
Artiom	0:f900f4ea9dfd	181	void automate_ventouse_relache_avant(void)
Artiom	0:f900f4ea9dfd	182	{
Artiom	0:f900f4ea9dfd	183
Artiom	0:f900f4ea9dfd	184
Artiom	0:f900f4ea9dfd	185	}
Artiom	0:f900f4ea9dfd	186
Artiom	0:f900f4ea9dfd	187	void goldenium_avant(void)
Artiom	0:f900f4ea9dfd	188	{
Artiom	0:f900f4ea9dfd	189
Artiom	0:f900f4ea9dfd	190	}
Artiom	0:f900f4ea9dfd	191
Artiom	0:f900f4ea9dfd	192	void automate_ventouse_goldenium_avant (void)
Artiom	0:f900f4ea9dfd	193	{
Artiom	0:f900f4ea9dfd	194	}
Artiom	0:f900f4ea9dfd	195
Artiom	0:f900f4ea9dfd	196	void accelerateur_avant(void)
Artiom	0:f900f4ea9dfd	197	{
Artiom	0:f900f4ea9dfd	198	}
Artiom	0:f900f4ea9dfd	199
Artiom	0:f900f4ea9dfd	200	void automate_ventouse_accelerateur_avant(void)
Artiom	0:f900f4ea9dfd	201	{
Artiom	0:f900f4ea9dfd	202
kyxstark	1:af508f84a079	203	}
Artiom	0:f900f4ea9dfd	204	#endif

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	23 May 2019
Imports:	1
Forks:	0
Commits:	23
Dependents:	4
Dependencies:	0
Followers:	96

actions_Gr.cpp@1:af508f84a079, 2019-05-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning