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:
- 18:2082875295df
- Parent:
- 17:5973f13f44e2
- Child:
- 19:2281a0ef54e4
diff -r 5973f13f44e2 -r 2082875295df actions_Gr.cpp
--- a/actions_Gr.cpp Wed May 29 17:18:16 2019 +0000
+++ b/actions_Gr.cpp Fri May 31 05:39:55 2019 +0000
@@ -53,7 +53,9 @@
char status_contact=0;
int nbPaletsVerts = 0, nbPaletsBleus = 0;
+Timer timer_bloquer;
+Timer timeout;
void roue(int allume)
{
@@ -78,7 +80,8 @@
static int16_t previous_pos = getPos(stockage_G,1);
static int cpt_vider = 0;
int flag_debut = 0;
- static int pos_vide[5] = {800, 200, 600, 1000, 400};
+ //static int pos_vide[8] = {800, 100, 500, 900, 200, 600, 1000, 400};
+ static int pos_vide[11] = {800, 100, 500, 900, 200, 600, 1000, 300, 700, 24, 400};
/*if(flag_vide_vert_rouge)
{
etat = init;
@@ -137,7 +140,6 @@
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");
}
@@ -248,13 +250,15 @@
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])
+ if(pos >= pos_vide[cpt_vider%11] && pos <= (pos_vide[cpt_vider%11]+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;
+ timer_bloquer.start();
+ timer_bloquer.reset();
}
break;
@@ -262,7 +266,7 @@
case fin_vide:
pos = getPos(roue_G,1) ;
wait_us(500);
- if(pos> (consigne_roue-5)) {
+ if(pos> (consigne_roue-5) || timer_bloquer>2) {
positionControl(roue_G,750,50,BLED_ON,1);
verification();
cpt_vider ++;
@@ -536,7 +540,8 @@
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};
+ //static int pos_vide[5] = {400, 1000, 600, 200, 800};
+ static int pos_vide[11] = {400, 24, 700, 300, 1000, 600, 200, 900, 500, 100, 800};
/*
if(flag_vide_vert_rouge){
etat = init;
@@ -665,13 +670,15 @@
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])
+ if(pos <= pos_vide[cpt_vider%11] && 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;
+ timer_bloquer.start();
+ timer_bloquer.reset();
}
break;
@@ -679,7 +686,7 @@
case fin_vide:
pos = getPos(roue_D,3) ;
wait_us(500);
- if(pos> (consigne_roue+5)) {
+ if(pos> (consigne_roue+5) || timer_bloquer>2) {
positionControl(roue_D,374,50,BLED_ON,3);
verification();
cpt_vider ++;
@@ -755,7 +762,7 @@
int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
static int flag_dernier = 0, flag_suivant = 0;
- static long cpt;
+ static long cpt, cpt_bloquer = 0;
static char memo_FIFO_couleur_lecture;
if(flag_ascenseur_force_off)etat = init;
@@ -765,6 +772,8 @@
//on attend le premier atome et place le herkulex en fonction
if( (flag_ascenseur || flag_ascenseur_force_on) && !flag_ascenseur_force_off ) {
SendCharCan(HACHEUR_ID_COUROIES,1);
+ /*timer_bloquer.start();
+ timer_bloquer.reset();*/
etat = atome;
}
break;
@@ -772,7 +781,12 @@
case atome :
//on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
cpt++;
- if( ((cpt > 30000)&& flag_dernier) )
+ /*if( (timer_bloquer.read() > 3.0) && (FIFO_couleur_lecture != FIFO_couleur_ecriture)){
+ flag_ascenseur_force_off = 1;
+ SendCharCan(HACHEUR_ID_COUROIES,0);
+ etat = init;
+ }
+ else*/ if( ((cpt > 30000)&& flag_dernier) )
{
flag_dernier = 0;
oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
@@ -788,6 +802,8 @@
}else if((cpt > 5000)&& flag_suivant){
flag_suivant =0;
oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
+
+
}else if( (cpt>60000) && !flag_ascenseur && !flag_ascenseur_force_on){
SendCharCan(HACHEUR_ID_COUROIES,0);
etat = init;
@@ -798,8 +814,10 @@
if(etat_cap == 1)nbPaletsBleus ++;
else if(etat_cap == 2) nbPaletsVerts ++;
oriente_doigt(etat_cap);
+ //timer_bloquer.reset();
etat = tmp;
}
+
/*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
{
oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO);
@@ -808,7 +826,13 @@
case tmp :
//on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
+ /*if( (timer_bloquer.read() > 5.0) && (FIFO_couleur_lecture != FIFO_couleur_ecriture)){
+ flag_ascenseur_force_off = 1;
+ SendCharCan(HACHEUR_ID_COUROIES,0);
+ etat = init;
+ }*/
if(etat_cap == 0) {
+ //timer_bloquer.reset();
memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
@@ -963,42 +987,86 @@
verification();
}
+
void automate_ventouse_goldenium_avant (void)
{
- typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+
+ typedef enum {init,envoi_instruction,attente_ack_ventouse,reculer,xyt, fin} type_etat;
static type_etat etat = init;
switch(etat) {
case init: //attente d'initialisation
if(fgoldenium_avant)
- etat=envoi_instruction;
+ etat = envoi_instruction;
break;
case envoi_instruction://envoi instruction
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+ wait_us(50);
+ SendRawId(HACHEUR_STATUT_VENTOUSES);
+ wait_us(50);
goldenium_avant();
- char pompe=AV_CENTRE;
+ char pompe = AV_CENTRE;
+ SendSpeed(VITESSE_RALENTI,ACCELERATION_RALENTI,DECELERATION_RALENTI);
+ wait_us(50);
+ GoStraight(200,0,0,0);//on avance vers le goldenium
+ wait_us(50);
can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
- SendRawId(HACHEUR_STATUT_VENTOUSES);
- GoStraight(distance_goldenium,0,0,0);
+ wait_us(50);
+ timeout.start();
+ timeout.reset();
etat = attente_ack_ventouse;
break;
- case attente_ack_ventouse:
- if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
- wait(1);
- GoStraight(-distance_goldenium,0,0,0);
- positionControl(AV_poigne_C,470,100,BLED_ON,2);//forklift
- verification();
+ case attente_ack_ventouse://auotmate ack carte pompe merde, renvoi ack goldenium saisi de temps en temps
+ if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {//dès qu'il est saisi on attend pour etre sur
+ wait(0.1);
+
+ ackFinAction = 0;
+ SendRawId(ASSERVISSEMENT_STOP);//on s'arrete
+ timeout.reset();
+ etat = reculer;
+ } else if(timeout.read_ms()>5000) {
+ ackFinAction = 0;
+ SendRawId(ASSERVISSEMENT_STOP);
+ timeout.reset();
+ etat = reculer;
+ }
+ break;
+
+ case reculer:
+ if(ackFinAction == ASSERVISSEMENT_STOP)
+ {
+ ackFinAction = 0;
+ GoStraight(-40,0,0,0);//on recule en x donné par la strat
+ wait(0.1);
+ positionControl(AV_poigne_C,450,150,BLED_ON,2);//forklift
fgoldenium_avant=0;
+ etat = xyt;
+ }
+ break;
+
+ case xyt:
+ if(ackFinAction == ASSERVISSEMENT_RECALAGE)
+ {
+ ackFinAction = 0;
+ SendSpeed(VITESSE_NORMAL,ACCELERATION,DECELERATION);
+ GoToPosition( x_goldenium,y_goldenium,1800,-1);//x et y donné par la strat ,theta constant,-1 marche arriere
+ etat = fin;
+ }
+ break;
+ case fin :
+ if(ackFinAction == ASSERVISSEMENT_XYT)
+ {
+ ackFinAction = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
}
break;
-
}
}
+
void accelerateur_avant(void)
{
}