CRAC Team
librairie actions petit robot carte esclave
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019 ACRAC_carte_esclave_GROS_ROBOT_2019
actions_Pr.cpp
- Committer:
- goldmas
- Date:
- 2021-05-18
- Revision:
- 29:24277e12c44c
- Parent:
- 28:4d435c77a221
- Child:
- 30:f052129d3b4e
File content as of revision 29:24277e12c44c:
#include "actions_Pr.h"
Timer timeout;
//variables//
char aut_bras_av_at = 0, aut_bras_av_re = 0;
char aut_bras_av_1_at = 0, aut_bras_av_1_re = 0;
char aut_bras_av_2_at = 0, aut_bras_av_2_re = 0;
char aut_bras_av_3_at = 0, aut_bras_av_3_re = 0;
char aut_manche_haut = 0, aut_manche_bas = 0 ;
unsigned char var_bras_choix_1 = 0, var_bras_choix_2= 0, var_bras_choix_3= 0, var_bras_manche = 0;
char vitesse_bras = 50;
//define bras gauche et droit
uint8_t servos_bras_avant[6] = {RLED_ON, 1, GLED_ON, 2, BLED_ON, 3};
uint8_t servos_bras_arriere[6] = {RLED_ON, 4, GLED_ON, 5, BLED_ON, 6};
//position des bras
uint16_t pos_initial_1[3] = {600,200,200};
uint16_t pos_prepa_attraper_1[3] = {512,220,200};
uint16_t pos_revenir_1[3] = {600,200,70};
uint16_t pos_prepa_relacher_1[3] = {512,220,70};
uint16_t pos_initial_2[3] = {760,130,200};
uint16_t pos_prepa_relacher_2[3] = {240,512,70};
uint16_t pos_prepa_attraper_2[3] = {220,630,200};
uint16_t pos_revenir_2[3] = {760,150,70};
uint16_t pos_prepa_attraper_3[3] = {320,430,200};
uint16_t pos_attraper[3] = {270,430,70};
uint16_t pos_relacher[3] = {240,512,200};
void gabarit_robot_droit(void)
{
pc.printf("\ngabarit robot avant\n\n");
positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 3);
positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 2);
verification();
}
void gabarit_robot_gauche(void)
{
pc.printf("\ngabarit robot arriere\n\n");
positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 2);
positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 3);
verification();
}
void gabarit_robot_manche(void)
{
positionControl(7,220,80,0,4);
positionControl(8,240,80,0,4);
//positionControl(11,780,80,0,4);
verification();
}
///////////////////////////////////////////////////fonction de test////////////////////////////////////////////////
void test_BRAS_1(void)
{
positionControl(7,410,80, 0,4);
positionControl(8,410,80, 0,4);
positionControl(11,230,80,0,4);
wait(2);
positionControl(7,230,80,0,4);
positionControl(8,230,80,0,4);
positionControl(11,780,80,0,4);
verification();
}
void test_BRAS_2(void)
{
verification();
}
void test_BRAS_3(void)
{
verification();
}
void test_BRAS_4(void)
{
verification();
}
///////////////////////////////////////////////SELECTION ATTRAPER BRAS//////////////////////////////////////////////
void selection_bras_attraper(void)
{
if(aut_bras_av_at)
{
if(bras_choix<6)
{
aut_bras_av_1_at = aut_bras_av_at;
automate_bras_attraper_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 20:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 21:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 210:
aut_bras_av_3_at=aut_bras_av_at;
automate_bras_attraper_3();
break;
case 43:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 53:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 54:
aut_bras_av_2_at=aut_bras_av_at;
automate_bras_attraper_2();
break;
case 66:
aut_bras_av_3_at=aut_bras_av_at;
automate_bras_attraper_3();
break;
}
}
}
}
///////////////////////////////////////////////SELECTION RELACHER BRAS//////////////////////////////////////////////
void selection_bras_relacher(void)
{
if(aut_bras_av_re)
{
if(bras_choix<6)
{
aut_bras_av_1_re = aut_bras_av_re;
automate_bras_relacher_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 20:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 21:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 210:
aut_bras_av_3_re=aut_bras_av_re;
automate_bras_relacher_3();
break;
case 43:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 53:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 54:
aut_bras_av_2_re=aut_bras_av_re;
automate_bras_relacher_2();
break;
case 66:
aut_bras_av_3_re=aut_bras_av_re;
automate_bras_relacher_3();
break;
}
}
}
}
///////////////////////////////////////////////1 BRAS//////////////////////////////////////////////
void automate_bras_attraper_1(void)
{
typedef enum {init, aut_pos_prepa_attraper_1, aut_pos_prepa_attraper_2, aut_pos_prepa_attraper_3, aut_pos_attraper, aut_pos_revenir_1, aut_pos_revenir_2} type_etat;
static type_etat etat = init;
static unsigned char sens_avant=0;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_1_at)
{
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3)
{
sens_avant=1;
if(var_bras_choix_1==0)
var_bras_choix_1=3;
else if(var_bras_choix_1==1)
var_bras_choix_1=2;
else if(var_bras_choix_1==2)
var_bras_choix_1=1;
}
etat=aut_pos_prepa_attraper_1;
}
break;
case aut_pos_prepa_attraper_1://envoi instruction mouvement 1
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, var_bras_choix_1);
}
verification();
etat = aut_pos_prepa_attraper_2;
break;
case aut_pos_prepa_attraper_2://envoi instruction mouvement 2
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, var_bras_choix_1);
}
verification();
etat = aut_pos_prepa_attraper_3;
break;
case aut_pos_prepa_attraper_3://envoi instruction mouvement 1
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, var_bras_choix_1);
}
verification();
etat = aut_pos_attraper;
break;
case aut_pos_attraper://envoi instruction mouvement 1
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, var_bras_choix_1);
}
verification();
etat = aut_pos_revenir_1;
break;
case aut_pos_revenir_1://envoi instruction mouvement 1
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, var_bras_choix_1);
}
verification();
etat = aut_pos_revenir_2;
break;
case aut_pos_revenir_2://envoi instruction mouvement 1
if(sens_avant == 0)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, var_bras_choix_1-2);
}
else
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, var_bras_choix_1);
}
verification();
aut_bras_av_1_at = 0;
aut_bras_av_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
void automate_bras_relacher_1(void)
{
typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
static type_etat etat = init;
static unsigned char sens_avant=0;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_1_re){
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3)
{
sens_avant=1;
if(var_bras_choix_1==0)
var_bras_choix_1=3;
else if(var_bras_choix_1==1)
var_bras_choix_1=2;
else if(var_bras_choix_1==2)
var_bras_choix_1=1;
}
etat=aut_pos_prepa_relacher_1;
}
break;
case aut_pos_prepa_relacher_1://envoi instruction
if(sens_avant==0){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, var_bras_choix_1 - 2);}
else{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, var_bras_choix_1);}
verification();
etat = aut_pos_prepa_relacher_2;
break;
case aut_pos_prepa_relacher_2://envoi instruction
if(sens_avant==0){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, var_bras_choix_1 - 2);}
else{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, var_bras_choix_1);}
verification();
etat = aut_pos_relacher;
break;
case aut_pos_relacher://envoi instruction
if(sens_avant==0){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, var_bras_choix_1 - 2);}
else{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, var_bras_choix_1);}
verification();
etat = aut_pos_initial_1;
break;
case aut_pos_initial_1://envoi instruction
if(sens_avant==0){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, var_bras_choix_1 - 2);}
else{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, var_bras_choix_1);}
verification();
etat = aut_pos_initial_2;
break;
case aut_pos_initial_2://envoi instruction
if(sens_avant==0){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, var_bras_choix_1 - 2);}
else{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, var_bras_choix_1);}
verification();
aut_bras_av_1_re = 0;
aut_bras_av_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
//////////////////////////////////////////////////2 bras//////////////////////////////////////////////////////
void automate_bras_attraper_2(void)
{
typedef enum {init, aut_pos_prepa_attraper_1, aut_pos_prepa_attraper_2, aut_pos_prepa_attraper_3, aut_pos_attraper, aut_pos_revenir_1, aut_pos_revenir_2} type_etat;
static type_etat etat = init;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_2_at){
var_bras_choix_2 = bras_choix;
etat=aut_pos_prepa_attraper_1;}
break;
case aut_pos_prepa_attraper_1://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
}
verification();
etat = aut_pos_prepa_attraper_2;
break;
case aut_pos_prepa_attraper_2://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
}
verification();
etat = aut_pos_prepa_attraper_3;
break;
case aut_pos_prepa_attraper_3://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
}
verification();
etat = aut_pos_attraper;
break;
case aut_pos_attraper://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
}
verification();
etat = aut_pos_revenir_1;
break;
case aut_pos_revenir_1://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
}
verification();
etat = aut_pos_revenir_2;
break;
case aut_pos_revenir_2://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
}
verification();
aut_bras_av_2_at = 0;
aut_bras_av_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
void automate_bras_relacher_2(void)
{
typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
static type_etat etat = init;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_2_re){
var_bras_choix_2 = bras_choix;
etat=aut_pos_prepa_relacher_1;}
break;
case aut_pos_prepa_relacher_1://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
}
verification();
etat = aut_pos_prepa_relacher_2;
break;
case aut_pos_prepa_relacher_2://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
}
verification();
etat = aut_pos_relacher;
break;
case aut_pos_relacher://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
}
verification();
etat = aut_pos_initial_1;
break;
case aut_pos_initial_1://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
}
verification();
etat = aut_pos_initial_2;
break;
case aut_pos_initial_2://envoi instruction
if(var_bras_choix_2 == 21){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
}else if(var_bras_choix_2 == 20){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
}else if(var_bras_choix_2 == 10){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
}else if(var_bras_choix_2 == 43){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
}else if(var_bras_choix_2 == 53){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
}else if(var_bras_choix_2 == 54){
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
}
verification();
aut_bras_av_2_re = 0;
aut_bras_av_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
////////////////////////////////////////////////3 BRAS/////////////////////////////////////////////////////
void automate_bras_attraper_3(void)
{
typedef enum {init, aut_pos_prepa_attraper_1, aut_pos_prepa_attraper_2, aut_pos_prepa_attraper_3, aut_pos_attraper, aut_pos_revenir_1, aut_pos_revenir_2} type_etat;
static type_etat etat = init;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_3_at){
var_bras_choix_3 = bras_choix;
etat=aut_pos_prepa_attraper_1;}
break;
case aut_pos_prepa_attraper_1://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
//verification_3_bras(0);
verification();
}
etat = aut_pos_prepa_attraper_2;
break;
case aut_pos_prepa_attraper_2://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
//verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
//verification_3_bras(0);
verification();
}
etat = aut_pos_prepa_attraper_3;
break;
case aut_pos_prepa_attraper_3://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
//verification_3_bras(0);
verification();
}
etat = aut_pos_attraper;
break;
case aut_pos_attraper://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
//verification_3_bras(0);
verification();
}
etat = aut_pos_revenir_1;
break;
case aut_pos_revenir_1://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
//verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
//verification_3_bras(0);
verification();
}
etat = aut_pos_revenir_2;
break;
case aut_pos_revenir_2://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
//verification_3_bras(0);
verification();
}
aut_bras_av_3_at = 0;
aut_bras_av_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
void automate_bras_relacher_3(void)
{
typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
static type_etat etat = init;
switch(etat)
{
case init: //attente d'initialisation
if(aut_bras_av_3_re){
etat=aut_pos_prepa_relacher_1;}
var_bras_choix_3 = bras_choix;
break;
case aut_pos_prepa_relacher_1://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
verification_3_bras(0);
}
etat = aut_pos_prepa_relacher_2;
break;
case aut_pos_prepa_relacher_2://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
verification_3_bras(0);
}
etat = aut_pos_relacher;
break;
case aut_pos_relacher://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
//verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
//verification_3_bras(0);
}
etat = aut_pos_initial_1;
break;
case aut_pos_initial_1://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
//verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
//verification_3_bras(0);
}
etat = aut_pos_initial_2;
break;
case aut_pos_initial_2://envoi instruction
if(var_bras_choix_3 == 210)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
verification_3_bras(1);
}
else if(var_bras_choix_3 == 66)
{
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
verification_3_bras(0);
}
aut_bras_av_3_re = 0;
aut_bras_av_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
//////////////////////////////////AUTOMATE MANCHE A AIR & Drapeau/////////////////////////////////////
void automate_manche_air_haut(void)
{
if(aut_manche_haut) // 0 = amche à aire droite 1 = manche à aire gauche
{
var_bras_manche = bras_choix ;
if(var_bras_manche == 1)
{
positionControl(8,240,80, 0,4);
verification();
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
else if (var_bras_manche == 0)
{
positionControl(7,220,80, 0,4);
verification();
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
aut_manche_haut = 0 ;
}
}
void automate_manche_air_bas(void)
{
if(aut_manche_bas) // 0 = amche à aire droite 1 = manche à aire gauche
{
var_bras_manche = bras_choix ;
if(var_bras_manche == 1)
{
positionControl(8,512,80, 0,4);
verification();
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
else if (var_bras_manche == 0)
{
positionControl(7,512,80, 0,4);
verification();
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
aut_manche_bas = 0 ;
}
}