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:
- 15:1fefa6b1569b
- Parent:
- 14:bf2810b95f25
- Child:
- 16:06463f24811d
--- a/actions_Gr.cpp Sun May 26 12:46:41 2019 +0000
+++ b/actions_Gr.cpp Mon May 27 22:59:28 2019 +0000
@@ -23,11 +23,13 @@
#define MASK_FC_GAUCHE 0x04
#define MASK_CT_GAUCHE 0x08
-#define POS_DOIGT_GAUCHE 585
-#define POS_DOIGT_DROIT 389
+#define POS_DOIGT_GAUCHE 605
+#define POS_DOIGT_DROIT 369
+#define POS_DOIGT_GAUCHE_SUIV 535
+#define POS_DOIGT_DROIT_SUIV 439
char status_pompe=0;
-bool flag_ascenseur = 0;
+bool flag_ascenseur = 0, flag_ascenseur_commande = 0;
char fpresentoir_avant=0, fpresentoir_arriere=0;
char fgoldenium_avant=0, fgoldenium_arriere=0;
@@ -42,19 +44,36 @@
DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
+PwmOut PWM_roue_Gauche(PA_15);
+PwmOut PWM_roue_Droite(PC_9);
+
char buffer_couleur_bas[SIZE_FIFO];
unsigned char FIFO_couleur_ecriture=0;
signed char FIFO_couleur_lecture=0;
char status_contact=0;
+void roue(int allume)
+{
+ if(cote)
+ {
+ PWM_roue_Gauche.write(float(allume));// = 1.0;//VIT_ROUE*allume;
+ }
+ else
+ {
+ PWM_roue_Droite.write(float(allume));// = 1.0;//VIT_ROUE*allume;
+ }
+}
+
void convoyeur_gauche_jaune(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, tmp_vide, fin_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_G,1);
+ static int cpt_vider = 0;
+ int flag_debut;
/*if(flag_vide_vert_rouge)
{
etat = init;
@@ -101,6 +120,7 @@
case pret :
if(flag_vide_vert_rouge) {
flag_vide_vert_rouge =0;
+ flag_debut = 1;
etat = vide;
} else if((status_contact & MASK_CT_GAUCHE)) {
consigne_pos = (getPos(stockage_G,1)+550);
@@ -133,23 +153,64 @@
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:
- consigne_pos = getPos(stockage_G,1)+500;
- compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1);
- etat = fin_vide;
+ pos = getPos(stockage_G,1);
+ consigne_pos = (pos+605)%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);
+ //positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
+ pc.printf("TMP_VIDE\n");
+ etat = tmp_vide;
+
+ }else{
+ //arreter moteurs
+ etat = init;
+ cpt_vider = 0;
+ roue(0);
+ pc.printf("INIT\n");
+ }
break;
- case fin_vide:
+
+ case tmp_vide:
+
pos = getPos(stockage_G,1) ;
wait_us(500);
if(pos> (consigne_pos-5)) {
- etat = init;
- //pc.printf("PRET\n");
+ cpt_vider +=1;
+ consigne_roue = 850-55;
+ positionControl(stockage_G,(pos),40,BLED_ON,1);
+ positionControl(roue_G,consigne_roue,40,BLED_ON,1);
+ etat = fin_vide;
+ pc.printf("FIN_VIDE\n");
+ }
+
+ break;
+
+ case fin_vide:
+ pos = getPos(roue_G,1) ;
+ wait_us(500);
+ if(pos> (consigne_roue-5)) {
+ wait_ms(300.0);
+ positionControl(roue_G,780,1,BLED_ON,1);
+ pc.printf("VIDE\n");
+ wait_ms(300.0);
+ etat = vide;
}
break;
@@ -184,15 +245,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);
+ 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);
@@ -298,17 +359,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);
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);
+
+ pc.printf("pos : %d consigne : %d\n",pos,consigne_pos);
+ compteTour(stockage_G,1023,1,consigne_pos,BLED_ON,1);
wait_us(500);
etat = tmp_pret;
////pc.printf("TMP_PRET\n");
@@ -500,13 +561,23 @@
uint16_t pos_av_centre[2] = {550,550};//470,350
-
+ uint8_t servos_roulette_gauche[2] = {GLED_ON, roue_G};
+ uint16_t pos_roulette_gauche[1] = {512};
+
+ uint8_t servos_roulette_droit[2] = {GLED_ON, roue_D};
+ uint16_t pos_roulette_droit[1] = {512};
+
+ PWM_roue_Gauche.period(0.00005);
+ PWM_roue_Droite.period(0.00005);
+
int speed=100;
-
+ roue(0);
deverouillage_torque();
positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
- wait(0.5);
-
+ positionControl_Mul_ensemble_complex(1,speed,servos_roulette_droit,pos_roulette_droit,3);
+ positionControl_Mul_ensemble_complex(1,speed,servos_roulette_gauche,pos_roulette_gauche,1);
+
+ SendRawId(HACHEUR_ETAT_CONTACTS);
}
@@ -523,6 +594,7 @@
if(etat_cap) {
buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
flag_ascenseur = 1;
+ SendCharCan(HACHEUR_ID_COUROIES,1);
if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
}
@@ -549,12 +621,14 @@
static type_etat etat = init;
int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
- static int flag_sortie = 0;
-
+ static int flag_dernier = 0, flag_suivant = 0;
+ static long cpt;
+ static char memo_FIFO_couleur_lecture;
+
switch(etat) {
case init :
//on attend le premier atome et place le herkulex en fonction
- if(flag_ascenseur) {
+ if(flag_ascenseur || flag_ascenseur_commande) {
//SendCharCan(HACHEUR_ID_COUROIES,1);
etat = atome;
}
@@ -562,7 +636,29 @@
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) )
+ {
+ flag_dernier = 0;
+ oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
+
+ flag_ascenseur = 0;
+
+ /*
+ if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
+ oriente_doigt(2);
+ else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
+ oriente_doigt(1);*/
+
+ }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_commande){
+ SendCharCan(HACHEUR_ID_COUROIES,0);
+ etat = init;
+ }
if(etat_cap != 0) {
+ cpt = 0;
oriente_doigt(etat_cap);
etat = tmp;
}
@@ -575,14 +671,21 @@
case tmp :
//on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
if(etat_cap == 0) {
- char memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
+ 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");
+ flag_dernier = 1;/*
if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
oriente_doigt(2);
else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
- oriente_doigt(1);
+ oriente_doigt(1);*/
+ }else{
+ flag_suivant = 1;
+ /*if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
+ oriente_doigt(2);
+ else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
+ oriente_doigt(1);*/
}
etat = atome;
}
@@ -594,24 +697,49 @@
{
if(cote) {
if(palet == 1) {//position herkulex stockage bleu
- positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
SendCharCan(HACHEUR_ID_COUROIES,2);
+ positionControl(doigt,POS_DOIGT_GAUCHE,20,BLED_ON,2);
} else if (palet == 2) {//position herkulex stockage rouge/vert
- positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
SendCharCan(HACHEUR_ID_COUROIES,1);
+ positionControl(doigt,POS_DOIGT_DROIT,20,GLED_ON,2);
- }
+ }else
+ positionControl(doigt,512,20,GLED_ON,2);
} else {
if(palet== 1) {//position herkulex stockage bleu
+ SendCharCan(HACHEUR_ID_COUROIES,2);
positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
- SendCharCan(HACHEUR_ID_COUROIES,1);
} else if (palet== 2) {//position herkulex stockage rouge/vert
+ SendCharCan(HACHEUR_ID_COUROIES,1);
positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
- SendCharCan(HACHEUR_ID_COUROIES,2);
- }
+ }else
+ positionControl(doigt,512,20,GLED_ON,2);
}
}
+void oriente_doigt_suiv(int palet)
+{
+ if(cote) {
+ if(palet == 1) {//position herkulex stockage bleu
+ SendCharCan(HACHEUR_ID_COUROIES,2);
+ positionControl(doigt,POS_DOIGT_GAUCHE_SUIV,20,BLED_ON,2);
+ } else if (palet == 2) {//position herkulex stockage rouge/vert
+ SendCharCan(HACHEUR_ID_COUROIES,1);
+ positionControl(doigt,POS_DOIGT_DROIT_SUIV,20,GLED_ON,2);
+
+ }else
+ positionControl(doigt,512,20,GLED_ON,2);
+ } else {
+ if(palet== 1) {//position herkulex stockage bleu
+ SendCharCan(HACHEUR_ID_COUROIES,2);
+ positionControl(doigt,POS_DOIGT_DROIT_SUIV,10,BLED_ON,2);
+ } else if (palet== 2) {//position herkulex stockage rouge/vert
+ SendCharCan(HACHEUR_ID_COUROIES,1);
+ positionControl(doigt,POS_DOIGT_GAUCHE_SUIV,10,GLED_ON,2);
+ }else
+ positionControl(doigt,512,20,GLED_ON,2);
+ }
+}
void presentoir_avant(void)
{
uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
@@ -688,11 +816,48 @@
void goldenium_avant(void)
{
+ uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
+ uint16_t pos_av_centre[2] = {165,165};
+ int speed=25;
+ positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
+ verification();
}
void automate_ventouse_goldenium_avant (void)
{
+ typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+ static type_etat etat = init;
+
+ switch(etat) {
+ case init: //attente d'initialisation
+ if(fgoldenium_avant)
+ etat=envoi_instruction;
+ break;
+
+ case envoi_instruction://envoi instruction
+ SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+ goldenium_avant();
+ char pompe=AV_CENTRE;
+ can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
+ SendRawId(HACHEUR_STATUT_VENTOUSES);
+ GoStraight(distance_goldenium,0,0,0);
+ 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();
+ fgoldenium_avant=0;
+ SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+ etat = init;
+ }
+ break;
+
+ }
}
void accelerateur_avant(void)