CRAC Team
librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 5:bb533bf81ee6
- Parent:
- 4:8ac0f7c17ac7
- Child:
- 6:aa800daf5ff9
--- a/actions_Gr.cpp Thu May 23 18:42:34 2019 +0000
+++ b/actions_Gr.cpp Fri May 24 19:36:22 2019 +0000
@@ -17,6 +17,12 @@
#define MASK_AV_DROIT_GAUCHE 0x05
#define MASK_AR_DROIT_GAUCHE 0x50
+#define MASK_FC_DROIT 0x01
+#define MASK_CT_DROIT 0x02
+
+#define MASK_FC_GAUCHE 0x04
+#define MASK_CT_GAUCHE 0x08
+
char status_pompe=0;
bool flag_ascenseur = 0;
@@ -32,16 +38,158 @@
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;
+char status_contact=0;
+void convoyeur_gauche(void){
+
+ typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
+ static type_etat etat = init;
+ static int16_t pos = 0;
+ static int16_t previous_pos = getPos(stockage_G,1);
+
+
+ switch(etat){
+
+ case init :
+ velocityControl(stockage_G,-512,BLED_ON,1);
+ pos = getPos(stockage_G,1);
+ if(previous_pos != pos){
+ etat = etalonnage;
+ pc.printf("ETALONNAGE\n");
+ }
+ break;
+ case etalonnage :
+ pos = getPos(stockage_G,1);
+ if(pos<100 and (status_contact & MASK_FC_GAUCHE)){
+ velocityControl(stockage_G,0,GLED_ON,1);
+ wait_ms(500);
+ pos = getPos(stockage_G,1) ;
+ positionControl(stockage_G,pos+180,1,BLED_ON,1);
+ etat = tmp_pret;
+ pc.printf("TMP_PRET\n");
+ }
+ break;
+
+ case tmp_pret :
+ previous_pos = pos;
+ pos = getPos(stockage_G,1) ;
+ if(pos> (previous_pos-5)){
+ etat = pret;
+ pc.printf("PRET\n");
+ }
+ break;
+
+ case pret :
+ if((status_contact & MASK_CT_GAUCHE)){
+ pos = getPos(stockage_G,1)+550;
+ positionControl(stockage_G,pos,1,BLED_ON,1);
+ previous_pos = pos;
+ etat = pousse;
+ pc.printf("POUSSE\n");
+ }
+ break;
+
+ case pousse :
+ pos = getPos(stockage_G,1);
+ if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){
+ pos = pos - 550;
+ positionControl(stockage_G,pos,1,BLED_ON,1);
+ previous_pos = pos;
+ etat = retour;
+ pc.printf("RETOUR\n");
+ }
+
+ break;
+ case retour :
+ pos = getPos(stockage_G,1) ;
+ if(pos> (previous_pos-5)){
+ etat = pret;
+ pc.printf("PRET\n");
+ }
+ break;
+ }
+}
+
+void convoyeur_droit(void){
+
+ typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
+ static type_etat etat = init;
+ static int16_t pos = 0;
+ static int16_t previous_pos = getPos(stockage_D,3);
+
+
+ switch(etat){
+
+ case init :
+ velocityControl(stockage_D,512,BLED_ON,3);
+ pos = getPos(stockage_D,3);
+ if(previous_pos != pos){
+ etat = etalonnage;
+ pc.printf("ETALONNAGE\n");
+ }
+ break;
+
+ case etalonnage :
+ pos = getPos(stockage_D,3);
+ if(pos>900 and (status_contact & MASK_FC_DROIT)){
+ velocityControl(stockage_D,0,GLED_ON,3);
+ wait_ms(500);
+ pos = getPos(stockage_D,3) ;
+ //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
+ compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
+ etat = tmp_pret;
+ pc.printf("TMP_PRET\n");
+ }
+ break;
+
+ case tmp_pret :
+ previous_pos = pos;
+ pos = getPos(stockage_D,3) ;
+ if(pos> (previous_pos+5)){
+ etat = pret;
+ pc.printf("PRET\n");
+ }
+ break;
+
+ case pret :
+ if((status_contact & MASK_CT_DROIT)){
+ //pc.printf("%d\n",pos);
+ pos = getPos(stockage_D,3)+ 550;
+ positionControl(stockage_D,pos,1,BLED_ON,3);
+ previous_pos = pos;
+ etat = pousse;
+ pc.printf("POUSSE\n");
+ }
+ break;
+
+ case pousse :
+ pos = getPos(stockage_D,3);
+ if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){
+ //pc.printf("%d\n",pos);
+ pos = pos - 550;
+ positionControl(stockage_D,pos,1,BLED_ON,3);
+ previous_pos = pos;
+ etat = retour;
+ pc.printf("RETOUR\n");
+ }
+
+ break;
+
+ case retour :
+ pos = getPos(stockage_D,3) ;
+ if(pos< (previous_pos+5)){
+ etat = pret;
+ pc.printf("PRET\n");
+ }
+ break;
+ }
+}
void gabarit_robot(void)
@@ -72,13 +220,14 @@
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) {
+ /* 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
- }
+ }*/
+ oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
}
- //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
+
FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
etat = tmp;
}
@@ -87,12 +236,6 @@
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;
@@ -115,16 +258,12 @@
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
- }*/
- SendCharCan(0x600,1);
+ SendCharCan(HACHEUR_ID_COUROIES,1);
etat = atome;
}
break;
@@ -132,37 +271,30 @@
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
+ oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
+ /*
+ if(cote)
+ if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
+ positionControl(doigt,640,10,BLED_ON,2);
+ } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
+ positionControl(doigt,384,10,GLED_ON,2);
+ }
+ }else{
+ 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
+ } 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;
- }
+ }else if(etat_cap != 0) {
+ 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;
}
@@ -170,6 +302,23 @@
}
}
+void oriente_doigt(int palet)
+{
+ if(cote) {
+ if(palet == 1) {//position herkulex stockage bleu
+ positionControl(doigt,640,10,BLED_ON,2);
+ } else if (palet == 2) {//position herkulex stockage rouge/vert
+ positionControl(doigt,384,10,GLED_ON,2);
+ }
+ } else {
+ if(palet== 1) {//position herkulex stockage bleu
+ positionControl(doigt,384,10,BLED_ON,2);
+ } else if (palet== 2) {//position herkulex stockage rouge/vert
+ positionControl(doigt,640,10,GLED_ON,2);
+ }
+ }
+}
+
void presentoir_avant(void)
{
uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};