librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 14:bf2810b95f25
- Parent:
- 13:0b5310c6dc44
- Child:
- 15:1fefa6b1569b
diff -r 0b5310c6dc44 -r bf2810b95f25 actions_Gr.cpp --- a/actions_Gr.cpp Sun May 26 11:26:06 2019 +0000 +++ b/actions_Gr.cpp Sun May 26 12:46:41 2019 +0000 @@ -37,6 +37,7 @@ char favant_relache=0,farriere_relache=0; char faccelerateur_avant=0,faccelerateur_arriere=0; +int flag_vide_vert_rouge=0,flag_vide_bleu=0; DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1 DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2 @@ -47,116 +48,143 @@ char status_contact=0; -void convoyeur_gauche_jaune(void){ - - typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ; +void convoyeur_gauche_jaune(void) +{ + + typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ; static type_etat etat = init; static int16_t pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_G,1); - - - switch(etat){ - - case init : + /*if(flag_vide_vert_rouge) + { + etat = init; + flag_vide_vert_rouge = 0; + }*/ + + switch(etat) { + + case init : velocityControl(stockage_G,-512,BLED_ON,1); pos = getPos(stockage_G,1); - if(previous_pos != pos){ + if(previous_pos != pos) { etat = etalonnage; - //pc.printf("ETALONNAGE\n"); - + //pc.printf("ETALONNAGE\n"); + } break; case etalonnage : pos = getPos(stockage_G,1); wait_us(500); - if( ( (21<pos && pos<260) /*|| (404<pos && pos<812)*/ ) and (status_contact & MASK_FC_GAUCHE)){ + if( ( (21<pos && pos<260) /*|| (404<pos && pos<812)*/ ) and (status_contact & MASK_FC_GAUCHE)) { velocityControl(stockage_G,0,GLED_ON,1); wait_ms(300); pos = getPos(stockage_G,1) ; consigne_pos = pos + 180; positionControl(stockage_G,consigne_pos,1,BLED_ON,1); etat = tmp_pret; - //pc.printf("TMP_PRET\n"); + //pc.printf("TMP_PRET\n"); } break; - + case tmp_pret : pos = getPos(stockage_G,1); wait_us(500); - if(pos> (consigne_pos-5)){ + if(pos> (consigne_pos-5)) { //pc.printf("pos pret %d\n", pos); - etat = pret; - //pc.printf("PRET\n"); - - } + etat = pret; + //pc.printf("PRET\n"); + + } break; - - case pret : - if((status_contact & MASK_CT_GAUCHE)){ + + case pret : + if(flag_vide_vert_rouge) { + flag_vide_vert_rouge =0; + etat = vide; + } else if((status_contact & MASK_CT_GAUCHE)) { consigne_pos = (getPos(stockage_G,1)+550); - + //if(consigne_pos>1105) consigne_pos -= 1105; //pc.printf("consigne pousse %d\n", consigne_pos); deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); //compteTour(stockage_G,+1023,0,consigne_pos,BLED_ON,1); - + etat = pousse; - //pc.printf("POUSSE\n"); + //pc.printf("POUSSE\n"); } break; - + case pousse : pos = getPos(stockage_G,1); //pc.printf("%d\n", pos); wait_us(500); - if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){ - consigne_pos = pos - 550; + if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0) { + consigne_pos = pos - 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); etat = retour; - //pc.printf("RETOUR\n"); + //pc.printf("RETOUR\n"); } - + break; case retour : pos = getPos(stockage_G,1) ; wait_us(500); - if(pos< (consigne_pos+5)){ - etat = pret; - //pc.printf("PRET\n"); - } + if(pos< (consigne_pos+5)) { + etat = pret; + //pc.printf("PRET\n"); + } + break; + case vide: + consigne_pos = getPos(stockage_G,1)+500; + compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1); + etat = fin_vide; break; - } -} + case fin_vide: + pos = getPos(stockage_G,1) ; + wait_us(500); + if(pos> (consigne_pos-5)) { + etat = init; + //pc.printf("PRET\n"); + } -void convoyeur_droit_jaune(void){ - - typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ; + break; + } +} + +void convoyeur_droit_jaune(void) +{ + + 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 previous_pos = getPos(stockage_D,3); - - - switch(etat){ - - case init : + /* + if(flag_vide_bleu){ + etat = init; + flag_vide_bleu = 0; + }*/ + + switch(etat) { + + case init : velocityControl(stockage_D,512,BLED_ON,3); wait_us(500); pos = getPos(stockage_D,3); - if(previous_pos != pos){ + if(previous_pos != pos) { etat = etalonnage; - //pc.printf("ETALONNAGE\n"); + //pc.printf("ETALONNAGE\n"); } break; 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) ; @@ -172,20 +200,24 @@ //pc.printf("TMP_PRET\n"); } break; - + case tmp_pret : pos = getPos(stockage_D,3) ; - wait_us(500); - if(pos < (consigne_pos+5)){ - + wait_us(500); + if(pos < (consigne_pos+5)) { + //pc.printf("pos fin %d\n", pos); - etat = pret; - //pc.printf("PRET\n"); - } + etat = pret; + //pc.printf("PRET\n"); + } break; - - case pret : - if((status_contact & MASK_CT_DROIT)){ + + case pret : + + if(flag_vide_bleu) { + flag_vide_bleu =0; + etat = vide; + } else if((status_contact & MASK_CT_DROIT)) { //pc.printf("%d\n",pos); consigne_pos = getPos(stockage_D,3)+ 550; deverouillage_torque_convoyeurs_droit(); @@ -196,13 +228,13 @@ //pc.printf("POUSSE\n"); } break; - + case pousse : pos = getPos(stockage_D,3); - wait_us(500); - if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0){ + wait_us(500); + if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0) { //pc.printf("%d\n",pos); - consigne_pos = pos - 550; + consigne_pos = pos - 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); @@ -210,51 +242,70 @@ etat = retour; //pc.printf("RETOUR\n"); } - + break; - + case retour : pos = getPos(stockage_D,3) ; wait_us(500); - if(pos< (consigne_pos+5)){ - etat = pret; - //pc.printf("PRET\n"); - } + if(pos< (consigne_pos+5)) { + etat = pret; + //pc.printf("PRET\n"); + } + break; + + case vide: + consigne_pos = getPos(stockage_D,3); + compteTour(stockage_D,+1023,2,consigne_pos,BLED_ON,3); + etat = fin_vide; break; - } + case fin_vide: + pos = getPos(stockage_D,3) ; + wait_us(500); + if(pos> (consigne_pos-5)) { + etat = init; + //pc.printf("PRET\n"); + } + + break; + } } -void convoyeur_gauche_violet(void){ - - typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ; +void convoyeur_gauche_violet(void) +{ + + 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 previous_pos = getPos(stockage_G,1); - - - switch(etat){ - - case init : + /* + if(flag_vide_bleu){ + etat = init; + flag_vide_bleu = 0; + }*/ + switch(etat) { + + case init : velocityControl(stockage_G,-512,BLED_ON,1); - wait_us(500); + wait_us(500); pos = getPos(stockage_G,1); - wait_us(500); - if(previous_pos != pos){ + wait_us(500); + if(previous_pos != pos) { etat = etalonnage; - // pc.printf("ETALONNAGE\n"); + // pc.printf("ETALONNAGE\n"); } break; 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); wait_us(500); consigne_pos = pos + 500; - + //pc.printf("consigne pos %d\n",consigne_pos); if(consigne_pos >1105) consigne_pos -= 1105; compteTour(stockage_G,1023,2,consigne_pos,BLED_ON,1); @@ -263,18 +314,21 @@ ////pc.printf("TMP_PRET\n"); } break; - + case tmp_pret : pos = getPos(stockage_G,1) ; - wait_us(500); - if(pos> (consigne_pos-5)){ - etat = pret; - //pc.printf("PRET\n"); - } + wait_us(500); + if(pos> (consigne_pos-5)) { + etat = pret; + //pc.printf("PRET\n"); + } break; - - case pret : - if((status_contact & MASK_CT_GAUCHE)){ + + case pret : + if(flag_vide_bleu) { + flag_vide_bleu =0; + etat = vide; + } else if((status_contact & MASK_CT_GAUCHE)) { //pc.printf("%d\n",pos); consigne_pos = getPos(stockage_G,1)- 550; wait_us(500); @@ -286,13 +340,13 @@ //pc.printf("POUSSE\n"); } break; - + case pousse : pos = getPos(stockage_G,1); - wait_us(500); - if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){ + wait_us(500); + if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) { //pc.printf("%d\n",pos); - consigne_pos = pos + 550; + consigne_pos = pos + 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); @@ -300,36 +354,56 @@ etat = retour; //pc.printf("RETOUR\n"); } - + break; - + case retour : pos = getPos(stockage_G,1) ; - wait_us(500); - if(pos> (consigne_pos-5)){ - etat = pret; - //pc.printf("PRET\n"); - } + wait_us(500); + if(pos> (consigne_pos-5)) { + etat = pret; + //pc.printf("PRET\n"); + } break; - } + + case vide: + consigne_pos = getPos(stockage_G,1); + compteTour(stockage_G,-1023,2,consigne_pos,BLED_ON,1); + etat = fin_vide; + break; + case fin_vide: + pos = getPos(stockage_G,1) ; + wait_us(500); + if(pos< (consigne_pos+5)) { + etat = init; + //pc.printf("PRET\n"); + } + + break; + } } -void convoyeur_droit_violet(void){ - - typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ; +void convoyeur_droit_violet(void) +{ + + typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ; static type_etat etat = init; static int16_t pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_D,3); - - - switch(etat){ - - case init : + /* + if(flag_vide_vert_rouge){ + etat = init; + flag_vide_vert_rouge = 0; + } */ + + switch(etat) { + + case init : velocityControl(stockage_D,+512,BLED_ON,3); - wait_us(500); + wait_us(500); pos = getPos(stockage_D,3); - wait_us(500); - if(previous_pos != pos){ + wait_us(500); + if(previous_pos != pos) { //pc.printf("ETALONNAGE\n"); etat = etalonnage; } @@ -337,9 +411,9 @@ case etalonnage : pos = getPos(stockage_D,3); - wait_us(500); + wait_us(500); //pc.printf("%d\n", pos); - if( /*(21<pos && pos<260) ||*/ (754<pos && pos<812) and (status_contact & MASK_FC_DROIT)){ //!!!!!!!!!!!!!changer les bornes + if( /*(21<pos && pos<260) ||*/ (754<pos && pos<812) and (status_contact & MASK_FC_DROIT)) { //!!!!!!!!!!!!!changer les bornes velocityControl(stockage_D,0,GLED_ON,3); wait_ms(500); consigne_pos = getPos(stockage_D,3) - 180 ; @@ -347,58 +421,79 @@ positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = tmp_pret; - // pc.printf("TMP_PRET\n"); + // pc.printf("TMP_PRET\n"); } break; - - case tmp_pret : + + case tmp_pret : pos = getPos(stockage_D,3) ; - wait_us(500); - if(pos< (consigne_pos+5)){ - etat = pret; - pc.printf("PRET\n"); - } + wait_us(500); + if(pos< (consigne_pos+5)) { + etat = pret; + //pc.printf("PRET\n"); + } break; - - case pret : - if((status_contact & MASK_CT_DROIT)){ + + case pret : + if(flag_vide_vert_rouge) { + flag_vide_vert_rouge =0; + etat = vide; + } else if((status_contact & MASK_CT_DROIT)) { consigne_pos = getPos(stockage_D,3) - 550; wait_us(500); deverouillage_torque_convoyeurs_droit(); wait_us(500); - pc.printf("consigne %d", consigne_pos); + //pc.printf("consigne %d", consigne_pos); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = pousse; - pc.printf("POUSSE\n"); + //pc.printf("POUSSE\n"); } break; - + case pousse : pos = getPos(stockage_D,3); - wait_us(500); - if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0){ - consigne_pos = pos + 550; - pc.printf("consigne %d", consigne_pos); + wait_us(500); + if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0) { + consigne_pos = pos + 550; + //pc.printf("consigne %d", consigne_pos); deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = retour; - pc.printf("RETOUR\n"); + //pc.printf("RETOUR\n"); } - + break; case retour : pos = getPos(stockage_D,3) ; - wait_us(500); - if(pos> (consigne_pos-5)){ - etat = pret; - pc.printf("PRET\n"); - } + wait_us(500); + if(pos> (consigne_pos-5)) { + etat = pret; + // pc.printf("PRET\n"); + } + break; + + case vide: + consigne_pos = getPos(stockage_D,3); + //pc.printf("consigne %d",consigne_pos); + compteTour(stockage_D,-1023,3,consigne_pos,BLED_ON,3); + etat = fin_vide; break; - } -} + + case fin_vide: + pos = getPos(stockage_D,3) ; + //pc.printf("pos %d", pos); + wait_us(500); + if(pos< (consigne_pos+5)) { + etat = init; + //pc.printf("PRET\n"); + } + + break; + } +} void gabarit_robot(void) { uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C}; @@ -411,7 +506,7 @@ deverouillage_torque(); positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2); wait(0.5); - + } @@ -431,7 +526,7 @@ if(FIFO_couleur_ecriture == FIFO_couleur_lecture) { oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]); } - + FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO; etat = tmp; } @@ -455,7 +550,7 @@ 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 @@ -467,8 +562,7 @@ case atome : //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO - if(etat_cap != 0) - { + if(etat_cap != 0) { oriente_doigt(etat_cap); etat = tmp; } @@ -483,9 +577,8 @@ if(etat_cap == 0) { char 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"); + if(FIFO_couleur_lecture == FIFO_couleur_ecriture) { + //pc.printf("Fifo vide\n"); if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1) oriente_doigt(2); else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2) @@ -506,7 +599,7 @@ } else if (palet == 2) {//position herkulex stockage rouge/vert positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2); SendCharCan(HACHEUR_ID_COUROIES,1); - + } } else { if(palet== 1) {//position herkulex stockage bleu