librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 11:dfff2e200d0c
- Parent:
- 10:fba0699b7a74
- Child:
- 13:0b5310c6dc44
diff -r fba0699b7a74 -r dfff2e200d0c actions_Gr.cpp --- a/actions_Gr.cpp Sat May 25 18:39:49 2019 +0000 +++ b/actions_Gr.cpp Sun May 26 09:58:39 2019 +0000 @@ -51,7 +51,7 @@ 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 pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_G,1); @@ -62,61 +62,65 @@ pos = getPos(stockage_G,1); if(previous_pos != pos){ etat = etalonnage; - pc.printf("ETALONNAGE\n"); + //pc.printf("ETALONNAGE\n"); + } break; case etalonnage : pos = getPos(stockage_G,1); - if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ + wait_us(500); + if( ( (21<pos && pos<260) || (404<pos && pos<812) ) and (status_contact & MASK_FC_GAUCHE)){ velocityControl(stockage_G,0,GLED_ON,1); - wait_ms(500); + wait_ms(300); pos = getPos(stockage_G,1) ; - positionControl(stockage_G,pos+180,1,BLED_ON,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 : - previous_pos = pos; - pos = getPos(stockage_G,1) ; - if(pos> (previous_pos-5)){ + pos = getPos(stockage_G,1); + wait_us(500); + if(pos> (consigne_pos-5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); + } break; case pret : if((status_contact & MASK_CT_GAUCHE)){ - pos = getPos(stockage_G,1)+550; + consigne_pos = getPos(stockage_G,1)+550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); - positionControl(stockage_G,pos,1,BLED_ON,1); - previous_pos = pos; + positionControl(stockage_G,consigne_pos,1,BLED_ON,1); etat = pousse; - pc.printf("POUSSE\n"); + //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; + wait_us(500); + 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,pos,1,BLED_ON,1); - previous_pos = pos; + 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) ; - if(pos> (previous_pos-5)){ + wait_us(500); + if(pos< (consigne_pos+5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; } @@ -126,7 +130,7 @@ typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ; static type_etat etat = init; - static int16_t pos = 0; + static int16_t pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_D,3); @@ -134,75 +138,76 @@ case init : velocityControl(stockage_D,512,BLED_ON,3); - wait_us(500); + wait_us(500); pos = getPos(stockage_D,3); if(previous_pos != pos){ etat = etalonnage; - //pc.printf("ETALONNAGE\n"); + pc.printf("ETALONNAGE\n"); } break; case etalonnage : pos = getPos(stockage_D,3); - if(pos>600 and (status_contact & MASK_FC_DROIT)){ + wait_us(500); + if(( (21<pos && pos<428) || (572<pos && pos<980) ) and (status_contact & MASK_FC_DROIT)){ velocityControl(stockage_D,0,GLED_ON,3); - wait_ms(500); + wait_ms(300); pos = getPos(stockage_D,3) ; //positionControl(stockage_D,pos-2000,1,BLED_ON,3); - compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3); + consigne_pos =pos-500; + if(consigne_pos<0) consigne_pos += 1105; + pc.printf("pos : %d, consigne : %d \n", pos, consigne_pos); + compteTour(stockage_D,-1023,2,consigne_pos,BLED_ON,3); wait_us(500); etat = tmp_pret; - //pc.printf("TMP_PRET\n"); + pc.printf("TMP_PRET\n"); } break; case tmp_pret : - previous_pos = pos; pos = getPos(stockage_D,3) ; wait_us(500); - if(pos> (previous_pos+5)){ + if(pos < (consigne_pos+5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; case pret : if((status_contact & MASK_CT_DROIT)){ //pc.printf("%d\n",pos); - pos = getPos(stockage_D,3)+ 550; + consigne_pos = getPos(stockage_D,3)+ 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); - positionControl(stockage_D,pos,1,BLED_ON,3); + positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); - previous_pos = pos; etat = pousse; - //pc.printf("POUSSE\n"); + pc.printf("POUSSE\n"); } break; case pousse : pos = getPos(stockage_D,3); wait_us(500); - if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){ + if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0){ //pc.printf("%d\n",pos); - pos = pos - 550; + consigne_pos = pos - 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); - positionControl(stockage_D,pos,1,BLED_ON,3); + positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); - previous_pos = pos; etat = retour; - //pc.printf("RETOUR\n"); + pc.printf("RETOUR\n"); } break; case retour : pos = getPos(stockage_D,3) ; - wait_us(500); - if(pos< (previous_pos+5)){ + wait_us(500); + if(pos< (consigne_pos+5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; } @@ -212,7 +217,7 @@ typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ; static type_etat etat = init; - static int16_t pos = 0; + static int16_t pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_G,1); @@ -232,14 +237,15 @@ case etalonnage : pos = getPos(stockage_G,1); pc.printf("%d\n",pos); - if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){ + if((21<pos && pos<428) || (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) ; wait_us(500); - compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1); + consigne_pos = pos + 500; + if(consigne_pos >1105) consigne_pos -= 1105; + compteTour(stockage_G,1023,2,consigne_pos,BLED_ON,1); wait_us(500); - previous_pos = pos; etat = tmp_pret; ////pc.printf("TMP_PRET\n"); } @@ -248,7 +254,7 @@ case tmp_pret : pos = getPos(stockage_G,1) ; wait_us(500); - if(pos> (previous_pos-5)){ + if(pos> (consigne_pos-5)){ etat = pret; pc.printf("PRET\n"); } @@ -257,13 +263,12 @@ case pret : if((status_contact & MASK_CT_GAUCHE)){ //pc.printf("%d\n",pos); - pos = getPos(stockage_G,1)- 550; + consigne_pos = getPos(stockage_G,1)- 550; wait_us(500); deverouillage_torque_convoyeurs_gauche(); wait_us(500); - positionControl(stockage_G,pos,1,BLED_ON,1); + positionControl(stockage_G,consigne_pos,1,BLED_ON,1); wait_us(500); - previous_pos = pos; etat = pousse; //pc.printf("POUSSE\n"); } @@ -272,14 +277,13 @@ case pousse : pos = getPos(stockage_G,1); wait_us(500); - if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){ + if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){ pc.printf("%d\n",pos); - pos = pos + 550; + consigne_pos = pos + 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); - positionControl(stockage_G,pos,1,BLED_ON,1); + positionControl(stockage_G,consigne_pos,1,BLED_ON,1); wait_us(500); - previous_pos = pos; etat = retour; //pc.printf("RETOUR\n"); } @@ -289,7 +293,7 @@ case retour : pos = getPos(stockage_G,1) ; wait_us(500); - if(pos> (previous_pos-5)){ + if(pos> (consigne_pos-5)){ etat = pret; pc.printf("PRET\n"); } @@ -301,7 +305,7 @@ 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 pos = 0, consigne_pos; static int16_t previous_pos = getPos(stockage_D,3); @@ -322,14 +326,13 @@ pos = getPos(stockage_D,3); wait_us(500); //pc.printf("%d\n", pos); - if(pos>900 and (status_contact & MASK_FC_DROIT)){ + if( (21<pos && pos<260) || (404<pos && pos<812) and (status_contact & MASK_FC_DROIT)){ //!!!!!!!!!!!!!changer les bornes velocityControl(stockage_D,0,GLED_ON,3); wait_ms(500); - pos = getPos(stockage_D,3) ; + consigne_pos = getPos(stockage_D,3) - 180 ; wait_us(500); - positionControl(stockage_D,pos-180,1,BLED_ON,3); + positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); - previous_pos = pos; etat = tmp_pret; // pc.printf("TMP_PRET\n"); } @@ -338,7 +341,7 @@ case tmp_pret : pos = getPos(stockage_D,3) ; wait_us(500); - if(pos< (previous_pos+5)){ + if(pos< (consigne_pos+5)){ etat = pret; //pc.printf("PRET\n"); } @@ -346,13 +349,12 @@ case pret : if((status_contact & MASK_CT_DROIT)){ - pos = getPos(stockage_D,3)- 550; + consigne_pos = getPos(stockage_D,3) - 550; wait_us(500); deverouillage_torque_convoyeurs_droit(); wait_us(500); - positionControl(stockage_D,pos,1,BLED_ON,3); + positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); - previous_pos = pos; etat = pousse; //pc.printf("POUSSE\n"); } @@ -361,13 +363,12 @@ case pousse : pos = getPos(stockage_D,3); wait_us(500); - if(pos<(previous_pos+5) and (status_contact & MASK_CT_DROIT)==0){ - pos = pos + 550; + if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0){ + consigne_pos = pos + 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); - positionControl(stockage_D,pos,1,BLED_ON,3); + positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); - previous_pos = pos; etat = retour; //pc.printf("RETOUR\n"); } @@ -376,7 +377,7 @@ case retour : pos = getPos(stockage_D,3) ; wait_us(500); - if(pos> (previous_pos-5)){ + if(pos> (consigne_pos-5)){ etat = pret; //pc.printf("PRET\n"); }