CRAC Team
/
actions_Pr
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019 ACRAC_carte_esclave_GROS_ROBOT_2019
Diff: actions_Pr.cpp
- Revision:
- 34:2d7d09f9965c
- Parent:
- 33:c8d5b8d1ffc0
- Child:
- 35:89c1a096e896
diff -r c8d5b8d1ffc0 -r 2d7d09f9965c actions_Pr.cpp
--- a/actions_Pr.cpp Tue May 03 10:49:44 2022 +0000
+++ b/actions_Pr.cpp Mon May 16 16:47:36 2022 +0000
@@ -2,15 +2,15 @@
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_bras_at = 0, aut_bras_re = 0;
+char aut_bras_1_at = 0, aut_bras_1_re = 0;
+char aut_bras_2_at = 0, aut_bras_2_re = 0;
+char aut_bras_3_at = 0, aut_bras_3_re = 0;
+char aut_bras_prepa = 0, aut_bras_pose = 0;
+char aut_bras_1_prepa = 0, aut_bras_1_pose = 0;
+char aut_bras_2_prepa = 0, aut_bras_2_pose = 0;
+char aut_bras_3_prepa = 0, aut_bras_3_pose = 0;
char aut_manche_haut = 0, aut_manche_bas = 0, aut_manche_moy = 0 ;
-char aut_bras_av_prepa = 0, aut_bras_av_pose = 0;
-char aut_bras_av_1_prepa = 0, aut_bras_av_1_pose = 0;
-char aut_bras_av_2_prepa = 0, aut_bras_av_2_pose = 0;
-char aut_bras_av_3_prepa = 0, aut_bras_av_3_pose = 0;
//moi
@@ -109,49 +109,49 @@
///////////////////////////////////////////////SELECTION ATTRAPER BRAS//////////////////////////////////////////////
void selection_bras_attraper(void)
{
- if(aut_bras_av_at) {
+ if(aut_bras_at) {
if(bras_choix<6) {
- aut_bras_av_1_at = aut_bras_av_at;
+ aut_bras_1_at = aut_bras_at;
automate_bras_attraper_1();
} else if(bras_choix>5) {
switch(bras_choix) {
case 10:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 20:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 21:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 210:
- aut_bras_av_3_at=aut_bras_av_at;
+ aut_bras_3_at=aut_bras_at;
automate_bras_attraper_3();
break;
case 43:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 53:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 54:
- aut_bras_av_2_at=aut_bras_av_at;
+ aut_bras_2_at=aut_bras_at;
automate_bras_attraper_2();
break;
case 66:
- aut_bras_av_3_at=aut_bras_av_at;
+ aut_bras_3_at=aut_bras_at;
automate_bras_attraper_3();
break;
}
@@ -162,49 +162,49 @@
///////////////////////////////////////////////SELECTION RELACHER BRAS//////////////////////////////////////////////
void selection_bras_relacher(void)
{
- if(aut_bras_av_re) {
+ if(aut_bras_re) {
if(bras_choix<6) {
- aut_bras_av_1_re = aut_bras_av_re;
+ aut_bras_1_re = aut_bras_re;
automate_bras_relacher_1();
} else if(bras_choix>5) {
switch(bras_choix) {
case 10:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 20:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 21:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 210:
- aut_bras_av_3_re=aut_bras_av_re;
+ aut_bras_3_re=aut_bras_re;
automate_bras_relacher_3();
break;
case 43:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 53:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 54:
- aut_bras_av_2_re=aut_bras_av_re;
+ aut_bras_2_re=aut_bras_re;
automate_bras_relacher_2();
break;
case 66:
- aut_bras_av_3_re=aut_bras_av_re;
+ aut_bras_3_re=aut_bras_re;
automate_bras_relacher_3();
break;
}
@@ -215,49 +215,49 @@
///////////////////////////////////////////////SELECTION PREPA BRAS//////////////////////////////////////////////
void selection_bras_prepa(void)
{
- if(aut_bras_av_prepa) {
+ if(aut_bras_prepa) {
if(bras_choix<6) {
- aut_bras_av_1_prepa = aut_bras_av_prepa;
+ aut_bras_1_prepa = aut_bras_prepa;
automate_bras_prepa_1();
} else if(bras_choix>5) {
switch(bras_choix) {
case 10:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 20:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 21:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 210:
- aut_bras_av_3_prepa=aut_bras_av_prepa;
+ aut_bras_3_prepa=aut_bras_prepa;
automate_bras_prepa_3();
break;
case 43:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 53:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 54:
- aut_bras_av_2_prepa=aut_bras_av_prepa;
+ aut_bras_2_prepa=aut_bras_prepa;
automate_bras_prepa_2();
break;
case 66:
- aut_bras_av_3_prepa=aut_bras_av_prepa;
+ aut_bras_3_prepa=aut_bras_prepa;
automate_bras_prepa_3();
break;
}
@@ -268,49 +268,49 @@
///////////////////////////////////////////////SELECTION POSE BRAS//////////////////////////////////////////////
void selection_bras_poser(void)
{
- if(aut_bras_av_pose) {
+ if(aut_bras_pose) {
if(bras_choix<6) {
- aut_bras_av_1_pose = aut_bras_av_pose;
+ aut_bras_1_pose = aut_bras_pose;
automate_bras_poser_1();
} else if(bras_choix>5) {
switch(bras_choix) {
case 10:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 20:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 21:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 210:
- aut_bras_av_3_pose=aut_bras_av_pose;
+ aut_bras_3_pose=aut_bras_pose;
automate_bras_poser_3();
break;
case 43:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 53:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 54:
- aut_bras_av_2_pose=aut_bras_av_pose;
+ aut_bras_2_pose=aut_bras_pose;
automate_bras_poser_2();
break;
case 66:
- aut_bras_av_3_pose=aut_bras_av_pose;
+ aut_bras_3_pose=aut_bras_pose;
automate_bras_poser_3();
break;
}
@@ -324,14 +324,13 @@
///////////////////////////////////////////////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) {
+ if(aut_bras_1_at) {
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3) {
@@ -404,8 +403,8 @@
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 ;
+ aut_bras_1_at = 0;
+ aut_bras_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -421,7 +420,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_1_re) {
+ if(aut_bras_1_re) {
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3) {
@@ -484,8 +483,8 @@
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 ;
+ aut_bras_1_re = 0;
+ aut_bras_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -500,7 +499,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_2_at) {
+ if(aut_bras_2_at) {
var_bras_choix_2 = bras_choix;
etat=aut_pos_prepa_attraper_1;
}
@@ -647,8 +646,8 @@
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 ;
+ aut_bras_2_at = 0;
+ aut_bras_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -663,7 +662,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_2_re) {
+ if(aut_bras_2_re) {
var_bras_choix_2 = bras_choix;
etat=aut_pos_prepa_relacher_1;
}
@@ -786,8 +785,8 @@
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 ;
+ aut_bras_2_re = 0;
+ aut_bras_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -802,7 +801,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_3_at) {
+ if(aut_bras_3_at) {
var_bras_choix_3 = bras_choix;
etat=aut_pos_prepa_attraper_1;
}
@@ -904,8 +903,8 @@
//verification();
}
verification();
- aut_bras_av_3_at = 0;
- aut_bras_av_at = 0 ;
+ aut_bras_3_at = 0;
+ aut_bras_at = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -920,7 +919,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_3_re) {
+ if(aut_bras_3_re) {
etat=aut_pos_prepa_relacher_1;
}
var_bras_choix_3 = bras_choix;
@@ -998,8 +997,8 @@
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 ;
+ aut_bras_3_re = 0;
+ aut_bras_re = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -1082,7 +1081,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_1_prepa) {
+ if(aut_bras_1_prepa) {
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3) {
@@ -1116,8 +1115,8 @@
}
verification();
- aut_bras_av_1_prepa = 0;
- aut_bras_av_prepa = 0 ;
+ aut_bras_1_prepa = 0;
+ aut_bras_prepa = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -1132,7 +1131,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_2_prepa) {
+ if(aut_bras_2_prepa) {
var_bras_choix_2 = bras_choix;
etat=aut_pos_prepa_relacher_1;
}
@@ -1183,8 +1182,8 @@
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
}
verification();
- aut_bras_av_2_prepa = 0;
- aut_bras_av_prepa = 0 ;
+ aut_bras_2_prepa = 0;
+ aut_bras_prepa = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -1199,7 +1198,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_3_prepa) {
+ if(aut_bras_3_prepa) {
etat=aut_pos_prepa_relacher_1;
}
var_bras_choix_3 = bras_choix;
@@ -1233,8 +1232,8 @@
verification_3_bras(0);
}
- aut_bras_av_1_prepa = 0;
- aut_bras_av_prepa = 0 ;
+ aut_bras_1_prepa = 0;
+ aut_bras_prepa = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -1252,7 +1251,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_1_pose) {
+ if(aut_bras_1_pose) {
var_bras_choix_1 = bras_choix;
sens_avant=0;
if(var_bras_choix_1<3) {
@@ -1295,8 +1294,8 @@
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, var_bras_choix_1);
}
verification();
- aut_bras_av_1_pose = 0;
- aut_bras_av_pose = 0 ;
+ aut_bras_1_pose = 0;
+ aut_bras_pose = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
@@ -1312,7 +1311,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_2_pose) {
+ if(aut_bras_2_pose) {
var_bras_choix_2 = bras_choix;
etat=aut_pos_relacher;
}
@@ -1387,48 +1386,13 @@
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
}
verification();
- aut_bras_av_2_pose = 0;
- aut_bras_av_pose = 0 ;
+ aut_bras_2_pose = 0;
+ aut_bras_pose = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
-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("noope");
- pc.printf(" %f",sample);
- return 9;
-
-}
////////////////////////////////////////////////3 BRAS/////////////////////////////////////////////////////
void automate_bras_poser_3(void)
@@ -1439,7 +1403,7 @@
switch(etat) {
case init: //attente d'initialisation
- if(aut_bras_av_3_pose) {
+ if(aut_bras_3_pose) {
etat=aut_pos_relacher;
}
var_bras_choix_3 = bras_choix;
@@ -1487,13 +1451,53 @@
positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
verification_3_bras(0);
}
- aut_bras_av_3_pose = 0;
- aut_bras_av_pose = 0 ;
+ aut_bras_3_pose = 0;
+ aut_bras_pose = 0 ;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
etat = init;
break;
}
}
+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---------------------------------------------------------------------------------------------------------------------------
+
+////////////////////////////////////////////////BRAS DE MESURE/////////////////////////////////////////////////////
+
+char bras_choix=0,num_ca=0,msg_carre=0, ouv_ferm_CN=0;
int traitement()
{
@@ -1723,4 +1727,1881 @@
}
return 0;
-}
\ No newline at end of file
+}
+
+////////////////////////////////////////////////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] = {570,350,680};
+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,450,650};
+
+//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] = {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,510};
+//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] = {455,415,290};
+uint16_t rangement2_HAR[3] = {925,925,790};
+//Positions intermédiaires haut
+uint16_t pose_HAR[3] = {500,570,370};
+
+//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] = {305,130,410};
+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,570,370};
+
+
+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(TEMPO_POMPE);
+ etat = rangement;
+ 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_POMPE);
+ 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_POMPE);
+ 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_POMPE);
+ 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_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
+ else if(bras_choix==1)
+ positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
+ else if(bras_choix==2)
+ positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
+ else if(bras_choix==3)
+ positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
+ else if(bras_choix==4)
+ positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
+ else if(bras_choix==5)
+ 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_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_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_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_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_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_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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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_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_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_2_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);
+}*/