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-24
- Revision:
- 5:bb533bf81ee6
- Parent:
- 4:8ac0f7c17ac7
- Child:
- 6:aa800daf5ff9
File content as of revision 5:bb533bf81ee6:
#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 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; 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(void){ typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ; static type_etat etat = init; static int16_t pos = 0; static int16_t previous_pos = getPos(stockage_G,1); 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); if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ velocityControl(stockage_G,0,GLED_ON,1); wait_ms(500); pos = getPos(stockage_G,1) ; positionControl(stockage_G,pos+180,1,BLED_ON,1); etat = tmp_pret; pc.printf("TMP_PRET\n"); } break; case tmp_pret : previous_pos = pos; pos = getPos(stockage_G,1) ; if(pos> (previous_pos-5)){ etat = pret; pc.printf("PRET\n"); } break; case pret : if((status_contact & MASK_CT_GAUCHE)){ pos = getPos(stockage_G,1)+550; positionControl(stockage_G,pos,1,BLED_ON,1); previous_pos = pos; etat = pousse; pc.printf("POUSSE\n"); } break; case pousse : pos = getPos(stockage_G,1); if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){ pos = pos - 550; positionControl(stockage_G,pos,1,BLED_ON,1); previous_pos = pos; etat = retour; pc.printf("RETOUR\n"); } break; case retour : pos = getPos(stockage_G,1) ; if(pos> (previous_pos-5)){ etat = pret; pc.printf("PRET\n"); } break; } } void convoyeur_droit(void){ typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ; static type_etat etat = init; static int16_t pos = 0; static int16_t previous_pos = getPos(stockage_D,3); switch(etat){ case init : velocityControl(stockage_D,512,BLED_ON,3); pos = getPos(stockage_D,3); if(previous_pos != pos){ etat = etalonnage; pc.printf("ETALONNAGE\n"); } break; case etalonnage : pos = getPos(stockage_D,3); if(pos>900 and (status_contact & MASK_FC_DROIT)){ velocityControl(stockage_D,0,GLED_ON,3); wait_ms(500); pos = getPos(stockage_D,3) ; //positionControl(stockage_D,pos-2000,1,BLED_ON,3); compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3); etat = tmp_pret; pc.printf("TMP_PRET\n"); } break; case tmp_pret : previous_pos = pos; pos = getPos(stockage_D,3) ; if(pos> (previous_pos+5)){ etat = pret; pc.printf("PRET\n"); } break; case pret : if((status_contact & MASK_CT_DROIT)){ //pc.printf("%d\n",pos); pos = getPos(stockage_D,3)+ 550; positionControl(stockage_D,pos,1,BLED_ON,3); previous_pos = pos; etat = pousse; pc.printf("POUSSE\n"); } break; case pousse : pos = getPos(stockage_D,3); if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){ //pc.printf("%d\n",pos); pos = pos - 550; positionControl(stockage_D,pos,1,BLED_ON,3); previous_pos = pos; etat = retour; pc.printf("RETOUR\n"); } break; case retour : pos = getPos(stockage_D,3) ; if(pos< (previous_pos+5)){ etat = pret; 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); } 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) { /* 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]); } FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO; 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) { 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(buffer_couleur_bas[FIFO_couleur_lecture] != 0) { oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]); /* if(cote) if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu positionControl(doigt,640,10,BLED_ON,2); } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert positionControl(doigt,384,10,GLED_ON,2); } }else{ if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu positionControl(doigt,384,10,BLED_ON,2); } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert positionControl(doigt,640,10,GLED_ON,2); } }*/ etat = tmp; }else if(etat_cap != 0) { etat = tmp; } break; case tmp : //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture if(etat_cap == 0) { FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO; etat = atome; } break; } } void oriente_doigt(int palet) { if(cote) { if(palet == 1) {//position herkulex stockage bleu positionControl(doigt,640,10,BLED_ON,2); } else if (palet == 2) {//position herkulex stockage rouge/vert positionControl(doigt,384,10,GLED_ON,2); } } else { if(palet== 1) {//position herkulex stockage bleu positionControl(doigt,384,10,BLED_ON,2); } else if (palet== 2) {//position herkulex stockage rouge/vert positionControl(doigt,640,10,GLED_ON,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