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:
- 14:bf2810b95f25
- Parent:
- 13:0b5310c6dc44
- Child:
- 15:1fefa6b1569b
--- a/actions_Gr.cpp Sun May 26 11:26:06 2019 +0000
+++ b/actions_Gr.cpp Sun May 26 12:46:41 2019 +0000
@@ -37,6 +37,7 @@
char favant_relache=0,farriere_relache=0;
char faccelerateur_avant=0,faccelerateur_arriere=0;
+int flag_vide_vert_rouge=0,flag_vide_bleu=0;
DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
@@ -47,116 +48,143 @@
char status_contact=0;
-void convoyeur_gauche_jaune(void){
-
- typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
+void convoyeur_gauche_jaune(void)
+{
+
+ typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ;
static type_etat etat = init;
static int16_t pos = 0, consigne_pos;
static int16_t previous_pos = getPos(stockage_G,1);
-
-
- switch(etat){
-
- case init :
+ /*if(flag_vide_vert_rouge)
+ {
+ etat = init;
+ flag_vide_vert_rouge = 0;
+ }*/
+
+ switch(etat) {
+
+ case init :
velocityControl(stockage_G,-512,BLED_ON,1);
pos = getPos(stockage_G,1);
- if(previous_pos != pos){
+ if(previous_pos != pos) {
etat = etalonnage;
- //pc.printf("ETALONNAGE\n");
-
+ //pc.printf("ETALONNAGE\n");
+
}
break;
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) ;
consigne_pos = pos + 180;
positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
etat = tmp_pret;
- //pc.printf("TMP_PRET\n");
+ //pc.printf("TMP_PRET\n");
}
break;
-
+
case tmp_pret :
pos = getPos(stockage_G,1);
wait_us(500);
- if(pos> (consigne_pos-5)){
+ if(pos> (consigne_pos-5)) {
//pc.printf("pos pret %d\n", pos);
- etat = pret;
- //pc.printf("PRET\n");
-
- }
+ etat = pret;
+ //pc.printf("PRET\n");
+
+ }
break;
-
- case pret :
- if((status_contact & MASK_CT_GAUCHE)){
+
+ case pret :
+ if(flag_vide_vert_rouge) {
+ flag_vide_vert_rouge =0;
+ etat = vide;
+ } else if((status_contact & MASK_CT_GAUCHE)) {
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");
+ //pc.printf("POUSSE\n");
}
break;
-
+
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;
+ if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0) {
+ consigne_pos = pos - 550;
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
etat = retour;
- //pc.printf("RETOUR\n");
+ //pc.printf("RETOUR\n");
}
-
+
break;
case retour :
pos = getPos(stockage_G,1) ;
wait_us(500);
- if(pos< (consigne_pos+5)){
- etat = pret;
- //pc.printf("PRET\n");
- }
+ 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;
break;
- }
-}
+ case fin_vide:
+ pos = getPos(stockage_G,1) ;
+ wait_us(500);
+ if(pos> (consigne_pos-5)) {
+ etat = init;
+ //pc.printf("PRET\n");
+ }
-void convoyeur_droit_jaune(void){
-
- typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
+ break;
+ }
+}
+
+void convoyeur_droit_jaune(void)
+{
+
+ 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 previous_pos = getPos(stockage_D,3);
-
-
- switch(etat){
-
- case init :
+ /*
+ if(flag_vide_bleu){
+ etat = init;
+ flag_vide_bleu = 0;
+ }*/
+
+ switch(etat) {
+
+ case init :
velocityControl(stockage_D,512,BLED_ON,3);
wait_us(500);
pos = getPos(stockage_D,3);
- if(previous_pos != pos){
+ 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<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) ;
@@ -172,20 +200,24 @@
//pc.printf("TMP_PRET\n");
}
break;
-
+
case tmp_pret :
pos = getPos(stockage_D,3) ;
- wait_us(500);
- if(pos < (consigne_pos+5)){
-
+ wait_us(500);
+ if(pos < (consigne_pos+5)) {
+
//pc.printf("pos fin %d\n", pos);
- etat = pret;
- //pc.printf("PRET\n");
- }
+ etat = pret;
+ //pc.printf("PRET\n");
+ }
break;
-
- case pret :
- if((status_contact & MASK_CT_DROIT)){
+
+ case pret :
+
+ if(flag_vide_bleu) {
+ flag_vide_bleu =0;
+ etat = vide;
+ } else if((status_contact & MASK_CT_DROIT)) {
//pc.printf("%d\n",pos);
consigne_pos = getPos(stockage_D,3)+ 550;
deverouillage_torque_convoyeurs_droit();
@@ -196,13 +228,13 @@
//pc.printf("POUSSE\n");
}
break;
-
+
case pousse :
pos = getPos(stockage_D,3);
- wait_us(500);
- if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0){
+ wait_us(500);
+ if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0) {
//pc.printf("%d\n",pos);
- consigne_pos = pos - 550;
+ consigne_pos = pos - 550;
deverouillage_torque_convoyeurs_droit();
wait_us(500);
positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
@@ -210,51 +242,70 @@
etat = retour;
//pc.printf("RETOUR\n");
}
-
+
break;
-
+
case retour :
pos = getPos(stockage_D,3) ;
wait_us(500);
- if(pos< (consigne_pos+5)){
- etat = pret;
- //pc.printf("PRET\n");
- }
+ if(pos< (consigne_pos+5)) {
+ etat = pret;
+ //pc.printf("PRET\n");
+ }
+ break;
+
+ case vide:
+ consigne_pos = getPos(stockage_D,3);
+ compteTour(stockage_D,+1023,2,consigne_pos,BLED_ON,3);
+ etat = fin_vide;
break;
- }
+ case fin_vide:
+ pos = getPos(stockage_D,3) ;
+ wait_us(500);
+ if(pos> (consigne_pos-5)) {
+ etat = init;
+ //pc.printf("PRET\n");
+ }
+
+ break;
+ }
}
-void convoyeur_gauche_violet(void){
-
- typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
+void convoyeur_gauche_violet(void)
+{
+
+ 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 previous_pos = getPos(stockage_G,1);
-
-
- switch(etat){
-
- case init :
+ /*
+ if(flag_vide_bleu){
+ etat = init;
+ flag_vide_bleu = 0;
+ }*/
+ switch(etat) {
+
+ case init :
velocityControl(stockage_G,-512,BLED_ON,1);
- wait_us(500);
+ wait_us(500);
pos = getPos(stockage_G,1);
- wait_us(500);
- if(previous_pos != pos){
+ wait_us(500);
+ if(previous_pos != pos) {
etat = etalonnage;
- // pc.printf("ETALONNAGE\n");
+ // pc.printf("ETALONNAGE\n");
}
break;
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);
@@ -263,18 +314,21 @@
////pc.printf("TMP_PRET\n");
}
break;
-
+
case tmp_pret :
pos = getPos(stockage_G,1) ;
- wait_us(500);
- if(pos> (consigne_pos-5)){
- etat = pret;
- //pc.printf("PRET\n");
- }
+ wait_us(500);
+ if(pos> (consigne_pos-5)) {
+ etat = pret;
+ //pc.printf("PRET\n");
+ }
break;
-
- case pret :
- if((status_contact & MASK_CT_GAUCHE)){
+
+ case pret :
+ if(flag_vide_bleu) {
+ flag_vide_bleu =0;
+ etat = vide;
+ } else if((status_contact & MASK_CT_GAUCHE)) {
//pc.printf("%d\n",pos);
consigne_pos = getPos(stockage_G,1)- 550;
wait_us(500);
@@ -286,13 +340,13 @@
//pc.printf("POUSSE\n");
}
break;
-
+
case pousse :
pos = getPos(stockage_G,1);
- wait_us(500);
- if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){
+ wait_us(500);
+ if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) {
//pc.printf("%d\n",pos);
- consigne_pos = pos + 550;
+ consigne_pos = pos + 550;
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
@@ -300,36 +354,56 @@
etat = retour;
//pc.printf("RETOUR\n");
}
-
+
break;
-
+
case retour :
pos = getPos(stockage_G,1) ;
- wait_us(500);
- if(pos> (consigne_pos-5)){
- etat = pret;
- //pc.printf("PRET\n");
- }
+ wait_us(500);
+ if(pos> (consigne_pos-5)) {
+ etat = pret;
+ //pc.printf("PRET\n");
+ }
break;
- }
+
+ case vide:
+ consigne_pos = getPos(stockage_G,1);
+ compteTour(stockage_G,-1023,2,consigne_pos,BLED_ON,1);
+ etat = fin_vide;
+ break;
+ case fin_vide:
+ pos = getPos(stockage_G,1) ;
+ wait_us(500);
+ if(pos< (consigne_pos+5)) {
+ etat = init;
+ //pc.printf("PRET\n");
+ }
+
+ break;
+ }
}
-void convoyeur_droit_violet(void){
-
- typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
+void convoyeur_droit_violet(void)
+{
+
+ typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ;
static type_etat etat = init;
static int16_t pos = 0, consigne_pos;
static int16_t previous_pos = getPos(stockage_D,3);
-
-
- switch(etat){
-
- case init :
+ /*
+ if(flag_vide_vert_rouge){
+ etat = init;
+ flag_vide_vert_rouge = 0;
+ } */
+
+ switch(etat) {
+
+ case init :
velocityControl(stockage_D,+512,BLED_ON,3);
- wait_us(500);
+ wait_us(500);
pos = getPos(stockage_D,3);
- wait_us(500);
- if(previous_pos != pos){
+ wait_us(500);
+ if(previous_pos != pos) {
//pc.printf("ETALONNAGE\n");
etat = etalonnage;
}
@@ -337,9 +411,9 @@
case etalonnage :
pos = getPos(stockage_D,3);
- wait_us(500);
+ wait_us(500);
//pc.printf("%d\n", pos);
- if( /*(21<pos && pos<260) ||*/ (754<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 ;
@@ -347,58 +421,79 @@
positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
wait_us(500);
etat = tmp_pret;
- // pc.printf("TMP_PRET\n");
+ // pc.printf("TMP_PRET\n");
}
break;
-
- case tmp_pret :
+
+ case tmp_pret :
pos = getPos(stockage_D,3) ;
- wait_us(500);
- if(pos< (consigne_pos+5)){
- etat = pret;
- pc.printf("PRET\n");
- }
+ wait_us(500);
+ if(pos< (consigne_pos+5)) {
+ etat = pret;
+ //pc.printf("PRET\n");
+ }
break;
-
- case pret :
- if((status_contact & MASK_CT_DROIT)){
+
+ case pret :
+ if(flag_vide_vert_rouge) {
+ flag_vide_vert_rouge =0;
+ etat = vide;
+ } else if((status_contact & MASK_CT_DROIT)) {
consigne_pos = getPos(stockage_D,3) - 550;
wait_us(500);
deverouillage_torque_convoyeurs_droit();
wait_us(500);
- pc.printf("consigne %d", consigne_pos);
+ //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;
-
+
case pousse :
pos = getPos(stockage_D,3);
- 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);
+ 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;
case retour :
pos = getPos(stockage_D,3) ;
- wait_us(500);
- if(pos> (consigne_pos-5)){
- etat = pret;
- pc.printf("PRET\n");
- }
+ wait_us(500);
+ if(pos> (consigne_pos-5)) {
+ etat = pret;
+ // pc.printf("PRET\n");
+ }
+ break;
+
+ case vide:
+ consigne_pos = getPos(stockage_D,3);
+ //pc.printf("consigne %d",consigne_pos);
+ compteTour(stockage_D,-1023,3,consigne_pos,BLED_ON,3);
+ etat = fin_vide;
break;
- }
-}
+
+ case fin_vide:
+ pos = getPos(stockage_D,3) ;
+ //pc.printf("pos %d", pos);
+ wait_us(500);
+ if(pos< (consigne_pos+5)) {
+ etat = init;
+ //pc.printf("PRET\n");
+ }
+
+ break;
+ }
+}
void gabarit_robot(void)
{
uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
@@ -411,7 +506,7 @@
deverouillage_torque();
positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
wait(0.5);
-
+
}
@@ -431,7 +526,7 @@
if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
}
-
+
FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
etat = tmp;
}
@@ -455,7 +550,7 @@
int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
static int flag_sortie = 0;
-
+
switch(etat) {
case init :
//on attend le premier atome et place le herkulex en fonction
@@ -467,8 +562,7 @@
case atome :
//on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
- if(etat_cap != 0)
- {
+ if(etat_cap != 0) {
oriente_doigt(etat_cap);
etat = tmp;
}
@@ -483,9 +577,8 @@
if(etat_cap == 0) {
char 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");
+ if(FIFO_couleur_lecture == FIFO_couleur_ecriture) {
+ //pc.printf("Fifo vide\n");
if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
oriente_doigt(2);
else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
@@ -506,7 +599,7 @@
} else if (palet == 2) {//position herkulex stockage rouge/vert
positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
SendCharCan(HACHEUR_ID_COUROIES,1);
-
+
}
} else {
if(palet== 1) {//position herkulex stockage bleu