CRAC Team
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-31
- Revision:
- 20:109dbdd0230b
- Parent:
- 19:2281a0ef54e4
- Child:
- 21:04ae86f7fdc8
File content as of revision 20:109dbdd0230b:
#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 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, flag_ascenseur_force_on = 0, flag_ascenseur_force_off = 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
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;
int nbPaletsVerts = 0, nbPaletsBleus = 0;
Timer timer_bloquer;
Timer timeout;
void rateau(int etat){
uint8_t servos_rateau_on[4] = {GLED_ON, rateau_D,GLED_ON, rateau_G};
uint16_t pos_rateau_on[2] = {280,774};
uint8_t servos_rateau_off[4] = {GLED_ON, rateau_D, GLED_ON, rateau_G};
uint16_t pos_rateau_off[2] = {512,512};
if( etat == 1){
positionControl_Mul_ensemble_complex(2,100,servos_rateau_on,pos_rateau_on,3);
}
else{
positionControl_Mul_ensemble_complex(2,100,servos_rateau_off,pos_rateau_off,3);
}
}
void roue(int allume)
{
if(cote)
{
PWM_roue_Gauche.write(float(allume));// = 1.0;//VIT_ROUE*allume;
PWM_roue_Droite.write(0);
}
else
{
PWM_roue_Droite.write(float(allume));// = 1.0;//VIT_ROUE*allume;
PWM_roue_Gauche.write(0);
}
}
void convoyeur_gauche_jaune(void)
{
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, consigne_roue;
static int16_t previous_pos = getPos(stockage_G,1);
static int cpt_vider = 0;
int flag_debut = 0;
//static int pos_vide[8] = {800, 100, 500, 900, 200, 600, 1000, 400};
static int pos_vide[11] = {800, 100, 500, 900, 200, 600, 1000, 300, 700, 24, 400};
/*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;
flag_debut = 0;
etat = vide;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
} 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:
pos = getPos(stockage_G,1);
if(!flag_debut){
flag_debut =1;
consigne_pos = (pos+420)%1105;
}
else{
consigne_pos = (pos+400)%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<7){
compteTour(stockage_G,+200,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 tmp_vide:
pos = getPos(stockage_G,1) ;
wait_us(500);
if(pos< (consigne_pos+5)) {
cpt_vider +=1;
consigne_roue = 815;
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;
}*/
case vide:
velocityControl(stockage_G, 400, BLED_ON,1);
wait_ms(200);
roue(1);
if(cpt_vider<9){
etat = tmp_vide;
//pc.printf("TMP_VIDE\n");
}else{
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
SendCharCan(NB_PALETS_VERTS, nbPaletsVerts);
nbPaletsVerts =0;
cpt_vider = 0;
roue(0);
uint8_t servos_roulette_gauche[2] = {GLED_ON, roue_G};
uint16_t pos_roulette_gauche[1] = {512};
positionControl_Mul_ensemble_complex(1,100,servos_roulette_gauche,pos_roulette_gauche,1);
//pc.printf("INIT\n");
}
break;
case tmp_vide:
pos = getPos(stockage_G,1);
wait_us(100);
if(pos >= pos_vide[cpt_vider%11] && pos <= (pos_vide[cpt_vider%11]+20)){ //if(pos == pos_vide[cpt_vider%5])
velocityControl(stockage_G, 0, BLED_ON,1);
positionControl(stockage_G,pos-30,1,BLED_ON,1);
//pc.printf("pos :%d\n", pos);
consigne_roue = 840;
positionControl(roue_G,consigne_roue,1,BLED_ON,1);
etat = fin_vide;
timer_bloquer.start();
timer_bloquer.reset();
}
break;
case fin_vide:
pos = getPos(roue_G,1) ;
wait_us(500);
if(pos> (consigne_roue-5) || timer_bloquer>2) {
positionControl(roue_G,750,50,BLED_ON,1);
verification();
cpt_vider ++;
etat = vide;
}
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);
static int flag_ack = 0;
int vInit = 512;
/*
if(flag_vide_bleu){
etat = init;
flag_vide_bleu = 0;
}*/
switch(etat) {
case init :
velocityControl(stockage_D,vInit,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);
if(flag_ack){
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
SendCharCan(NB_PALETS_BLEU, nbPaletsBleus);
nbPaletsBleus = 0;
}
flag_ack = 1;
etat = pret;
//pc.printf("PRET\n");
}
break;
case pret :
if(flag_vide_bleu) {
flag_vide_bleu =0;
vInit = 1023;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
//pc.printf("vide\n");
} 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;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
//pc.printf("INIT\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, pos_pret;
static int16_t previous_pos = getPos(stockage_G,1);
static int vInit = -512;
static int flag_ack = 0;
/*
if(flag_vide_bleu){
etat = init;
flag_vide_bleu = 0;
}*/
switch(etat) {
case init :
velocityControl(stockage_G,vInit,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 depart: %d\n",pos);
wait_us(500);
pos_pret = pos + 500;
if(pos_pret >1105) pos_pret -= 1105;
//pc.printf("pos : %d consigne : %d\n",pos,pos_pret);
compteTour(stockage_G,1023,1,pos_pret,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> (pos_pret-5)) {
etat = pret;
if(flag_ack){
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
SendCharCan(NB_PALETS_BLEU, nbPaletsBleus);
nbPaletsBleus = 0;
}
flag_ack = 1;
//pc.printf("PRET\n");
}
break;
case pret :
if(flag_vide_bleu) {
flag_vide_bleu =0;
vInit = -1023;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
} else if((status_contact & MASK_CT_GAUCHE)) {
consigne_pos = (pos_pret- 550);
if(consigne_pos<0)consigne_pos+=1105;
wait_us(500);
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
//pc.printf("pos:%d consigne:%d\n",pos, consigne_pos);
//compteTour(stockage_G,-1023,0,consigne_pos,BLED_ON,1);
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);
deverouillage_torque_convoyeurs_gauche();
wait_us(500);
//compteTour(stockage_G,1023,0,pos_pret,BLED_ON,1);
positionControl(stockage_G,pos_pret,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> (pos_pret-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;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
SendChar(NB_PALETS_BLEU, nbPaletsVerts);
//pc.printf("PRET\n");
}
break;*/
}
}
void convoyeur_droit_violet(void)
{
typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide,tmp_vide} type_etat ;
static type_etat etat = init;
static int16_t pos = 0, consigne_pos,consigne_roue;
static int16_t previous_pos = getPos(stockage_D,3);
static int cpt_vider=0;
//static int pos_vide[5] = {400, 1000, 600, 200, 800};
static int pos_vide[11] = {400, 24, 700, 300, 1000, 600, 200, 900, 500, 100, 800};
/*
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;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
} 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;
*/
case vide:
velocityControl(stockage_D, -400, BLED_ON,3);
wait_ms(200);
roue(1);
if(cpt_vider<9){
etat = tmp_vide;
//pc.printf("TMP_VIDE\n");
}else{
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
SendCharCan(NB_PALETS_VERTS, nbPaletsVerts);
nbPaletsVerts =0;
cpt_vider = 0;
roue(0);
uint8_t servos_roulette_droit[2] = {GLED_ON, roue_D};
uint16_t pos_roulette_droit[1] = {512};
positionControl_Mul_ensemble_complex(1,100,servos_roulette_droit,pos_roulette_droit,3);
//pc.printf("INIT\n");
}
break;
case tmp_vide:
pos = getPos(stockage_D,3);
wait_us(100);
if(pos <= pos_vide[cpt_vider%11] && pos >= (pos_vide[cpt_vider%5]-20)){ //if(pos == pos_vide[cpt_vider%5])
velocityControl(stockage_D, 0, BLED_ON,3);
positionControl(stockage_D,pos+30,1,BLED_ON,3);
//pc.printf("pos :%d\n", pos);
consigne_roue = 225;
positionControl(roue_D,consigne_roue,1,BLED_ON,3);
etat = fin_vide;
timer_bloquer.start();
timer_bloquer.reset();
}
break;
case fin_vide:
pos = getPos(roue_D,3) ;
wait_us(500);
if(pos> (consigne_roue+5) || timer_bloquer>2) {
positionControl(roue_D,374,50,BLED_ON,3);
verification();
cpt_vider ++;
etat = vide;
}
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
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);
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);
rateau(0);
SendRawId(HACHEUR_ETAT_CONTACTS);
}
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 && !flag_ascenseur_force_off) {
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]);
}
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_dernier = 0, flag_suivant = 0;
static long cpt, cpt_bloquer = 0;
static char memo_FIFO_couleur_lecture;
if(flag_ascenseur_force_off)etat = init;
switch(etat) {
case init :
//on attend le premier atome et place le herkulex en fonction
if( (flag_ascenseur || flag_ascenseur_force_on) && !flag_ascenseur_force_off ) {
SendCharCan(HACHEUR_ID_COUROIES,1);
/*timer_bloquer.start();
timer_bloquer.reset();*/
etat = atome;
}
break;
case atome :
//on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
cpt++;
/*if( (timer_bloquer.read() > 3.0) && (FIFO_couleur_lecture != FIFO_couleur_ecriture)){
flag_ascenseur_force_off = 1;
SendCharCan(HACHEUR_ID_COUROIES,0);
etat = init;
}
else*/ 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_force_on){
SendCharCan(HACHEUR_ID_COUROIES,0);
etat = init;
}
if(etat_cap != 0) {
cpt = 0;
if(etat_cap == 1)nbPaletsBleus ++;
else if(etat_cap == 2) nbPaletsVerts ++;
oriente_doigt(etat_cap);
//timer_bloquer.reset();
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( (timer_bloquer.read() > 5.0) && (FIFO_couleur_lecture != FIFO_couleur_ecriture)){
flag_ascenseur_force_off = 1;
SendCharCan(HACHEUR_ID_COUROIES,0);
etat = init;
}*/
if(etat_cap == 0) {
//timer_bloquer.reset();
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);*/
}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;
}
break;
}
}
void oriente_doigt(int palet)
{
if(cote) {
if(palet == 1) {//position herkulex stockage bleu
SendCharCan(HACHEUR_ID_COUROIES,2);
positionControl(doigt,POS_DOIGT_GAUCHE,20,BLED_ON,2);
} else if (palet == 2) {//position herkulex stockage rouge/vert
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);
} else if (palet== 2) {//position herkulex stockage rouge/vert
SendCharCan(HACHEUR_ID_COUROIES,1);
positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,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};
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)
{
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,reculer,xyt, fin} 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);
wait_us(50);
SendRawId(HACHEUR_STATUT_VENTOUSES);
wait_us(50);
goldenium_avant();
char pompe = AV_CENTRE;
SendSpeed(VITESSE_RALENTI,ACCELERATION_RALENTI,DECELERATION_RALENTI);
wait_us(50);
GoStraight(200,0,0,0);//on avance vers le goldenium
wait_us(50);
can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
wait_us(50);
timeout.start();
timeout.reset();
etat = attente_ack_ventouse;
break;
case attente_ack_ventouse://auotmate ack carte pompe merde, renvoi ack goldenium saisi de temps en temps
if (ackFinAction == ASSERVISSEMENT_ERROR_MOTEUR || ackFinAction == ASSERVISSEMENT_RECALAGE ) //((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) //dès qu'il est saisi on attend pour etre sur
{
wait(0.1);
ackFinAction = 0;
//(ASSERVISSEMENT_STOP);
SendRawId(0x710);//on s'arrete
timeout.reset();
etat = reculer;
} else if(timeout.read_ms()>5000 && 0) {
ackFinAction = 0;
//SendRawId(ASSERVISSEMENT_STOP);
SendRawId(0x711);//on s'arrete
timeout.reset();
etat = reculer;
}
break;
case reculer:
if( ackFinAction == ASSERVISSEMENT_STOP || ackFinAction == ASSERVISSEMENT_ERROR_MOTEUR || ackFinAction == ASSERVISSEMENT_STOP || 1 )
{
ackFinAction = 0;
GoStraight(-40,0,0,0);//on recule en x donné par la strat
wait(0.1);
positionControl(AV_poigne_C,450,150,BLED_ON,2);//forklift
fgoldenium_avant=0;
etat = xyt;
}
break;
case xyt:
if(ackFinAction == ASSERVISSEMENT_RECALAGE)
{
ackFinAction = 0;
SendSpeed(VITESSE_NORMAL,ACCELERATION,DECELERATION);
GoToPosition( x_goldenium,y_goldenium,1800,-1);//x et y donné par la strat ,theta constant,-1 marche arriere
etat = fin;
}
break;
case fin :
if(ackFinAction == ASSERVISSEMENT_XYT)
{
ackFinAction = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
}
break;
}
}
void accelerateur_avant(void)
{
}
void automate_ventouse_accelerateur_avant(void)
{
}
#endif