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
Revision 35:89c1a096e896, committed 2022-05-18
- Comitter:
- ares1999
- Date:
- Wed May 18 12:06:17 2022 +0000
- Parent:
- 34:2d7d09f9965c
- Commit message:
- o
Changed in this revision
| actions_Pr.cpp | Show annotated file Show diff for this revision Revisions of this file |
| actions_Pr.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 2d7d09f9965c -r 89c1a096e896 actions_Pr.cpp
--- a/actions_Pr.cpp Mon May 16 16:47:36 2022 +0000
+++ b/actions_Pr.cpp Wed May 18 12:06:17 2022 +0000
@@ -1,1463 +1,5 @@
#include "actions_Pr.h"
-Timer timeout;
-//variables//
-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 ;
-
-
-//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;
-
-//
-
-unsigned char var_bras_choix_1 = 0, var_bras_choix_2= 0, var_bras_choix_3= 0, var_bras_manche = 0;
-char vitesse_bras = 35; //50
-
-//define bras gauche et droit
-uint8_t servos_bras_avant[6] = {RLED_ON, 1, GLED_ON, 2, BLED_ON, 3};
-uint8_t servos_bras_arriere[6] = {RLED_ON, 4, GLED_ON, 5, BLED_ON, 6};
-
-
-//position des bras
-uint16_t pos_initial_1[3] = {600,200,200};
-uint16_t pos_prepa_attraper_1[3] = {512,220,200};
-uint16_t pos_revenir_1[3] = {600,200,70};
-uint16_t pos_prepa_relacher_1[3] = {512,220,70};
-
-uint16_t pos_initial_2[3] = {760,130,200};
-uint16_t pos_prepa_relacher_2[3] = {240,512,70};
-uint16_t pos_prepa_attraper_2[3] = {300,630,200}; //220,...,...
-uint16_t pos_revenir_2[3] = {760,150,70};
-
-uint16_t pos_prepa_attraper_3[3] = {320,430,200};
-
-uint16_t pos_attraper[3] = {270,430,70};
-uint16_t pos_relacher[3] = {240,512,200};
-
-void gabarit_robot_droit(void)
-{
- pc.printf("\ngabarit robot avant\n\n");
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 3);
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_avant, pos_initial_2, 2);
- verification();
-}
-
-void gabarit_robot_gauche(void)
-{
- pc.printf("\ngabarit robot arriere\n\n");
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, 50, servos_bras_arriere, pos_initial_2, 3);
- verification();
-}
-
-void gabarit_robot_manche(void)
-{
- positionControl(7,220,80,RLED_ON,4);
- positionControl(8,240,80,RLED_ON,4);
- positionControl(11,780,80,RLED_ON,4); //pour rajouter le drapeau changer le baudrate !!
- verification();
-}
-
-///////////////////////////////////////////////////fonction de test////////////////////////////////////////////////
-void test_BRAS_1(void)
-{
- positionControl(7,410,20, RLED_ON,4);
- verification();
- positionControl(8,410,20, RLED_ON,4);
- //positionControl(11,230,80,0,4);
- verification();
- wait(1);
- positionControl(7,230,20,RLED_ON,4);
- verification();
- positionControl(8,230,20,RLED_ON,4);
- //positionControl(11,780,80,0,4);
- verification();
-}
-
-void test_BRAS_2(void)
-{
- verification();
-}
-
-void test_BRAS_3(void)
-{
- verification();
-}
-
-void test_BRAS_4(void)
-{
- verification();
-}
-
-///////////////////////////////////////////////SELECTION ATTRAPER BRAS//////////////////////////////////////////////
-void selection_bras_attraper(void)
-{
- if(aut_bras_at) {
- if(bras_choix<6) {
- aut_bras_1_at = aut_bras_at;
- automate_bras_attraper_1();
- } else if(bras_choix>5) {
- switch(bras_choix) {
- case 10:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 20:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 21:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 210:
- aut_bras_3_at=aut_bras_at;
- automate_bras_attraper_3();
- break;
-
- case 43:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 53:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 54:
- aut_bras_2_at=aut_bras_at;
- automate_bras_attraper_2();
- break;
-
- case 66:
- aut_bras_3_at=aut_bras_at;
- automate_bras_attraper_3();
- break;
- }
- }
- }
-}
-
-///////////////////////////////////////////////SELECTION RELACHER BRAS//////////////////////////////////////////////
-void selection_bras_relacher(void)
-{
- if(aut_bras_re) {
- if(bras_choix<6) {
- aut_bras_1_re = aut_bras_re;
- automate_bras_relacher_1();
- } else if(bras_choix>5) {
- switch(bras_choix) {
- case 10:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 20:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 21:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 210:
- aut_bras_3_re=aut_bras_re;
- automate_bras_relacher_3();
- break;
-
- case 43:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 53:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 54:
- aut_bras_2_re=aut_bras_re;
- automate_bras_relacher_2();
- break;
-
- case 66:
- aut_bras_3_re=aut_bras_re;
- automate_bras_relacher_3();
- break;
- }
- }
- }
-}
-
-///////////////////////////////////////////////SELECTION PREPA BRAS//////////////////////////////////////////////
-void selection_bras_prepa(void)
-{
- if(aut_bras_prepa) {
- if(bras_choix<6) {
- aut_bras_1_prepa = aut_bras_prepa;
- automate_bras_prepa_1();
- } else if(bras_choix>5) {
- switch(bras_choix) {
- case 10:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 20:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 21:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 210:
- aut_bras_3_prepa=aut_bras_prepa;
- automate_bras_prepa_3();
- break;
-
- case 43:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 53:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 54:
- aut_bras_2_prepa=aut_bras_prepa;
- automate_bras_prepa_2();
- break;
-
- case 66:
- aut_bras_3_prepa=aut_bras_prepa;
- automate_bras_prepa_3();
- break;
- }
- }
- }
-}
-
-///////////////////////////////////////////////SELECTION POSE BRAS//////////////////////////////////////////////
-void selection_bras_poser(void)
-{
- if(aut_bras_pose) {
- if(bras_choix<6) {
- aut_bras_1_pose = aut_bras_pose;
- automate_bras_poser_1();
- } else if(bras_choix>5) {
- switch(bras_choix) {
- case 10:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 20:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 21:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 210:
- aut_bras_3_pose=aut_bras_pose;
- automate_bras_poser_3();
- break;
-
- case 43:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 53:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 54:
- aut_bras_2_pose=aut_bras_pose;
- automate_bras_poser_2();
- break;
-
- case 66:
- aut_bras_3_pose=aut_bras_pose;
- automate_bras_poser_3();
- break;
- }
- }
- }
-}
-
-/**********************************************BRAS ATTRAPER*****************************************/
-
-
-///////////////////////////////////////////////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_1_at) {
- var_bras_choix_1 = bras_choix;
- sens_avant=0;
- if(var_bras_choix_1<3) {
- sens_avant=1;
- if(var_bras_choix_1==0)
- var_bras_choix_1=3;
- else if(var_bras_choix_1==1)
- var_bras_choix_1=2;
- else if(var_bras_choix_1==2)
- var_bras_choix_1=1;
- }
- etat=aut_pos_prepa_attraper_1;
- }
- break;
-
- case aut_pos_prepa_attraper_1://envoi instruction mouvement 1
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_prepa_attraper_2;
- break;
-
- case aut_pos_prepa_attraper_2://envoi instruction mouvement 2
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_prepa_attraper_3;
- break;
-
- case aut_pos_prepa_attraper_3://envoi instruction mouvement 1
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_attraper;
- break;
-
- case aut_pos_attraper://envoi instruction mouvement 1
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_revenir_1;
- break;
-
- case aut_pos_revenir_1://envoi instruction mouvement 1
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_revenir_2;
- break;
-
- case aut_pos_revenir_2://envoi instruction mouvement 1
- if(sens_avant == 0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, var_bras_choix_1-2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, var_bras_choix_1);
- }
- verification();
- aut_bras_1_at = 0;
- aut_bras_at = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
-
- }
-}
-
-void automate_bras_relacher_1(void)
-{
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
- static unsigned char sens_avant=0;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_1_re) {
- var_bras_choix_1 = bras_choix;
- sens_avant=0;
- if(var_bras_choix_1<3) {
- sens_avant=1;
- if(var_bras_choix_1==0)
- var_bras_choix_1=3;
- else if(var_bras_choix_1==1)
- var_bras_choix_1=2;
- else if(var_bras_choix_1==2)
- var_bras_choix_1=1;
- }
- etat=aut_pos_prepa_relacher_1;
- }
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_relacher;
- break;
-
- case aut_pos_relacher://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, var_bras_choix_1);
- }
- verification();
- aut_bras_1_re = 0;
- aut_bras_re = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-//////////////////////////////////////////////////2 bras//////////////////////////////////////////////////////
-void automate_bras_attraper_2(void)
-{
-
- typedef enum {init, aut_pos_prepa_attraper_1, aut_pos_prepa_attraper_2, aut_pos_prepa_attraper_3, aut_pos_attraper, aut_pos_revenir_1, aut_pos_revenir_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_2_at) {
- var_bras_choix_2 = bras_choix;
- etat=aut_pos_prepa_attraper_1;
- }
- break;
-
- case aut_pos_prepa_attraper_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
- }
- verification();
- etat = aut_pos_prepa_attraper_2;
- break;
-
- case aut_pos_prepa_attraper_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
- }
- verification();
- etat = aut_pos_prepa_attraper_3;
- break;
-
- case aut_pos_prepa_attraper_3://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
- }
- verification();
- etat = aut_pos_attraper;
- break;
-
- case aut_pos_attraper://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
- }
- verification();
- etat = aut_pos_revenir_1;
- break;
-
- case aut_pos_revenir_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
- }
- verification();
- etat = aut_pos_revenir_2;
- break;
-
- case aut_pos_revenir_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
- }
- verification();
- aut_bras_2_at = 0;
- aut_bras_at = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-void automate_bras_relacher_2(void)
-{
-
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_2_re) {
- var_bras_choix_2 = bras_choix;
- etat=aut_pos_prepa_relacher_1;
- }
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- }
- verification();
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- }
- verification();
- etat = aut_pos_relacher;
- break;
-
- case aut_pos_relacher://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- }
- verification();
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- }
- verification();
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- }
- verification();
- aut_bras_2_re = 0;
- aut_bras_re = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-////////////////////////////////////////////////3 BRAS/////////////////////////////////////////////////////
-void automate_bras_attraper_3(void)
-{
-
- typedef enum {init, aut_pos_prepa_attraper_1, aut_pos_prepa_attraper_2, aut_pos_prepa_attraper_3, aut_pos_attraper, aut_pos_revenir_1, aut_pos_revenir_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_3_at) {
- var_bras_choix_3 = bras_choix;
- etat=aut_pos_prepa_attraper_1;
- }
- break;
-
- case aut_pos_prepa_attraper_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_1, 2);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_1, 3);
-// verification_3_bras(0);
- //verification();
- }
- verification();
- etat = aut_pos_prepa_attraper_2;
- break;
-
- case aut_pos_prepa_attraper_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_2, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_2, 3);
- verification_3_bras(0);
- //verification();
- }
- etat = aut_pos_prepa_attraper_3;
- break;
-
- case aut_pos_prepa_attraper_3://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_attraper_3, 2);
-// verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_attraper_3, 3);
-// verification_3_bras(0);
- //verification();
- }
- verification();
- etat = aut_pos_attraper;
- break;
-
- case aut_pos_attraper://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_attraper, 2);
-// verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_attraper, 3);
-// verification_3_bras(0);
- //verification();
- }
- verification();
- etat = aut_pos_revenir_1;
- break;
-
- case aut_pos_revenir_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_1, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_1, 3);
- //verification_3_bras(0);
- //verification();
- }
- etat = aut_pos_revenir_2;
- break;
-
- case aut_pos_revenir_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_revenir_2, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_revenir_2, 3);
- //verification_3_bras(0);
- //verification();
- }
- verification();
- aut_bras_3_at = 0;
- aut_bras_at = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-void automate_bras_relacher_3(void)
-{
-
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_3_re) {
- etat=aut_pos_prepa_relacher_1;
- }
- var_bras_choix_3 = bras_choix;
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- verification_3_bras(0);
- }
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- verification_3_bras(0);
- }
- etat = aut_pos_relacher;
- break;
-
- case aut_pos_relacher://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- //verification_3_bras(0);
- }
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- //verification_3_bras(0);
- }
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- verification_3_bras(0);
- }
- aut_bras_3_re = 0;
- aut_bras_re = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-//////////////////////////////////AUTOMATE MANCHE A AIR & Drapeau/////////////////////////////////////
-void automate_manche_air_haut(void)
-{
- if(aut_manche_haut) { // 0 = Manche à aire droite 1 = manche à aire gauche
- var_bras_manche = bras_choix ;
- if(var_bras_manche == 1) {
- positionControl(7,220,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
-
- else if (var_bras_manche == 0) {
- positionControl(8,240,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
- aut_manche_haut = 0 ;
- }
-}
-
-void automate_manche_air_bas(void)
-{
- if(aut_manche_bas) { // 0 = manche à aire droite 1 = manche à aire gauche
- var_bras_manche = bras_choix ;
- if(var_bras_manche == 1) {
- positionControl(7,512,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
-
- else if (var_bras_manche == 0) {
- positionControl(8,512,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
- aut_manche_bas = 0 ;
- }
-}
-
-void automate_manche_air_moy(void)
-{
- if(aut_manche_moy) { // 0 = manche à aire droite 1 = manche à aire gauche
- var_bras_manche = bras_choix ;
- if(var_bras_manche == 1) {
- positionControl(7,460,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
-
- else if (var_bras_manche == 0) {
- positionControl(8,460,30,RLED_ON,4);
- verification();
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- }
- aut_manche_moy = 0 ;
- }
-}
-
-void pavilon_deploye(void)
-{
- positionControl(11,230,80,RLED_ON,4);
- verification() ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-}
-
-/**********************************************BRAS PREPA*****************************************/
-
-///////////////////////////////////////////////1 BRAS//////////////////////////////////////////////
-void automate_bras_prepa_1(void)
-{
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2} type_etat;
- static type_etat etat = init;
- static unsigned char sens_avant=0;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_1_prepa) {
- var_bras_choix_1 = bras_choix;
- sens_avant=0;
- if(var_bras_choix_1<3) {
- sens_avant=1;
- if(var_bras_choix_1==0)
- var_bras_choix_1=3;
- else if(var_bras_choix_1==1)
- var_bras_choix_1=2;
- else if(var_bras_choix_1==2)
- var_bras_choix_1=1;
- }
- etat=aut_pos_prepa_relacher_1;
- }
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, var_bras_choix_1);
- }
- verification();
-
- aut_bras_1_prepa = 0;
- aut_bras_prepa = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-//////////////////////////////////////////////////2 bras//////////////////////////////////////////////////////
-void automate_bras_prepa_2(void)
-{
-
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_2_prepa) {
- var_bras_choix_2 = bras_choix;
- etat=aut_pos_prepa_relacher_1;
- }
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- }
- verification();
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- }
- verification();
- aut_bras_2_prepa = 0;
- aut_bras_prepa = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-////////////////////////////////////////////////3 BRAS/////////////////////////////////////////////////////
-void automate_bras_prepa_3(void)
-{
-
- typedef enum {init, aut_pos_prepa_relacher_1, aut_pos_prepa_relacher_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_3_prepa) {
- etat=aut_pos_prepa_relacher_1;
- }
- var_bras_choix_3 = bras_choix;
- break;
-
- case aut_pos_prepa_relacher_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_1, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_1, 3);
- verification_3_bras(0);
- }
- etat = aut_pos_prepa_relacher_2;
- break;
-
- case aut_pos_prepa_relacher_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_prepa_relacher_2, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_prepa_relacher_2, 3);
- verification_3_bras(0);
- }
-
- aut_bras_1_prepa = 0;
- aut_bras_prepa = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-/**********************************************BRAS POSER*****************************************/
-
-///////////////////////////////////////////////1 BRAS//////////////////////////////////////////////
-void automate_bras_poser_1(void)
-{
- typedef enum {init, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
- static unsigned char sens_avant=0;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_1_pose) {
- var_bras_choix_1 = bras_choix;
- sens_avant=0;
- if(var_bras_choix_1<3) {
- sens_avant=1;
- if(var_bras_choix_1==0)
- var_bras_choix_1=3;
- else if(var_bras_choix_1==1)
- var_bras_choix_1=2;
- else if(var_bras_choix_1==2)
- var_bras_choix_1=1;
- }
- etat=aut_pos_relacher;
- }
- break;
-
- case aut_pos_relacher://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, var_bras_choix_1);
- }
- verification();
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(sens_avant==0) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, var_bras_choix_1 - 2);
- } else {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, var_bras_choix_1);
- }
- verification();
- aut_bras_1_pose = 0;
- aut_bras_pose = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-//////////////////////////////////////////////////2 bras//////////////////////////////////////////////////////
-void automate_bras_poser_2(void)
-{
-
- typedef enum {init, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_2_pose) {
- var_bras_choix_2 = bras_choix;
- etat=aut_pos_relacher;
- }
- break;
-
- case aut_pos_relacher://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- }
- verification();
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- }
- verification();
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(var_bras_choix_2 == 21) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- } else if(var_bras_choix_2 == 20) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 10) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 43) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- } else if(var_bras_choix_2 == 53) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- } else if(var_bras_choix_2 == 54) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- }
- verification();
- aut_bras_2_pose = 0;
- aut_bras_pose = 0 ;
- SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
- etat = init;
- break;
- }
-}
-
-////////////////////////////////////////////////3 BRAS/////////////////////////////////////////////////////
-void automate_bras_poser_3(void)
-{
-
- typedef enum {init, aut_pos_relacher, aut_pos_initial_1, aut_pos_initial_2} type_etat;
- static type_etat etat = init;
-
- switch(etat) {
- case init: //attente d'initialisation
- if(aut_bras_3_pose) {
- etat=aut_pos_relacher;
- }
- var_bras_choix_3 = bras_choix;
- break;
-
- case aut_pos_relacher://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_relacher, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_relacher, 3);
- //verification_3_bras(0);
- }
- etat = aut_pos_initial_1;
- break;
-
- case aut_pos_initial_1://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_1, 2);
- //verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_1, 3);
- //verification_3_bras(0);
- }
- etat = aut_pos_initial_2;
- break;
-
- case aut_pos_initial_2://envoi instruction
- if(var_bras_choix_3 == 210) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 3);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_avant, pos_initial_2, 2);
- verification_3_bras(1);
- } else if(var_bras_choix_3 == 66) {
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 1);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 2);
- positionControl_Mul_ensemble_complex(3, vitesse_bras, servos_bras_arriere, pos_initial_2, 3);
- verification_3_bras(0);
- }
- aut_bras_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;
@@ -1494,7 +36,15 @@
}
//--------------------------------------------------------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;
@@ -1523,8 +73,8 @@
case ranger:
pc.printf("RANGER ");
- positionControl(ID_MILLIEU-choix_color*2, 800, PLAYTIME, RLED_ON, 1);
- positionControl(ID_HAUT-choix_color*2, 512, PLAYTIME, RLED_ON, 1);
+ 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;
@@ -1541,8 +91,8 @@
case mesure:
if(tab[num_ca]==9) {
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 1);
- positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, 1);
+ 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);
@@ -1566,7 +116,7 @@
break;
} else {
correction=correction+5;
- positionControl(ID_MILLIEU-choix_color*2, 602-correction, 40, GLED_ON, 1); // on positionne la base 565
+ positionControl(ID_MILLIEU-choix_color*2, 602-correction, 40, GLED_ON, SERIAL_SPECIAL); // on positionne la base 565
}
}
@@ -1593,13 +143,13 @@
case bascule:
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 5);
- positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, 5);
- positionControl(ID_HAUT-choix_color*2, 119, 20, GLED_ON, 5);
- positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, 5);
- positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, 5);
- positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, 5);
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 5);
+ 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
@@ -1607,8 +157,8 @@
break;
case pretest:
- positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, 1); // on positionne la base 770
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 1);
+ 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;
@@ -1623,25 +173,25 @@
}
void BF_test_mesure(bool BJ)
{
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 1);
- positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, 1);
+ 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, 1);
- positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, 1);
- positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, 1);
- positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, 1);
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 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, 1);
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 1);
+ 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, 1);
- positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, 1);
+ 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);
@@ -1741,7 +291,7 @@
//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 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};
@@ -1753,29 +303,29 @@
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};
+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] = {450,675,345};
+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,510};
+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] = {455,415,290};
+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,570,370};
+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] = {305,130,410};
+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};
@@ -1787,7 +337,7 @@
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};
+uint16_t pose_HM[3] = {500,500,570};
char aut_rangement = 0, aut_CN = 0;
@@ -1841,8 +391,10 @@
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HAR, SERIAL_GAUCHE);
//verification();
- wait_ms(TEMPO_POMPE);
- etat = rangement;
+ wait_ms(2000);
+ aut_pose_haut = 0;
+ etat = init;
+ SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
break;
case rangement:
if(bras_choix==0)
@@ -1917,7 +469,7 @@
else if(bras_choix==5)
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
//verification();
- wait_ms(TEMPO_POMPE);
+ wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
@@ -1946,7 +498,7 @@
}
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
@@ -1957,70 +509,88 @@
etat=rangement2;
break;
case rangement2:
- if(bras_choix==21)
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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);
+ wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
- if(bras_choix==21)
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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;
@@ -2030,10 +600,10 @@
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
@@ -2044,40 +614,46 @@
etat=rangement2;
break;
case rangement2:
- if(bras_choix==210)
+ 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)
+ }
+ 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)
+ 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)
+ }
+ 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);
+ wait_ms(TEMPO_LONGUE);
etat = prise;
break;
case prise:
- if(bras_choix==210)
+ 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)
+ }
+ 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;
@@ -2087,7 +663,7 @@
break;
default:
break;
- }*/
+ }
}
////////////////////////////////////////////////////////////////Automate passe////////////////////////////////////////////////////////////////
@@ -2160,18 +736,36 @@
etat = prise_haut;
break;
case prise_haut:
- if(bras_choix==0)
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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)
+ }
+ 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;
@@ -2357,26 +951,44 @@
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);
}
@@ -2417,42 +1029,42 @@
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);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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(TEMPO);
+ 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);
}
@@ -2553,11 +1165,19 @@
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);
@@ -2703,7 +1323,7 @@
positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
//verification();
wait_ms(TEMPO);
- aut_2_bras_prise_bas=0;
+ aut_1_bras_prise_bas=0;
aut_prise_bas = 0;
etat = init;
SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
diff -r 2d7d09f9965c -r 89c1a096e896 actions_Pr.h --- a/actions_Pr.h Mon May 16 16:47:36 2022 +0000 +++ b/actions_Pr.h Wed May 18 12:06:17 2022 +0000 @@ -6,60 +6,6 @@ #define Enemy_color 2 #define Mycolor 1 -extern char aut_bras_at, aut_bras_re; -extern char aut_bras_1_at, aut_bras_1_re; -extern char aut_bras_2_at, aut_bras_2_re; -extern char aut_bras_3_at, aut_bras_3_re; -extern char aut_manche_haut, aut_manche_bas, aut_manche_moy ; -extern char aut_bras_prepa, aut_bras_pose; -extern char aut_bras_1_prepa, aut_bras_1_pose; -extern char aut_bras_2_prepa, aut_bras_2_pose; -extern char aut_bras_3_prepa, aut_bras_3_pose; - -//////////////////////////à garder////////////////////////// -void gabarit_robot_droit(void); -void gabarit_robot_gauche(void); -void gabarit_robot_manche(void); -void pavilon_deploye(void); - -//////////////////////////test////////////////////////////// -void test_BRAS_A(void); -void test_BRAS_B(void); -void test_BRAS_C(void); -void test_BRAS_D(void); - -void test_BRAS_1(void); -void test_BRAS_2(void); -void test_BRAS_3(void); -void test_BRAS_4(void); -void test_BRAS_5(void); -void test_BRAS_6(void); - -//////////////////////////selection///////////////////////// -void selection_bras_attraper(void); -void selection_bras_relacher(void); - -void selection_bras_prepa(void); -void selection_bras_poser(void); - -//////////////////////////automates///////////////////////// -void automate_bras_attraper_1(void); -void automate_bras_relacher_1(void); -void automate_bras_attraper_2(void); -void automate_bras_relacher_2(void); -void automate_bras_attraper_3(void); -void automate_bras_relacher_3(void); -void automate_manche_air_haut(void) ; -void automate_manche_air_bas(void) ; -void automate_manche_air_moy(void) ; -void automate_bras_prepa_1(void); -void automate_bras_poser_1(void); -void automate_bras_prepa_2(void); -void automate_bras_poser_2(void); -void automate_bras_prepa_3(void); -void automate_bras_poser_3(void); - - //////////////////////////BRAS DE MESURE////////////////////////////// extern char bras_choix,num_ca,msg_carre,ouv_ferm_CN;