librairie actions gros robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019
actions_Gr.cpp
- Committer:
- marwanesaich
- Date:
- 2019-05-26
- Revision:
- 14:bf2810b95f25
- Parent:
- 13:0b5310c6dc44
- Child:
- 15:1fefa6b1569b
File content as of revision 14:bf2810b95f25:
#include "actions_Gr.h" #ifdef ROBOT_BIG #define MASK_PRESENTOIR_AV 0x07 #define MASK_PRESENTOIR_AR 0x70 #define MASK_GOLDENIUM_AV 0x02 #define MASK_GOLDENIUM_AR 0x20 #define MASK_SOL_AV 0x08 #define MASK_SOL_AR 0x80 #define MASK_AV_DROIT 0x01 #define MASK_AR_DROIT 0x10 #define MASK_AV_DROIT_GAUCHE 0x05 #define MASK_AR_DROIT_GAUCHE 0x50 #define MASK_FC_DROIT 0x01 #define MASK_CT_DROIT 0x02 #define MASK_FC_GAUCHE 0x04 #define MASK_CT_GAUCHE 0x08 #define POS_DOIGT_GAUCHE 585 #define POS_DOIGT_DROIT 389 char status_pompe=0; bool flag_ascenseur = 0; char fpresentoir_avant=0, fpresentoir_arriere=0; char fgoldenium_avant=0, fgoldenium_arriere=0; char fsol_avant=0,fsol_arriere=0; char fsol_avant_relache=0,fsol_arriere_relache=0; char fbalance_avant=0,fbalance_arriere=0; 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 char buffer_couleur_bas[SIZE_FIFO]; unsigned char FIFO_couleur_ecriture=0; signed char FIFO_couleur_lecture=0; char status_contact=0; 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); /*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) { etat = etalonnage; //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)) { 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"); } break; case tmp_pret : pos = getPos(stockage_G,1); wait_us(500); if(pos> (consigne_pos-5)) { //pc.printf("pos pret %d\n", pos); etat = pret; //pc.printf("PRET\n"); } break; 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"); } 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; deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); 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"); } 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"); } 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); /* 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) { etat = etalonnage; //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)) { velocityControl(stockage_D,0,GLED_ON,3); wait_ms(300); pos = getPos(stockage_D,3) ; //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); compteTour(stockage_D,-1023,1,consigne_pos,BLED_ON,3); //pc.printf("pos fin tour : %d \n",getPos(stockage_D,3)); wait_us(500); etat = tmp_pret; //pc.printf("TMP_PRET\n"); } break; case tmp_pret : pos = getPos(stockage_D,3) ; wait_us(500); if(pos < (consigne_pos+5)) { //pc.printf("pos fin %d\n", pos); etat = pret; //pc.printf("PRET\n"); } break; 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(); wait_us(500); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = pousse; //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) { //pc.printf("%d\n",pos); consigne_pos = pos - 550; 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"); } break; case retour : pos = getPos(stockage_D,3) ; wait_us(500); 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, 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); /* if(flag_vide_bleu){ etat = init; flag_vide_bleu = 0; }*/ switch(etat) { 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"); } 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 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); wait_us(500); etat = tmp_pret; ////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"); } break; 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); deverouillage_torque_convoyeurs_gauche(); wait_us(500); positionControl(stockage_G,consigne_pos,1,BLED_ON,1); wait_us(500); etat = pousse; //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) { //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); wait_us(500); 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"); } 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, 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); /* 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); pos = getPos(stockage_D,3); wait_us(500); if(previous_pos != pos) { //pc.printf("ETALONNAGE\n"); etat = etalonnage; } break; case etalonnage : pos = getPos(stockage_D,3); 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 velocityControl(stockage_D,0,GLED_ON,3); wait_ms(500); consigne_pos = getPos(stockage_D,3) - 180 ; wait_us(500); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = tmp_pret; // pc.printf("TMP_PRET\n"); } break; case tmp_pret : pos = getPos(stockage_D,3) ; wait_us(500); if(pos< (consigne_pos+5)) { etat = pret; //pc.printf("PRET\n"); } break; 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); positionControl(stockage_D,consigne_pos,1,BLED_ON,3); wait_us(500); etat = pousse; //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); 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"); } break; case retour : pos = getPos(stockage_D,3) ; 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}; uint16_t pos_av_centre[2] = {550,550};//470,350 int speed=100; deverouillage_torque(); positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2); wait(0.5); } void fifo_couleur(void) { typedef enum {n_atome, atome, tmp} type_etat ; static type_etat etat = n_atome; int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2; switch(etat) { case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO if(etat_cap) { buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert flag_ascenseur = 1; 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; } break; case tmp : //on attend que l'atome traité soit totalement passé if(!etat_cap) { etat = n_atome; } break; } } void ascenseur(void) { typedef enum {init, atome, tmp} type_etat; static type_etat etat = init; 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 if(flag_ascenseur) { //SendCharCan(HACHEUR_ID_COUROIES,1); etat = atome; } break; case atome : //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO if(etat_cap != 0) { oriente_doigt(etat_cap); 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; } } void oriente_doigt(int palet) { if(cote) { if(palet == 1) {//position herkulex stockage bleu positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2); SendCharCan(HACHEUR_ID_COUROIES,2); } 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 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2); SendCharCan(HACHEUR_ID_COUROIES,1); } else if (palet== 2) {//position herkulex stockage rouge/vert positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2); SendCharCan(HACHEUR_ID_COUROIES,2); } } } void presentoir_avant(void) { uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C}; uint16_t pos_av_centre[2] = {512,512}; int speed=1; positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2); verification(); } void automate_ventouse_presentoir_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(fpresentoir_avant) etat=envoi_instruction; break; case envoi_instruction://envoi instruction SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); presentoir_avant(); SendRawId(HACHEUR_GET_PRESENTOIR_AV); SendRawId(HACHEUR_STATUT_VENTOUSES); etat = attente_ack_ventouse; break; case attente_ack_ventouse: if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) { fpresentoir_avant=0; SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); etat = init; } break; } } void automate_ventouse_relache_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(favant_relache) etat=envoi_instruction; break; case envoi_instruction://envoi instruction SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION); SendRawId(HACHEUR_STATUT_VENTOUSES); etat = attente_ack_ventouse; break; case attente_ack_ventouse: SendRawId(HACHEUR_RELEASE_AV); if((status_pompe&MASK_PRESENTOIR_AV)== 0) { favant_relache=0; SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION); etat = init; } break; } } void goldenium_avant(void) { } void automate_ventouse_goldenium_avant (void) { } void accelerateur_avant(void) { } void automate_ventouse_accelerateur_avant(void) { } #endif