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:
- ares1999
- Date:
- 2022-05-18
- Revision:
- 35:89c1a096e896
- Parent:
- 34:2d7d09f9965c
File content as of revision 35:89c1a096e896:
#include "actions_Pr.h"
int mesure_resistor(float vpot,bool color) // definition de la fontion qui mesure les resistances
{
float sample = vpot;
if ((sample > 0.8f) && (sample < 0.87f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance bleu
if (color == 1) {
pc.printf("test non ok \n");
return Enemy_color;
} else {
pc.printf("test non ok1 \n");
return Mycolor;
}
} else if ((sample > 0.37f) && (sample < 0.42f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance rouge
pc.printf("test ok0 \n");
return rouge;
} else if ((sample > 0.70f) && (sample < 0.78f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance jaune
if (color == 1) {
pc.printf("test non ok2 \n");
return Mycolor;
} else {
pc.printf("test non ok3 \n");
return Enemy_color;
}
}
pc.printf("nope");
pc.printf(" %f",sample);
return 9;
}
//--------------------------------------------------------2022---------------------------------------------------------------------------------------------------------------------------
//moi
int tab[7]= {9,Mycolor,9,9,9,9,9};
bool choix_color=0;
AnalogIn releve(PA_5);
AnalogIn releve1(PA_7);
bool delay=true;
int resultat;
uint8_t correction=0;
char msg_num_ca=0;
////////////////////////////////////////////////BRAS DE MESURE/////////////////////////////////////////////////////
char bras_choix=0,num_ca=0,msg_carre=0, ouv_ferm_CN=0;
int traitement()
{
typedef enum {init,ranger,mesure, bascule, pretest} type_etat;
static type_etat etat = init;
switch(etat) {
case init:
if(msg_carre==1) {
etat=ranger;
msg_carre=0;
} else if(msg_carre==2) {
etat=mesure;
msg_carre=0;
delay=true;
} else if(msg_carre==3) {
etat=pretest;
msg_carre=0;
}
break;
case ranger:
pc.printf("RANGER ");
positionControl(ID_MILLIEU-choix_color*2, 800, PLAYTIME, RLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT-choix_color*2, 512, PLAYTIME, RLED_ON, SERIAL_SPECIAL);
if(msg_carre==2) {
etat=mesure;
msg_carre=0;
delay=true;
} else if(msg_carre==3) {
etat=pretest;
msg_carre=0;
}
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
case mesure:
if(tab[num_ca]==9) {
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
while(delay ) {
if(choix_color==0){
resultat=mesure_resistor(releve1,choix_color);
}
else{
resultat=mesure_resistor(releve,choix_color);
}
if(resultat==0) {
delay=false;
}
if(resultat==1) {
delay=false;
}
if(resultat==2) {
delay=false;
}
if(correction==30) {
pc.printf("eror");
correction=0;
etat=pretest;
break;
} else {
correction=correction+5;
positionControl(ID_MILLIEU-choix_color*2, 602-correction, 40, GLED_ON, SERIAL_SPECIAL); // on positionne la base 565
}
}
tab[num_ca]=resultat;
algo_carre (num_ca);
}
if(tab[num_ca]==rouge) {
etat=pretest;
SendCharCan(0x35,tab[num_ca]);
} else if(tab[num_ca]==Mycolor) {
etat=bascule;
SendCharCan(0x35,tab[num_ca]);
} else if(tab[num_ca]==Enemy_color) {
etat=pretest;
SendCharCan(0x35,tab[num_ca]);
}
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
case bascule:
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT-choix_color*2, 119, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
etat=pretest;//peut etre a changer
break;
case pretest:
positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL); // on positionne la base 770
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat=init;
break;
}
return 0;
}
void set_color(bool color)
{
choix_color=color;
pc.printf("ident haut : %X \n",ID_HAUT - choix_color*2);
}
void BF_test_mesure(bool BJ)
{
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
if(mesure_resistor(releve,BJ)==1) {
positionControl(ID_HAUT-choix_color*2, 119, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
SendCharCan(0x35,0x01);
} else if(mesure_resistor(releve,BJ)==0) {
positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
SendCharCan(0x35,0x00);
} else if(mesure_resistor(releve,BJ)==2) {
positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL);
positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
SendCharCan(0x35,0x02);
}
}
int algo_carre (int nombre)
{
switch (nombre) {
case 0:
if(resultat==Mycolor) {
tab[2]=rouge;
} else {
tab[2]=Mycolor;
}
break;
case 2:
if(resultat==Mycolor) {
tab[0]=rouge;
} else {
tab[1]=Mycolor;
}
break;
case 3:
if(resultat==Mycolor) {
tab[4]=Enemy_color;
tab[5]=Enemy_color;
tab[6]=Mycolor;
} else {
tab[4]=Mycolor;
tab[5]=Mycolor;
tab[6]=Enemy_color;
}
break;
case 4:
if(resultat==Mycolor) {
tab[3]=Enemy_color;
tab[5]=Mycolor;
tab[6]=Enemy_color;
} else {
tab[3]=Mycolor;
tab[5]=Enemy_color;
tab[6]=Mycolor;
}
break;
case 5:
if(resultat==Mycolor) {
tab[3]=Enemy_color;
tab[4]=Mycolor;
tab[6]=Enemy_color;
} else {
tab[3]=Mycolor;
tab[4]=Enemy_color;
tab[6]=Mycolor;
}
break;
case 6:
if(resultat==Mycolor) {
tab[3]=Mycolor;
tab[4]=Enemy_color;
tab[5]=Enemy_color;
} else {
tab[3]=Enemy_color;
tab[4]=Mycolor;
tab[5]=Mycolor;
}
break;
}
return 0;
}
////////////////////////////////////////////////BRAS 2022/////////////////////////////////////////////////////
//Variables "data" fonction positionControl_Mul_ensemble_complex
uint8_t servos_BAV[6] = {RLED_ON, BAV_BASE, GLED_ON, BAV_MILIEU, BLED_ON, BAV_HAUT};
uint8_t servos_BAR[6] = {RLED_ON, BAR_BASE, GLED_ON, BAR_MILIEU, BLED_ON, BAR_HAUT};
uint8_t servos_HAV[6] = {RLED_ON, HAV_BASE, GLED_ON, HAV_MILIEU, BLED_ON, HAV_HAUT};
uint8_t servos_HAR[6] = {RLED_ON, HAR_BASE, GLED_ON, HAR_MILIEU, BLED_ON, HAR_HAUT};
//Variables position des bras
//bras Bas Gauche
uint16_t rangement_BAV[3] = {950,75,850};
uint16_t pre_prise_BAV[3] = {790,210,790};
uint16_t prise_BAV[3] = {560,330,660};
uint16_t rangement2_BAV[3] = {950,75,510};
//Positions intermédiaires bas
uint16_t pre_passe_BAV[3] = {725,335,420};
uint16_t passe_BAV[3] = {365,755,310};
uint16_t pre_rangement_BAV[3] = {325,745,510};
//bras Haut Gauche
uint16_t rangement_HAV[3] = {100,100,175};
uint16_t pre_prise_HAV[3] = {660,660,490};
uint16_t prise_HAV[3] = {570,610,710};
uint16_t rangement2_HAV[3] = {100,100,230};
//Positions intermédiaires haut
uint16_t pose_HAV[3] = {520,500,570};
//bras Bas Droite
uint16_t rangement_BAR[3] = {75,950,175};
uint16_t pre_prise_BAR[3] = {210,790,210};
uint16_t prise_BAR[3] = {460,690,340}; //{450,675,345};
uint16_t rangement2_BAR[3] = {75,950,510};
//Positions intermédiaires
uint16_t pre_passe_BAR[3] = {300,690,600};
uint16_t passe_BAR[3] = {660,270,715};
uint16_t pre_rangement_BAR[3] = {700,280,490};
//bras Haut Droite
uint16_t rangement_HAR[3] = {925,925,850};
uint16_t pre_prise_HAR[3] = {365,365,535};
uint16_t prise_HAR[3] = {500,450,300};
uint16_t rangement2_HAR[3] = {925,925,790};
//Positions intermédiaires haut
uint16_t pose_HAR[3] = {500,500,450};
//bras Bas Milieu --> identique AV/AR
uint16_t rangement_BM[3] = {80,90,175};
uint16_t pre_prise_BM[3] = {250,190,330};
uint16_t prise_BM[3] = {290,140,420};
uint16_t rangement2_BM[3] = {80,90,510};
//Positions intermédiaires
uint16_t pre_passe_BM[3] = {290,300,600};
uint16_t passe_BM[3] = {625,690,780};
uint16_t pre_rangement_BM[3] = {675,690,510};
//bras Haut Milieu
uint16_t rangement_HM[3] = {895,900,845};
uint16_t pre_prise_HM[3] = {335,330,490};
uint16_t prise_HM[3] = {425,380,300};
uint16_t rangement2_HM[3] = {895,900,790};
//Positions intermédiaires haut
uint16_t pose_HM[3] = {500,500,570};
char aut_rangement = 0, aut_CN = 0;
char aut_prise_bas = 0, aut_passe = 0, aut_relache_bas = 0, aut_pose_haut = 0;
char aut_1_bras_prise_bas = 0, aut_1_bras_passe = 0, aut_1_bras_relache_bas = 0, aut_1_bras_pose_haut = 0;
char aut_2_bras_prise_bas = 0, aut_2_bras_passe = 0, aut_2_bras_relache_bas = 0, aut_2_bras_pose_haut = 0;
char aut_3_bras_prise_bas = 0, aut_3_bras_passe = 0, aut_3_bras_relache_bas = 0, aut_3_bras_pose_haut = 0;
////////////////////////////////////////////////////////////////Automate pose_haut////////////////////////////////////////////////////////////////
void automate_pose_haut_1(void)
{
typedef enum {init, rangement2, pose, rangement} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement2;
break;
case rangement2:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pose;
break;
case pose:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HAR, SERIAL_GAUCHE);
//verification();
wait_ms(2000);
aut_pose_haut = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
case rangement:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
aut_1_bras_pose_haut=0;
aut_pose_haut = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
default:
break;
}
}
void automate_pose_haut_2(void)
{}
void automate_pose_haut_3(void)
{}
////////////////////////////////////////////////////////////////Automate relache bas////////////////////////////////////////////////////////////////
void automate_relache_bas_1(void)
{
typedef enum {init, rangement2, pre_prise, prise} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement2;
break;
case rangement2:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_POMPE);
aut_1_bras_relache_bas=0;
aut_relache_bas = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
default:
break;
}
}
void automate_relache_bas_2(void)
{
typedef enum {init, rangement2, pre_prise, prise} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement2;
break;
case rangement2:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_POMPE);
aut_2_bras_relache_bas=0;
aut_relache_bas = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
default:
break;
}
}
void automate_relache_bas_3(void)
{
typedef enum {init, rangement2, pre_prise, prise} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement2;
break;
case rangement2:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO_POMPE);
aut_3_bras_relache_bas=0;
aut_relache_bas = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
default:
break;
}
}
////////////////////////////////////////////////////////////////Automate passe////////////////////////////////////////////////////////////////
void automate_passe_1(void)
{
typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=pre_passe;
break;
case pre_passe:
if(bras_choix==0){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==1){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
}
else if(bras_choix==2){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
}
else if(bras_choix==3){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
}
else if(bras_choix==4){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
}
else if(bras_choix==5){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = passe;
break;
case passe:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise_haut;
break;
case pre_prise_haut:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise_haut;
break;
case prise_haut:
if(bras_choix==0){
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==1){
positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
}
else if(bras_choix==2){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
}
else if(bras_choix==3){
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
}
else if(bras_choix==4){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
}
else if(bras_choix==5){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_POMPE);
etat = pre_rangement_bas;
break;
case pre_rangement_bas:
if(bras_choix==0){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==1){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
}
else if(bras_choix==2){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
}
else if(bras_choix==3){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
}
else if(bras_choix==4){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
}
else if(bras_choix==5){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = rangement_haut2;
break;
case rangement_haut2:
if(bras_choix==0){
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==1){
positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
}
else if(bras_choix==2){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
}
else if(bras_choix==3){
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
}
else if(bras_choix==4){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
}
else if(bras_choix==5){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO);
etat = rangement_bas;
break;
case rangement_bas:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO);
aut_1_bras_passe=0;
aut_passe = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
void automate_passe_2(void)
{
typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=pre_passe;
break;
case pre_passe:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = passe;
break;
case passe:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise_haut;
break;
case pre_prise_haut:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise_haut;
break;
case prise_haut:
if(bras_choix==21){
positionControl(HAR_HAUT, 290, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_POMPE);
etat = pre_rangement_bas;
break;
case pre_rangement_bas:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = rangement_haut2;
break;
case rangement_haut2:
if(bras_choix==21){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
wait_ms(200);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = rangement_bas;
break;
case rangement_bas:
if(bras_choix==21){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
}
else if(bras_choix==20){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==10){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==43){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
}
else if(bras_choix==53){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
}
else if(bras_choix==54){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO);
aut_1_bras_passe=0;
aut_passe = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
void automate_passe_3(void)
{
typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=pre_passe;
break;
case pre_passe:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = passe;
break;
case passe:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise_haut;
break;
case pre_prise_haut:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = prise_haut;
break;
case prise_haut:
if(bras_choix==210){
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_POMPE);
etat = pre_rangement_bas;
break;
case pre_rangement_bas:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = rangement_haut2;
break;
case rangement_haut2:
if(bras_choix==210){
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = rangement_bas;
break;
case rangement_bas:
if(bras_choix==210){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
}
else if(bras_choix==66){
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
}
//verification();
wait_ms(TEMPO);
aut_3_bras_passe=0;
aut_passe = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
////////////////////////////////////////////////////////////////Automate prise bas////////////////////////////////////////////////////////////////
void automate_prise_bas_1(void)
{
typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
static type_etat etat = init;
//static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement;
break;
case rangement:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO);
etat = prise;
break;
case prise:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO_POMPE);
etat = rangement2;
break;
case rangement2:
if(bras_choix==0)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
else if(bras_choix==1)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
else if(bras_choix==2)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
else if(bras_choix==3)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
else if(bras_choix==4)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO);
aut_1_bras_prise_bas=0;
aut_prise_bas = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
default:
break;
}
}
void automate_prise_bas_2(void)
{
typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
static type_etat etat = init;
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement;
break;
case rangement:
if(bras_choix == 21) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
} else if(bras_choix == 20) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 10) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
} else if(bras_choix == 43) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
} else if(bras_choix == 53) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
} else if(bras_choix == 54) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix == 21) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
} else if(bras_choix == 20) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 10) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
} else if(bras_choix == 43) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
} else if(bras_choix == 53) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
} else if(bras_choix == 54) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO);
etat = prise;
break;
case prise:
if(bras_choix == 21) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
} else if(bras_choix == 20) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 10) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
} else if(bras_choix == 43) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
} else if(bras_choix == 53) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
} else if(bras_choix == 54) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO_POMPE);
etat = rangement2;
break;
case rangement2:
if(bras_choix == 21) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
} else if(bras_choix == 20) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 10) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
} else if(bras_choix == 43) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
} else if(bras_choix == 53) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
} else if(bras_choix == 54) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
}
//verification();
wait_ms(TEMPO);
aut_2_bras_prise_bas = 0;
aut_prise_bas = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
default:
break;
}
}
void automate_prise_bas_3(void)
{
typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
static type_etat etat = init;
switch(etat) {
case init:
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
etat=rangement;
break;
case rangement:
if(bras_choix == 210) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 66) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
if(bras_choix == 210) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 66) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO);
etat = prise;
break;
case prise:
if(bras_choix == 210) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 66) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO_POMPE);
etat = rangement2;
break;
case rangement2:
if(bras_choix == 210) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
} else if(bras_choix == 66) {
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
}
//verification();
wait_ms(TEMPO);
aut_3_bras_prise_bas = 0;
aut_prise_bas = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
default:
break;
}
}
////////////////////////////////////////////////////////////////Gestion Chasse Neige////////////////////////////////////////////////////////////////
void Gestion_CN(void)
{
SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
switch(ouv_ferm_CN)
{
case 0: //ouverture
positionControl_Mul_ensemble(CN_GAUCHE_AV, 790, PLAYTIME, RLED_ON, CN_DROITE_AV, 240, RLED_ON, SERIAL_SPECIAL);
break;
case 1: //fermeture
positionControl_Mul_ensemble(CN_GAUCHE_AV, 240, PLAYTIME, RLED_ON, CN_DROITE_AV, 790, RLED_ON, SERIAL_SPECIAL);
break;
default:
break;
}
//verification();
wait_ms(TEMPO);
ouv_ferm_CN = 0;
aut_CN = 0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
////////////////////////////////////////////////////////////////Selection prise bas////////////////////////////////////////////////////////////////
void selection_bras_prise_bas(void)
{
if(bras_choix<6)
{
aut_1_bras_prise_bas = 1;
automate_prise_bas_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 20:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 21:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 210:
aut_3_bras_prise_bas = 1;
automate_prise_bas_3();
break;
case 43:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 53:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 54:
aut_2_bras_prise_bas = 1;
automate_prise_bas_2();
break;
case 66:
aut_3_bras_prise_bas = 1;
automate_prise_bas_3();
break;
default:
break;
}
}
}
////////////////////////////////////////////////////////////////Selection passe////////////////////////////////////////////////////////////////
void selection_bras_passe(void)
{
if(bras_choix<6)
{
aut_1_bras_passe = 1;
automate_passe_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 20:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 21:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 210:
aut_3_bras_passe = 1;
automate_passe_3();
break;
case 43:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 53:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 54:
aut_2_bras_passe = 1;
automate_passe_2();
break;
case 66:
aut_3_bras_passe = 1;
automate_passe_3();
break;
default:
break;
}
}
}
////////////////////////////////////////////////////////////////Selection relache bas////////////////////////////////////////////////////////////////
void selection_bras_relache_bas(void)
{
if(bras_choix<6)
{
aut_1_bras_relache_bas = 1;
automate_relache_bas_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 20:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 21:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 210:
aut_3_bras_relache_bas = 1;
automate_relache_bas_3();
break;
case 43:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 53:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 54:
aut_2_bras_relache_bas = 1;
automate_relache_bas_2();
break;
case 66:
aut_3_bras_relache_bas = 1;
automate_relache_bas_3();
break;
default:
break;
}
}
}
////////////////////////////////////////////////////////////////Selection pose haut////////////////////////////////////////////////////////////////
void selection_bras_pose_haut(void)
{
if(bras_choix<6)
{
aut_1_bras_pose_haut = 1;
automate_pose_haut_1();
}
else if(bras_choix>5)
{
switch(bras_choix)
{
case 10:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 20:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 21:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 210:
aut_3_bras_pose_haut = 1;
automate_pose_haut_3();
break;
case 43:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 53:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 54:
aut_2_bras_pose_haut = 1;
automate_pose_haut_2();
break;
case 66:
aut_3_bras_pose_haut = 1;
automate_pose_haut_3();
break;
default:
break;
}
}
}
////////////////////////////////////////////////////////////////Selection rangement////////////////////////////////////////////////////////////////
void selection_bras_rangement(void)
{
switch(bras_choix) {
case 0:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
break;
case 1:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
break;
case 2:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
break;
case 3:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
break;
case 4:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
break;
case 5:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
break;
case 10:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
break;
case 20:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
break;
case 21:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
break;
case 210:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
break;
case 43:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
break;
case 53:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
break;
case 54:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
break;
case 66: //3 4 5
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
break;
default:
break;
}
aut_rangement=0;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
}
/*void fct_bras_Gauche(char serial_select)
{
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl(BAV_HAUT, 295, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
//Passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl(BAV_HAUT, 535, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
wait_ms(TEMPO);
}
void fct_bras_Droite(char serial_select)
{
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl(BAR_HAUT, 730, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
//Passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl(BAR_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
wait_ms(TEMPO);
}
void fct_bras_AV_Milieu(char serial_select)
{
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl(BAV_HAUT, 785, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
//Passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl(BAV_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
wait_ms(TEMPO);
}
void fct_bras_AR_Milieu(char serial_select)
{
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl(BAR_HAUT, 785, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
//Passe bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Prise bras haut
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras haut
positionControl(BAR_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
wait_ms(TEMPO_LONGUE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, serial_select);
wait_ms(TEMPO_LONGUE);
//Rangement bras bas
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
wait_ms(TEMPO);
}
void fct_prise_tous_AV(void)
{
typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
static type_etat etat = rangement;
for(int i=1;i<=4;i++){
switch(etat) {
case rangement:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
//verification();
pc.printf("\n1\n\r");
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
//verification();
pc.printf("\n2\n\r");
wait_ms(TEMPO);
etat = prise;
break;
case prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
//verification();
pc.printf("\n3\n\r");
wait_ms(TEMPO_POMPE);
etat = rangement_2;
break;
case rangement_2:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
//verification();
pc.printf("\n4\n\r");
wait_ms(TEMPO);
etat = rangement;
break;
}
}
}
void fct_prise_Gauche(char serial_select) //servo 1,2,3
{
for(int i=1;i<=4;i++){
typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
static type_etat etat = rangement;
switch(etat) {
case rangement:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, serial_select);
//verification();
wait_ms(TEMPO);
etat = prise;
break;
case prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, serial_select);
//verification();
wait_ms(TEMPO_POMPE);
etat = rangement_2;
break;
case rangement_2:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, serial_select);
//verification();
wait_ms(TEMPO);
etat = rangement;
break;
}
}
}
void fct_prise_Droite(char serial_select) //servo 4,5,6
{
for(int i=1;i<=4;i++){
typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
static type_etat etat = rangement;
switch(etat) {
case rangement:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
//verification();
wait_ms(TEMPO_LONGUE);
etat = pre_prise;
break;
case pre_prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, serial_select);
//verification();
wait_ms(TEMPO);
etat = prise;
break;
case prise:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, serial_select);
//verification();
wait_ms(TEMPO_POMPE);
etat = rangement_2;
break;
case rangement_2:
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, serial_select);
//verification();
wait_ms(TEMPO);
etat = rangement;
break;
}
}
}
void fct_prise_AV_milieu(char serial_select)
{
//Rangement
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, serial_select);
wait_ms(TEMPO);
//Prise
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, serial_select);
wait_ms(TEMPO_POMPE);
//Rangement avec echantillon
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, serial_select);
wait_ms(TEMPO);
}
void fct_prise_AR_milieu(char serial_select)
{
//Rangement
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
wait_ms(TEMPO_LONGUE);
//Pré Prise
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, serial_select);
wait_ms(TEMPO);
//Prise
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, serial_select);
wait_ms(TEMPO_POMPE);
//Rangement avec echantillon
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, serial_select);
wait_ms(TEMPO);
}
////////////////////////////////////////////////CHASSE-NEIGE/////////////////////////////////////////////////////
void ouverture_CN(void)
{
positionControl_Mul_ensemble(CN_GAUCHE, 790, PLAYTIME, RLED_ON, CN_DROITE, 240, RLED_ON, SERIAL_SPECIAL);
}
void fermeture_CN(void)
{
positionControl_Mul_ensemble(CN_GAUCHE, 240, PLAYTIME, RLED_ON, CN_DROITE, 790, RLED_ON, SERIAL_SPECIAL);
}*/