librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 13:0b5310c6dc44
- Parent:
- 11:dfff2e200d0c
- Child:
- 14:bf2810b95f25
diff -r dfff2e200d0c -r 0b5310c6dc44 actions_Gr.cpp --- a/actions_Gr.cpp Sun May 26 09:58:39 2019 +0000 +++ b/actions_Gr.cpp Sun May 26 11:26:06 2019 +0000 @@ -70,7 +70,7 @@ 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) ; @@ -85,6 +85,7 @@ pos = getPos(stockage_G,1); wait_us(500); if(pos> (consigne_pos-5)){ + //pc.printf("pos pret %d\n", pos); etat = pret; //pc.printf("PRET\n"); @@ -93,10 +94,15 @@ case pret : if((status_contact & MASK_CT_GAUCHE)){ - consigne_pos = getPos(stockage_G,1)+550; + 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"); } @@ -104,6 +110,7 @@ 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; @@ -142,25 +149,27 @@ 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); wait_us(500); - if(( (21<pos && pos<428) || (572<pos && pos<980) ) 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); //positionControl(stockage_D,pos-2000,1,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); + consigne_pos =pos-550; + //if(consigne_pos<0) consigne_pos += 1105; + //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); etat = tmp_pret; - pc.printf("TMP_PRET\n"); + //pc.printf("TMP_PRET\n"); } break; @@ -168,8 +177,10 @@ pos = getPos(stockage_D,3) ; wait_us(500); if(pos < (consigne_pos+5)){ + + //pc.printf("pos fin %d\n", pos); etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; @@ -182,7 +193,7 @@ positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = pousse; - pc.printf("POUSSE\n"); + //pc.printf("POUSSE\n"); } break; @@ -197,7 +208,7 @@ positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = retour; - pc.printf("RETOUR\n"); + //pc.printf("RETOUR\n"); } break; @@ -207,7 +218,7 @@ wait_us(500); if(pos< (consigne_pos+5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; } @@ -236,13 +247,15 @@ case etalonnage : pos = getPos(stockage_G,1); - pc.printf("%d\n",pos); - if((21<pos && pos<428) || (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); wait_us(500); @@ -256,7 +269,7 @@ wait_us(500); if(pos> (consigne_pos-5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; @@ -278,7 +291,7 @@ pos = getPos(stockage_G,1); wait_us(500); if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){ - pc.printf("%d\n",pos); + //pc.printf("%d\n",pos); consigne_pos = pos + 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); @@ -295,7 +308,7 @@ wait_us(500); if(pos> (consigne_pos-5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; } @@ -326,7 +339,7 @@ pos = getPos(stockage_D,3); wait_us(500); //pc.printf("%d\n", pos); - if( (21<pos && pos<260) || (404<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 ; @@ -343,7 +356,7 @@ wait_us(500); if(pos< (consigne_pos+5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; @@ -353,10 +366,11 @@ wait_us(500); deverouillage_torque_convoyeurs_droit(); wait_us(500); + 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; @@ -365,12 +379,13 @@ 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; @@ -379,7 +394,7 @@ wait_us(500); if(pos> (consigne_pos-5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; }