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};