librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
Diff: actions_Gr.cpp
- Revision:
- 10:fba0699b7a74
- Parent:
- 9:05da4300730a
- Child:
- 11:dfff2e200d0c
--- a/actions_Gr.cpp Sat May 25 17:47:38 2019 +0000 +++ b/actions_Gr.cpp Sat May 25 18:39:49 2019 +0000 @@ -62,7 +62,7 @@ pos = getPos(stockage_G,1); if(previous_pos != pos){ etat = etalonnage; - //pc.printf("ETALONNAGE\n"); + pc.printf("ETALONNAGE\n"); } break; @@ -74,7 +74,7 @@ pos = getPos(stockage_G,1) ; positionControl(stockage_G,pos+180,1,BLED_ON,1); etat = tmp_pret; - //pc.printf("TMP_PRET\n"); + pc.printf("TMP_PRET\n"); } break; @@ -83,7 +83,7 @@ pos = getPos(stockage_G,1) ; if(pos> (previous_pos-5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; @@ -95,7 +95,7 @@ positionControl(stockage_G,pos,1,BLED_ON,1); previous_pos = pos; etat = pousse; - //pc.printf("POUSSE\n"); + pc.printf("POUSSE\n"); } break; @@ -108,7 +108,7 @@ positionControl(stockage_G,pos,1,BLED_ON,1); previous_pos = pos; etat = retour; - //pc.printf("RETOUR\n"); + pc.printf("RETOUR\n"); } break; @@ -116,7 +116,7 @@ pos = getPos(stockage_G,1) ; if(pos> (previous_pos-5)){ etat = pret; - //pc.printf("PRET\n"); + pc.printf("PRET\n"); } break; } @@ -134,6 +134,7 @@ case init : velocityControl(stockage_D,512,BLED_ON,3); + wait_us(500); pos = getPos(stockage_D,3); if(previous_pos != pos){ etat = etalonnage; @@ -149,6 +150,7 @@ pos = getPos(stockage_D,3) ; //positionControl(stockage_D,pos-2000,1,BLED_ON,3); compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3); + wait_us(500); etat = tmp_pret; //pc.printf("TMP_PRET\n"); } @@ -157,6 +159,7 @@ case tmp_pret : previous_pos = pos; pos = getPos(stockage_D,3) ; + wait_us(500); if(pos> (previous_pos+5)){ etat = pret; //pc.printf("PRET\n"); @@ -170,6 +173,7 @@ deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,pos,1,BLED_ON,3); + wait_us(500); previous_pos = pos; etat = pousse; //pc.printf("POUSSE\n"); @@ -178,12 +182,14 @@ case pousse : pos = getPos(stockage_D,3); + wait_us(500); if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){ //pc.printf("%d\n",pos); pos = pos - 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,pos,1,BLED_ON,3); + wait_us(500); previous_pos = pos; etat = retour; //pc.printf("RETOUR\n"); @@ -193,6 +199,7 @@ case retour : pos = getPos(stockage_D,3) ; + wait_us(500); if(pos< (previous_pos+5)){ etat = pret; //pc.printf("PRET\n"); @@ -213,10 +220,12 @@ case init : velocityControl(stockage_G,-512,BLED_ON,1); + wait_us(500); pos = getPos(stockage_G,1); + wait_us(500); if(previous_pos != pos){ etat = etalonnage; - pc.printf("ETALONNAGE\n"); + // pc.printf("ETALONNAGE\n"); } break; @@ -227,15 +236,18 @@ velocityControl(stockage_G,0,GLED_ON,1); wait_ms(500); pos = getPos(stockage_G,1) ; + wait_us(500); compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1); + wait_us(500); previous_pos = pos; 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> (previous_pos-5)){ etat = pret; pc.printf("PRET\n"); @@ -246,32 +258,37 @@ if((status_contact & MASK_CT_GAUCHE)){ //pc.printf("%d\n",pos); pos = getPos(stockage_G,1)- 550; + wait_us(500); deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,pos,1,BLED_ON,1); + wait_us(500); previous_pos = pos; etat = pousse; - pc.printf("POUSSE\n"); + //pc.printf("POUSSE\n"); } break; case pousse : pos = getPos(stockage_G,1); + wait_us(500); if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){ - //pc.printf("%d\n",pos); + pc.printf("%d\n",pos); pos = pos + 550; deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,pos,1,BLED_ON,1); + wait_us(500); previous_pos = pos; etat = retour; - pc.printf("RETOUR\n"); + //pc.printf("RETOUR\n"); } break; case retour : pos = getPos(stockage_G,1) ; + wait_us(500); if(pos> (previous_pos-5)){ etat = pret; pc.printf("PRET\n"); @@ -292,65 +309,76 @@ case init : velocityControl(stockage_D,+512,BLED_ON,3); + wait_us(500); pos = getPos(stockage_D,3); + wait_us(500); if(previous_pos != pos){ - pc.printf("ETALONNAGE\n"); + //pc.printf("ETALONNAGE\n"); etat = etalonnage; } break; case etalonnage : pos = getPos(stockage_D,3); - pc.printf("%d\n", pos); + wait_us(500); + //pc.printf("%d\n", pos); if(pos>900 and (status_contact & MASK_FC_DROIT)){ velocityControl(stockage_D,0,GLED_ON,3); wait_ms(500); pos = getPos(stockage_D,3) ; + wait_us(500); positionControl(stockage_D,pos-180,1,BLED_ON,3); + wait_us(500); previous_pos = pos; etat = tmp_pret; - pc.printf("TMP_PRET\n"); + // pc.printf("TMP_PRET\n"); } break; case tmp_pret : pos = getPos(stockage_D,3) ; + wait_us(500); if(pos< (previous_pos+5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; case pret : if((status_contact & MASK_CT_DROIT)){ pos = getPos(stockage_D,3)- 550; + wait_us(500); deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,pos,1,BLED_ON,3); + wait_us(500); previous_pos = pos; etat = pousse; - pc.printf("POUSSE\n"); + //pc.printf("POUSSE\n"); } break; case pousse : pos = getPos(stockage_D,3); + wait_us(500); if(pos<(previous_pos+5) and (status_contact & MASK_CT_DROIT)==0){ pos = pos + 550; deverouillage_torque_convoyeurs_droit(); wait_us(500); positionControl(stockage_D,pos,1,BLED_ON,3); + wait_us(500); previous_pos = pos; etat = retour; - pc.printf("RETOUR\n"); + //pc.printf("RETOUR\n"); } break; case retour : pos = getPos(stockage_D,3) ; + wait_us(500); if(pos> (previous_pos-5)){ etat = pret; - pc.printf("PRET\n"); + //pc.printf("PRET\n"); } break; } @@ -367,7 +395,7 @@ deverouillage_torque(); positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2); wait(0.5); - positionControl(doigt,POS_DOIGT_DROIT,100,BLED_ON,2); + } @@ -385,11 +413,6 @@ buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert flag_ascenseur = 1; if(FIFO_couleur_ecriture == FIFO_couleur_lecture) { - /* if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) { - positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu - } else { - positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert - }*/ oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]); } @@ -397,13 +420,6 @@ etat = tmp; } break; -/* - case atome : //on stocke dans la fifo la couleur de l'atome - buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert - flag_ascenseur = 1; - FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO; - etat = tmp; - break;*/ case tmp : //on attend que l'atome traité soit totalement passé if(!etat_cap) { @@ -440,19 +456,25 @@ oriente_doigt(etat_cap); etat = tmp; } - - /*if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) { - oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]); - etat = tmp; - }else if(etat_cap != 0) { - etat = tmp; + /*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture) + { + oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO); }*/ break; 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; FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO; + 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) + oriente_doigt(1); + } etat = atome; } break;