librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 16:06463f24811d
- Parent:
- 15:1fefa6b1569b
- Child:
- 17:5973f13f44e2
diff -r 1fefa6b1569b -r 06463f24811d actions_Gr.cpp --- a/actions_Gr.cpp Mon May 27 22:59:28 2019 +0000 +++ b/actions_Gr.cpp Tue May 28 18:02:59 2019 +0000 @@ -51,6 +51,8 @@ unsigned char FIFO_couleur_ecriture=0; signed char FIFO_couleur_lecture=0; char status_contact=0; +int nbPaletsVerts = 0, nbPaletsBleus = 0; + void roue(int allume) @@ -73,7 +75,8 @@ static int16_t pos = 0, consigne_pos, consigne_roue; static int16_t previous_pos = getPos(stockage_G,1); static int cpt_vider = 0; - int flag_debut; + int flag_debut = 0; + static int pos_vide[5] = {800, 200, 600, 1000, 400}; /*if(flag_vide_vert_rouge) { etat = init; @@ -120,7 +123,7 @@ case pret : if(flag_vide_vert_rouge) { flag_vide_vert_rouge =0; - flag_debut = 1; + flag_debut = 0; etat = vide; } else if((status_contact & MASK_CT_GAUCHE)) { consigne_pos = (getPos(stockage_G,1)+550); @@ -153,27 +156,31 @@ break; case retour : pos = getPos(stockage_G,1) ; - if(flag_debut){ - pos += 255; - flag_debut = 0; - } wait_us(500); if(pos< (consigne_pos+5)) { etat = pret; //pc.printf("PRET\n"); } break; - + /* case vide: pos = getPos(stockage_G,1); - consigne_pos = (pos+605)%1105; + if(!flag_debut){ + flag_debut =1; + consigne_pos = (pos+420)%1105; + } + else{ + + consigne_pos = (pos+400)%1105; + } + pc.printf("\n1 pos : %d consigne %d\n",pos, consigne_pos); //ACTIVERMOTEUR roue(1); //compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1); - if(cpt_vider<6){ - compteTour(stockage_G,+1023,0,consigne_pos,BLED_ON,1); + if(cpt_vider<7){ + compteTour(stockage_G,+200,0,consigne_pos,BLED_ON,1); //positionControl(stockage_G,consigne_pos,1,BLED_ON,1); pc.printf("TMP_VIDE\n"); etat = tmp_vide; @@ -191,10 +198,9 @@ pos = getPos(stockage_G,1) ; wait_us(500); - if(pos> (consigne_pos-5)) { + if(pos< (consigne_pos+5)) { cpt_vider +=1; - consigne_roue = 850-55; - positionControl(stockage_G,(pos),40,BLED_ON,1); + consigne_roue = 815; positionControl(roue_G,consigne_roue,40,BLED_ON,1); etat = fin_vide; pc.printf("FIN_VIDE\n"); @@ -214,6 +220,50 @@ } break; + }*/ + case vide: + + velocityControl(stockage_G, 400, BLED_ON,1); + wait_ms(200); + roue(1); + + if(cpt_vider<9){ + etat = tmp_vide; + //pc.printf("TMP_VIDE\n"); + + }else{ + etat = init; + cpt_vider = 0; + roue(0); + //pc.printf("INIT\n"); + } + break; + + case tmp_vide: + pos = getPos(stockage_G,1); + wait_us(100); + if(pos >= pos_vide[cpt_vider%5] && pos <= (pos_vide[cpt_vider%5]+20)){ //if(pos == pos_vide[cpt_vider%5]) + velocityControl(stockage_G, 0, BLED_ON,1); + positionControl(stockage_G,pos-30,1,BLED_ON,1); + //pc.printf("pos :%d\n", pos); + consigne_roue = 840; + positionControl(roue_G,consigne_roue,1,BLED_ON,1); + etat = fin_vide; + } + + break; + + case fin_vide: + pos = getPos(roue_G,1) ; + wait_us(500); + if(pos> (consigne_roue-5)) { + positionControl(roue_G,750,50,BLED_ON,1); + verification(); + cpt_vider ++; + etat = vide; + } + + break; } } @@ -224,6 +274,7 @@ static type_etat etat = init; static int16_t pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_D,3); + int vInit = 512; /* if(flag_vide_bleu){ etat = init; @@ -233,7 +284,7 @@ switch(etat) { case init : - velocityControl(stockage_D,512,BLED_ON,3); + velocityControl(stockage_D,vInit,BLED_ON,3); wait_us(500); pos = getPos(stockage_D,3); if(previous_pos != pos) { @@ -245,15 +296,15 @@ case etalonnage : pos = getPos(stockage_D,3); wait_us(500); - if(( (21<pos && pos<428) || (572<pos && pos<700) ) and (status_contact & MASK_FC_DROIT)) { + if(( /*(21<pos && pos<428) || */(572<pos && pos<700) ) and (status_contact & MASK_FC_DROIT)) { velocityControl(stockage_D,0,GLED_ON,3); wait_ms(300); pos = getPos(stockage_D,3) ; - pc.printf("pos %d\n", pos); + //pc.printf("pos %d\n", pos); //positionControl(stockage_D,pos-2000,1,BLED_ON,3); consigne_pos =pos-550; if(consigne_pos<0) consigne_pos += 1105; - pc.printf("consigne : %d \n",consigne_pos); + //pc.printf("consigne : %d \n",consigne_pos); compteTour(stockage_D,-1023,1,consigne_pos,BLED_ON,3); //pc.printf("pos fin tour : %d \n",getPos(stockage_D,3)); wait_us(500); @@ -277,9 +328,11 @@ if(flag_vide_bleu) { flag_vide_bleu =0; - etat = vide; + vInit = 1023; + etat = init; + //pc.printf("vide\n"); } else if((status_contact & MASK_CT_DROIT)) { - //pc.printf("%d\n",pos); + ///pc.printf("%d\n",pos); consigne_pos = getPos(stockage_D,3)+ 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); @@ -325,7 +378,7 @@ wait_us(500); if(pos> (consigne_pos-5)) { etat = init; - //pc.printf("PRET\n"); + //pc.printf("INIT\n"); } break; @@ -337,8 +390,9 @@ typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour, vide, fin_vide} type_etat ; static type_etat etat = init; - static int16_t pos = 0, consigne_pos; + static int16_t pos = 0, consigne_pos, pos_pret; static int16_t previous_pos = getPos(stockage_G,1); + static int vInit = -512; /* if(flag_vide_bleu){ etat = init; @@ -347,7 +401,7 @@ switch(etat) { case init : - velocityControl(stockage_G,-512,BLED_ON,1); + velocityControl(stockage_G,vInit,BLED_ON,1); wait_us(500); pos = getPos(stockage_G,1); wait_us(500); @@ -359,17 +413,17 @@ case etalonnage : pos = getPos(stockage_G,1); - if(((21<pos && pos<228) || (572<pos && pos<980)) and (status_contact & MASK_FC_GAUCHE)) { //!!!!!!!!!!!!!changer les bornes + if(((21<pos && pos<228)/* || (572<pos && pos<980)*/) and (status_contact & MASK_FC_GAUCHE)) { //!!!!!!!!!!!!!changer les bornes velocityControl(stockage_G,0,GLED_ON,1); wait_ms(500); pos = getPos(stockage_G,1) ; - //pc.printf("pos %d\n",pos); + //pc.printf("pos depart: %d\n",pos); wait_us(500); - consigne_pos = pos + 500; - if(consigne_pos >1105) consigne_pos -= 1105; + pos_pret = pos + 500; + if(pos_pret >1105) pos_pret -= 1105; - pc.printf("pos : %d consigne : %d\n",pos,consigne_pos); - compteTour(stockage_G,1023,1,consigne_pos,BLED_ON,1); + //pc.printf("pos : %d consigne : %d\n",pos,pos_pret); + compteTour(stockage_G,1023,1,pos_pret,BLED_ON,1); wait_us(500); etat = tmp_pret; ////pc.printf("TMP_PRET\n"); @@ -379,7 +433,7 @@ case tmp_pret : pos = getPos(stockage_G,1) ; wait_us(500); - if(pos> (consigne_pos-5)) { + if(pos> (pos_pret-5)) { etat = pret; //pc.printf("PRET\n"); } @@ -388,13 +442,17 @@ case pret : if(flag_vide_bleu) { flag_vide_bleu =0; - etat = vide; + vInit = -1023; + etat = init; } else if((status_contact & MASK_CT_GAUCHE)) { - //pc.printf("%d\n",pos); - consigne_pos = getPos(stockage_G,1)- 550; + consigne_pos = (pos_pret- 550); + if(consigne_pos<0)consigne_pos+=1105; wait_us(500); deverouillage_torque_convoyeurs_gauche(); wait_us(500); + //pc.printf("pos:%d consigne:%d\n",pos, consigne_pos); + + //compteTour(stockage_G,-1023,0,consigne_pos,BLED_ON,1); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); wait_us(500); etat = pousse; @@ -407,10 +465,11 @@ wait_us(500); if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) { //pc.printf("%d\n",pos); - consigne_pos = pos + 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); - positionControl(stockage_G,consigne_pos,1,BLED_ON,1); + + //compteTour(stockage_G,1023,0,pos_pret,BLED_ON,1); + positionControl(stockage_G,pos_pret,1,BLED_ON,1); wait_us(500); etat = retour; //pc.printf("RETOUR\n"); @@ -421,7 +480,7 @@ case retour : pos = getPos(stockage_G,1) ; wait_us(500); - if(pos> (consigne_pos-5)) { + if(pos> (pos_pret-5)) { etat = pret; //pc.printf("PRET\n"); } @@ -447,10 +506,12 @@ void convoyeur_droit_violet(void) { - typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ; + typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide,tmp_vide} type_etat ; static type_etat etat = init; - static int16_t pos = 0, consigne_pos; + static int16_t pos = 0, consigne_pos,consigne_roue; static int16_t previous_pos = getPos(stockage_D,3); + static int cpt_vider=0; + static int pos_vide[5] = {400, 1000, 600, 200, 800}; /* if(flag_vide_vert_rouge){ etat = init; @@ -535,7 +596,7 @@ // pc.printf("PRET\n"); } break; - +/* case vide: consigne_pos = getPos(stockage_D,3); //pc.printf("consigne %d",consigne_pos); @@ -553,6 +614,50 @@ } break; + */ + case vide: + + velocityControl(stockage_D, -400, BLED_ON,3); + wait_ms(200); + roue(1); + + if(cpt_vider<9){ + etat = tmp_vide; + //pc.printf("TMP_VIDE\n"); + + }else{ + etat = init; + cpt_vider = 0; + roue(0); + //pc.printf("INIT\n"); + } + break; + + case tmp_vide: + pos = getPos(stockage_D,3); + wait_us(100); + if(pos <= pos_vide[cpt_vider%5] && pos >= (pos_vide[cpt_vider%5]-20)){ //if(pos == pos_vide[cpt_vider%5]) + velocityControl(stockage_D, 0, BLED_ON,3); + positionControl(stockage_D,pos+30,1,BLED_ON,3); + //pc.printf("pos :%d\n", pos); + consigne_roue = 225; + positionControl(roue_D,consigne_roue,1,BLED_ON,3); + etat = fin_vide; + } + + break; + + case fin_vide: + pos = getPos(roue_D,3) ; + wait_us(500); + if(pos> (consigne_roue+5)) { + positionControl(roue_D,374,50,BLED_ON,3); + verification(); + cpt_vider ++; + etat = vide; + } + + break; } } void gabarit_robot(void) @@ -657,8 +762,11 @@ SendCharCan(HACHEUR_ID_COUROIES,0); etat = init; } + if(etat_cap != 0) { cpt = 0; + if(etat_cap == 1)nbPaletsBleus ++; + else if(etat_cap == 2) nbPaletsVerts ++; oriente_doigt(etat_cap); etat = tmp; } @@ -672,6 +780,7 @@ //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture if(etat_cap == 0) { memo_FIFO_couleur_lecture = FIFO_couleur_lecture; + FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO; if(FIFO_couleur_lecture == FIFO_couleur_ecriture) { //pc.printf("Fifo vide\n");