librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 0:f900f4ea9dfd
- Child:
- 1:af508f84a079
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/actions_Gr.cpp Thu May 23 10:47:57 2019 +0000 @@ -0,0 +1,204 @@ +#include "actions_Gr.h" + + +#ifdef ROBOT_BIG +#define MASK_PRESENTOIR_AV 0x07 +#define MASK_PRESENTOIR_AR 0x70 + +#define MASK_GOLDENIUM_AV 0x02 +#define MASK_GOLDENIUM_AR 0x20 + +#define MASK_SOL_AV 0x08 +#define MASK_SOL_AR 0x80 + +#define MASK_AV_DROIT 0x01 +#define MASK_AR_DROIT 0x10 + +#define MASK_AV_DROIT_GAUCHE 0x05 +#define MASK_AR_DROIT_GAUCHE 0x50 + + +char status_pompe=0; +bool flag_ascenseur = 0; + +char fpresentoir_avant=0, fpresentoir_arriere=0; +char fgoldenium_avant=0, fgoldenium_arriere=0; +char fsol_avant=0,fsol_arriere=0; +char fsol_avant_relache=0,fsol_arriere_relache=0; +char fbalance_avant=0,fbalance_arriere=0; +char favant_relache=0,farriere_relache=0; +char faccelerateur_avant=0,faccelerateur_arriere=0; + + +DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1 +DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2 +DigitalIn presence_droit(PB_5); +DigitalIn presence_gauche(PB_4); + + +char buffer_couleur_bas[SIZE_FIFO]; +unsigned char FIFO_couleur_ecriture=0; +signed char FIFO_couleur_lecture=0; + + + + + +void gabarit_gros_robot(void) +{ + uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C}; + + uint16_t pos_av_centre[2] = {550,600};//470,350 + + + int speed=100; + + deverouillage_torque(); + //positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2); + +} + + +void fifo_couleur(void) +{ + + typedef enum {n_atome, atome, tmp} type_etat ; + static type_etat etat = n_atome; + + int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2; + switch(etat) { + case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO + if(etat_cap) { + buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert + flag_ascenseur = 1; + if(FIFO_couleur_ecriture == FIFO_couleur_lecture) { + if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) { + positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu + } else { + positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert + } + } + //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]); + FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO; + etat = tmp; + } + break; + + case atome : //on stocke dans la fifo la couleur de l'atome + buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert + flag_ascenseur = 1; + /*if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) { + positionControl(doigt,384,1,BLED_ON,2); //position herkulex stockage bleu + } else { + positionControl(doigt,640,1,GLED_ON,2); //position herkulex stockage rouge/vert + }*/ + //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]); + FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO; + etat = tmp; + break; + + case tmp : //on attend que l'atome traité soit totalement passé + if(!etat_cap) { + etat = n_atome; + } + break; + + } + +} + + +void ascenseur(void) +{ + typedef enum {init, atome, tmp} type_etat; + static type_etat etat = init; + + int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2; + static int flag_sortie = 0; + switch(etat) { + case init : + //on attend le premier atome et place le herkulex en fonction + if(flag_ascenseur) { + /*if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) { + positionControl(doigt,384,1,BLED_ON,2); //position herkulex stockage bleu + } else { + positionControl(doigt,640,1,GLED_ON,2); //position herkulex stockage rouge/vert + }*/ + etat = atome; + } + break; + + case atome : + //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO + if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) { + if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) { + /*if(flag_sortie) { + if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu + positionControl(doigt,384,10,BLED_ON,2); + } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert + positionControl(doigt,640,10,GLED_ON,2); + } + flag_sortie = 0; + } + pc.printf("--fifo lecture %d\n", buffer_couleur_bas[FIFO_couleur_lecture]); + //on change la position du herkulex si le prochain atome est différent + if(buffer_couleur_bas[FIFO_couleur_lecture] != buffer_couleur_bas[FIFO_couleur_lecture+1] ) { + flag_sortie = 1; + }*/ + if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu + positionControl(doigt,384,10,BLED_ON,2); + } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert + positionControl(doigt,640,10,GLED_ON,2); + } + etat = tmp; + } else if(etat_cap != 0) { + //pc.printf("--fifo lecture %d --- vu : %d\n", buffer_couleur_bas[FIFO_couleur_lecture], etat_cap); + etat = tmp; + } + } + break; + + case tmp : + //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture + if(etat_cap == 0) { + //pc.printf("\t\tfifo + 1\n"); + FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO; + etat = atome; + } + break; + } +} +/* +void presentoir_avant(void) +{ +} + + +void automate_ventouse_presentoir_avant(void) +{ +} + +void automate_ventouse_relache_avant(void) +{ + + +} + +void goldenium_avant(void) +{ + +} + +void automate_ventouse_goldenium_avant (void) +{ +} + +void accelerateur_avant(void) +{ +} + +void automate_ventouse_accelerateur_avant(void) +{ + +}*/ +#endif \ No newline at end of file