CRAC Team
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
--- 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;
}