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:
- 16:06463f24811d
- Parent:
- 15:1fefa6b1569b
- Child:
- 17:5973f13f44e2
--- a/actions_Gr.cpp Mon May 27 22:59:28 2019 +0000
+++ b/actions_Gr.cpp Tue May 28 18:02:59 2019 +0000
@@ -51,6 +51,8 @@
unsigned char FIFO_couleur_ecriture=0;
signed char FIFO_couleur_lecture=0;
char status_contact=0;
+int nbPaletsVerts = 0, nbPaletsBleus = 0;
+
void roue(int allume)
@@ -73,7 +75,8 @@
static int16_t pos = 0, consigne_pos, consigne_roue;
static int16_t previous_pos = getPos(stockage_G,1);
static int cpt_vider = 0;
- int flag_debut;
+ int flag_debut = 0;
+ static int pos_vide[5] = {800, 200, 600, 1000, 400};
/*if(flag_vide_vert_rouge)
{
etat = init;
@@ -120,7 +123,7 @@
case pret :
if(flag_vide_vert_rouge) {
flag_vide_vert_rouge =0;
- flag_debut = 1;
+ flag_debut = 0;
etat = vide;
} else if((status_contact & MASK_CT_GAUCHE)) {
consigne_pos = (getPos(stockage_G,1)+550);
@@ -153,27 +156,31 @@
break;
case retour :
pos = getPos(stockage_G,1) ;
- if(flag_debut){
- pos += 255;
- flag_debut = 0;
- }
wait_us(500);
if(pos< (consigne_pos+5)) {
etat = pret;
//pc.printf("PRET\n");
}
break;
-
+ /*
case vide:
pos = getPos(stockage_G,1);
- consigne_pos = (pos+605)%1105;
+ if(!flag_debut){
+ flag_debut =1;
+ consigne_pos = (pos+420)%1105;
+ }
+ else{
+
+ consigne_pos = (pos+400)%1105;
+ }
+
pc.printf("\n1 pos : %d consigne %d\n",pos, consigne_pos);
//ACTIVERMOTEUR
roue(1);
//compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1);
- if(cpt_vider<6){
- compteTour(stockage_G,+1023,0,consigne_pos,BLED_ON,1);
+ if(cpt_vider<7){
+ compteTour(stockage_G,+200,0,consigne_pos,BLED_ON,1);
//positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
pc.printf("TMP_VIDE\n");
etat = tmp_vide;
@@ -191,10 +198,9 @@
pos = getPos(stockage_G,1) ;
wait_us(500);
- if(pos> (consigne_pos-5)) {
+ if(pos< (consigne_pos+5)) {
cpt_vider +=1;
- consigne_roue = 850-55;
- positionControl(stockage_G,(pos),40,BLED_ON,1);
+ consigne_roue = 815;
positionControl(roue_G,consigne_roue,40,BLED_ON,1);
etat = fin_vide;
pc.printf("FIN_VIDE\n");
@@ -214,6 +220,50 @@
}
break;
+ }*/
+ case vide:
+
+ velocityControl(stockage_G, 400, BLED_ON,1);
+ wait_ms(200);
+ roue(1);
+
+ if(cpt_vider<9){
+ etat = tmp_vide;
+ //pc.printf("TMP_VIDE\n");
+
+ }else{
+ etat = init;
+ cpt_vider = 0;
+ roue(0);
+ //pc.printf("INIT\n");
+ }
+ break;
+
+ case tmp_vide:
+ pos = getPos(stockage_G,1);
+ wait_us(100);
+ if(pos >= pos_vide[cpt_vider%5] && pos <= (pos_vide[cpt_vider%5]+20)){ //if(pos == pos_vide[cpt_vider%5])
+ velocityControl(stockage_G, 0, BLED_ON,1);
+ positionControl(stockage_G,pos-30,1,BLED_ON,1);
+ //pc.printf("pos :%d\n", pos);
+ consigne_roue = 840;
+ positionControl(roue_G,consigne_roue,1,BLED_ON,1);
+ etat = fin_vide;
+ }
+
+ break;
+
+ case fin_vide:
+ pos = getPos(roue_G,1) ;
+ wait_us(500);
+ if(pos> (consigne_roue-5)) {
+ positionControl(roue_G,750,50,BLED_ON,1);
+ verification();
+ cpt_vider ++;
+ etat = vide;
+ }
+
+ break;
}
}
@@ -224,6 +274,7 @@
static type_etat etat = init;
static int16_t pos = 0, consigne_pos;
static int16_t previous_pos = getPos(stockage_D,3);
+ int vInit = 512;
/*
if(flag_vide_bleu){
etat = init;
@@ -233,7 +284,7 @@
switch(etat) {
case init :
- velocityControl(stockage_D,512,BLED_ON,3);
+ velocityControl(stockage_D,vInit,BLED_ON,3);
wait_us(500);
pos = getPos(stockage_D,3);
if(previous_pos != pos) {
@@ -245,15 +296,15 @@
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) ;
- pc.printf("pos %d\n", pos);
+ //pc.printf("pos %d\n", pos);
//positionControl(stockage_D,pos-2000,1,BLED_ON,3);
consigne_pos =pos-550;
if(consigne_pos<0) consigne_pos += 1105;
- pc.printf("consigne : %d \n",consigne_pos);
+ //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);
@@ -277,9 +328,11 @@
if(flag_vide_bleu) {
flag_vide_bleu =0;
- etat = vide;
+ vInit = 1023;
+ etat = init;
+ //pc.printf("vide\n");
} else if((status_contact & MASK_CT_DROIT)) {
- //pc.printf("%d\n",pos);
+ ///pc.printf("%d\n",pos);
consigne_pos = getPos(stockage_D,3)+ 550;
deverouillage_torque_convoyeurs_droit();
wait_us(500);
@@ -325,7 +378,7 @@
wait_us(500);
if(pos> (consigne_pos-5)) {
etat = init;
- //pc.printf("PRET\n");
+ //pc.printf("INIT\n");
}
break;
@@ -337,8 +390,9 @@
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 pos = 0, consigne_pos, pos_pret;
static int16_t previous_pos = getPos(stockage_G,1);
+ static int vInit = -512;
/*
if(flag_vide_bleu){
etat = init;
@@ -347,7 +401,7 @@
switch(etat) {
case init :
- velocityControl(stockage_G,-512,BLED_ON,1);
+ velocityControl(stockage_G,vInit,BLED_ON,1);
wait_us(500);
pos = getPos(stockage_G,1);
wait_us(500);
@@ -359,17 +413,17 @@
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);
+ //pc.printf("pos depart: %d\n",pos);
wait_us(500);
- consigne_pos = pos + 500;
- if(consigne_pos >1105) consigne_pos -= 1105;
+ pos_pret = pos + 500;
+ if(pos_pret >1105) pos_pret -= 1105;
- pc.printf("pos : %d consigne : %d\n",pos,consigne_pos);
- compteTour(stockage_G,1023,1,consigne_pos,BLED_ON,1);
+ //pc.printf("pos : %d consigne : %d\n",pos,pos_pret);
+ compteTour(stockage_G,1023,1,pos_pret,BLED_ON,1);
wait_us(500);
etat = tmp_pret;
////pc.printf("TMP_PRET\n");
@@ -379,7 +433,7 @@
case tmp_pret :
pos = getPos(stockage_G,1) ;
wait_us(500);
- if(pos> (consigne_pos-5)) {
+ if(pos> (pos_pret-5)) {
etat = pret;
//pc.printf("PRET\n");
}
@@ -388,13 +442,17 @@
case pret :
if(flag_vide_bleu) {
flag_vide_bleu =0;
- etat = vide;
+ vInit = -1023;
+ etat = init;
} else if((status_contact & MASK_CT_GAUCHE)) {
- //pc.printf("%d\n",pos);
- consigne_pos = getPos(stockage_G,1)- 550;
+ consigne_pos = (pos_pret- 550);
+ if(consigne_pos<0)consigne_pos+=1105;
wait_us(500);
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
+ //pc.printf("pos:%d consigne:%d\n",pos, consigne_pos);
+
+ //compteTour(stockage_G,-1023,0,consigne_pos,BLED_ON,1);
positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
wait_us(500);
etat = pousse;
@@ -407,10 +465,11 @@
wait_us(500);
if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) {
//pc.printf("%d\n",pos);
- consigne_pos = pos + 550;
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
- positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
+
+ //compteTour(stockage_G,1023,0,pos_pret,BLED_ON,1);
+ positionControl(stockage_G,pos_pret,1,BLED_ON,1);
wait_us(500);
etat = retour;
//pc.printf("RETOUR\n");
@@ -421,7 +480,7 @@
case retour :
pos = getPos(stockage_G,1) ;
wait_us(500);
- if(pos> (consigne_pos-5)) {
+ if(pos> (pos_pret-5)) {
etat = pret;
//pc.printf("PRET\n");
}
@@ -447,10 +506,12 @@
void convoyeur_droit_violet(void)
{
- typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ;
+ typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide,tmp_vide} type_etat ;
static type_etat etat = init;
- static int16_t pos = 0, consigne_pos;
+ static int16_t pos = 0, consigne_pos,consigne_roue;
static int16_t previous_pos = getPos(stockage_D,3);
+ static int cpt_vider=0;
+ static int pos_vide[5] = {400, 1000, 600, 200, 800};
/*
if(flag_vide_vert_rouge){
etat = init;
@@ -535,7 +596,7 @@
// pc.printf("PRET\n");
}
break;
-
+/*
case vide:
consigne_pos = getPos(stockage_D,3);
//pc.printf("consigne %d",consigne_pos);
@@ -553,6 +614,50 @@
}
break;
+ */
+ case vide:
+
+ velocityControl(stockage_D, -400, BLED_ON,3);
+ wait_ms(200);
+ roue(1);
+
+ if(cpt_vider<9){
+ etat = tmp_vide;
+ //pc.printf("TMP_VIDE\n");
+
+ }else{
+ etat = init;
+ cpt_vider = 0;
+ roue(0);
+ //pc.printf("INIT\n");
+ }
+ break;
+
+ case tmp_vide:
+ pos = getPos(stockage_D,3);
+ wait_us(100);
+ if(pos <= pos_vide[cpt_vider%5] && pos >= (pos_vide[cpt_vider%5]-20)){ //if(pos == pos_vide[cpt_vider%5])
+ velocityControl(stockage_D, 0, BLED_ON,3);
+ positionControl(stockage_D,pos+30,1,BLED_ON,3);
+ //pc.printf("pos :%d\n", pos);
+ consigne_roue = 225;
+ positionControl(roue_D,consigne_roue,1,BLED_ON,3);
+ etat = fin_vide;
+ }
+
+ break;
+
+ case fin_vide:
+ pos = getPos(roue_D,3) ;
+ wait_us(500);
+ if(pos> (consigne_roue+5)) {
+ positionControl(roue_D,374,50,BLED_ON,3);
+ verification();
+ cpt_vider ++;
+ etat = vide;
+ }
+
+ break;
}
}
void gabarit_robot(void)
@@ -657,8 +762,11 @@
SendCharCan(HACHEUR_ID_COUROIES,0);
etat = init;
}
+
if(etat_cap != 0) {
cpt = 0;
+ if(etat_cap == 1)nbPaletsBleus ++;
+ else if(etat_cap == 2) nbPaletsVerts ++;
oriente_doigt(etat_cap);
etat = tmp;
}
@@ -672,6 +780,7 @@
//on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
if(etat_cap == 0) {
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");