librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 9:05da4300730a
- Parent:
- 8:7bd34e838ca3
- Child:
- 10:fba0699b7a74
--- a/actions_Gr.cpp Sat May 25 16:35:36 2019 +0000 +++ b/actions_Gr.cpp Sat May 25 17:47:38 2019 +0000 @@ -222,7 +222,8 @@ case etalonnage : pos = getPos(stockage_G,1); - if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ + pc.printf("%d\n",pos); + if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){ velocityControl(stockage_G,0,GLED_ON,1); wait_ms(500); pos = getPos(stockage_G,1) ; @@ -279,7 +280,81 @@ } } +void convoyeur_droit_violet(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_D,3); + + + switch(etat){ + + case init : + velocityControl(stockage_D,+512,BLED_ON,3); + pos = getPos(stockage_D,3); + if(previous_pos != pos){ + pc.printf("ETALONNAGE\n"); + etat = etalonnage; + } + break; + case etalonnage : + pos = getPos(stockage_D,3); + pc.printf("%d\n", pos); + 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-180,1,BLED_ON,3); + previous_pos = pos; + etat = tmp_pret; + pc.printf("TMP_PRET\n"); + } + break; + + case tmp_pret : + 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)){ + pos = getPos(stockage_D,3)- 550; + deverouillage_torque_convoyeurs_droit(); + wait_us(500); + 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){ + pos = pos + 550; + deverouillage_torque_convoyeurs_droit(); + wait_us(500); + 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) { uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};