CRAC Team / actions_Pr

librairie actions petit robot carte esclave

Dependents:   carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019 ACRAC_carte_esclave_GROS_ROBOT_2019

actions_Pr.cpp@35:89c1a096e896, 23 months ago (annotated)

Committer:
ares1999
Date:
Wed May 18 12:06:17 2022 +0000
Revision:
35:89c1a096e896
Parent:
34:2d7d09f9965c 
o

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:42544b5e8b9f 1 #include "actions_Pr.h"
Artiom 0:42544b5e8b9f 2
ares1999 34:2d7d09f9965c 3 int mesure_resistor(float vpot,bool color) // definition de la fontion qui mesure les resistances
ares1999 34:2d7d09f9965c 4 {
ares1999 34:2d7d09f9965c 5 float sample = vpot;
ares1999 34:2d7d09f9965c 6 if ((sample > 0.8f) && (sample < 0.87f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance bleu
ares1999 34:2d7d09f9965c 7 if (color == 1) {
ares1999 34:2d7d09f9965c 8 pc.printf("test non ok \n");
ares1999 34:2d7d09f9965c 9 return Enemy_color;
ares1999 34:2d7d09f9965c 10
ares1999 34:2d7d09f9965c 11 } else {
ares1999 34:2d7d09f9965c 12 pc.printf("test non ok1 \n");
ares1999 34:2d7d09f9965c 13 return Mycolor;
ares1999 34:2d7d09f9965c 14
ares1999 34:2d7d09f9965c 15
ares1999 34:2d7d09f9965c 16 }
ares1999 34:2d7d09f9965c 17 } else if ((sample > 0.37f) && (sample < 0.42f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance rouge
ares1999 34:2d7d09f9965c 18 pc.printf("test ok0 \n");
ares1999 34:2d7d09f9965c 19 return rouge;
ares1999 34:2d7d09f9965c 20
ares1999 34:2d7d09f9965c 21 } else if ((sample > 0.70f) && (sample < 0.78f)) { // marge d'ereur +-5% de ce que qu'on mesure de la resistance jaune
ares1999 34:2d7d09f9965c 22 if (color == 1) {
ares1999 34:2d7d09f9965c 23 pc.printf("test non ok2 \n");
ares1999 34:2d7d09f9965c 24 return Mycolor;
ares1999 34:2d7d09f9965c 25
ares1999 34:2d7d09f9965c 26 } else {
ares1999 34:2d7d09f9965c 27 pc.printf("test non ok3 \n");
ares1999 34:2d7d09f9965c 28 return Enemy_color;
ares1999 34:2d7d09f9965c 29
ares1999 34:2d7d09f9965c 30 }
ares1999 34:2d7d09f9965c 31
ares1999 34:2d7d09f9965c 32 }
ares1999 34:2d7d09f9965c 33 pc.printf("nope");
ares1999 34:2d7d09f9965c 34 pc.printf(" %f",sample);
ares1999 34:2d7d09f9965c 35 return 9;
ares1999 34:2d7d09f9965c 36
ares1999 34:2d7d09f9965c 37 }
ares1999 34:2d7d09f9965c 38 //--------------------------------------------------------2022---------------------------------------------------------------------------------------------------------------------------
ares1999 35:89c1a096e896 39 //moi
ares1999 35:89c1a096e896 40 int tab[7]= {9,Mycolor,9,9,9,9,9};
ares1999 35:89c1a096e896 41 bool choix_color=0;
ares1999 35:89c1a096e896 42 AnalogIn releve(PA_5);
ares1999 35:89c1a096e896 43 AnalogIn releve1(PA_7);
ares1999 35:89c1a096e896 44 bool delay=true;
ares1999 35:89c1a096e896 45 int resultat;
ares1999 35:89c1a096e896 46 uint8_t correction=0;
ares1999 35:89c1a096e896 47 char msg_num_ca=0;
ares1999 34:2d7d09f9965c 48 ////////////////////////////////////////////////BRAS DE MESURE/////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 49
ares1999 34:2d7d09f9965c 50 char bras_choix=0,num_ca=0,msg_carre=0, ouv_ferm_CN=0;
gein 31:dd39b63a012a 51
gein 31:dd39b63a012a 52 int traitement()
gein 31:dd39b63a012a 53 {
gein 31:dd39b63a012a 54 typedef enum {init,ranger,mesure, bascule, pretest} type_etat;
gein 31:dd39b63a012a 55 static type_etat etat = init;
gein 31:dd39b63a012a 56 switch(etat) {
gein 31:dd39b63a012a 57 case init:
ekey29 32:56e06a1894ac 58
gein 31:dd39b63a012a 59 if(msg_carre==1) {
gein 31:dd39b63a012a 60 etat=ranger;
gein 31:dd39b63a012a 61 msg_carre=0;
gein 31:dd39b63a012a 62 } else if(msg_carre==2) {
gein 31:dd39b63a012a 63 etat=mesure;
gein 31:dd39b63a012a 64 msg_carre=0;
gein 31:dd39b63a012a 65 delay=true;
gein 31:dd39b63a012a 66 } else if(msg_carre==3) {
gein 31:dd39b63a012a 67
gein 31:dd39b63a012a 68 etat=pretest;
gein 31:dd39b63a012a 69 msg_carre=0;
gein 31:dd39b63a012a 70 }
gein 31:dd39b63a012a 71
gein 31:dd39b63a012a 72 break;
gein 31:dd39b63a012a 73
gein 31:dd39b63a012a 74 case ranger:
gein 31:dd39b63a012a 75 pc.printf("RANGER ");
ares1999 35:89c1a096e896 76 positionControl(ID_MILLIEU-choix_color*2, 800, PLAYTIME, RLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 77 positionControl(ID_HAUT-choix_color*2, 512, PLAYTIME, RLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 78 if(msg_carre==2) {
gein 31:dd39b63a012a 79 etat=mesure;
gein 31:dd39b63a012a 80 msg_carre=0;
gein 31:dd39b63a012a 81 delay=true;
gein 31:dd39b63a012a 82 } else if(msg_carre==3) {
gein 31:dd39b63a012a 83
gein 31:dd39b63a012a 84 etat=pretest;
gein 31:dd39b63a012a 85 msg_carre=0;
gein 31:dd39b63a012a 86 }
ekey29 33:c8d5b8d1ffc0 87 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
gein 31:dd39b63a012a 88
gein 31:dd39b63a012a 89 break;
gein 31:dd39b63a012a 90
gein 31:dd39b63a012a 91
gein 31:dd39b63a012a 92 case mesure:
gein 31:dd39b63a012a 93 if(tab[num_ca]==9) {
ares1999 35:89c1a096e896 94 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 95 positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 96 while(delay ) {
ekey29 33:c8d5b8d1ffc0 97 if(choix_color==0){
ekey29 33:c8d5b8d1ffc0 98 resultat=mesure_resistor(releve1,choix_color);
ekey29 33:c8d5b8d1ffc0 99 }
ekey29 33:c8d5b8d1ffc0 100 else{
ekey29 33:c8d5b8d1ffc0 101 resultat=mesure_resistor(releve,choix_color);
ekey29 33:c8d5b8d1ffc0 102 }
gein 31:dd39b63a012a 103 if(resultat==0) {
gein 31:dd39b63a012a 104 delay=false;
gein 31:dd39b63a012a 105 }
gein 31:dd39b63a012a 106 if(resultat==1) {
gein 31:dd39b63a012a 107 delay=false;
gein 31:dd39b63a012a 108 }
gein 31:dd39b63a012a 109 if(resultat==2) {
gein 31:dd39b63a012a 110 delay=false;
gein 31:dd39b63a012a 111 }
gein 31:dd39b63a012a 112 if(correction==30) {
gein 31:dd39b63a012a 113 pc.printf("eror");
gein 31:dd39b63a012a 114 correction=0;
gein 31:dd39b63a012a 115 etat=pretest;
gein 31:dd39b63a012a 116 break;
gein 31:dd39b63a012a 117 } else {
gein 31:dd39b63a012a 118 correction=correction+5;
ares1999 35:89c1a096e896 119 positionControl(ID_MILLIEU-choix_color*2, 602-correction, 40, GLED_ON, SERIAL_SPECIAL); // on positionne la base 565
ekey29 33:c8d5b8d1ffc0 120
gein 31:dd39b63a012a 121 }
gein 31:dd39b63a012a 122 }
gein 31:dd39b63a012a 123
gein 31:dd39b63a012a 124 tab[num_ca]=resultat;
gein 31:dd39b63a012a 125 algo_carre (num_ca);
gein 31:dd39b63a012a 126
gein 31:dd39b63a012a 127
gein 31:dd39b63a012a 128
gein 31:dd39b63a012a 129 }
gein 31:dd39b63a012a 130 if(tab[num_ca]==rouge) {
gein 31:dd39b63a012a 131 etat=pretest;
gein 31:dd39b63a012a 132 SendCharCan(0x35,tab[num_ca]);
gein 31:dd39b63a012a 133 } else if(tab[num_ca]==Mycolor) {
gein 31:dd39b63a012a 134 etat=bascule;
gein 31:dd39b63a012a 135 SendCharCan(0x35,tab[num_ca]);
gein 31:dd39b63a012a 136 } else if(tab[num_ca]==Enemy_color) {
gein 31:dd39b63a012a 137 etat=pretest;
gein 31:dd39b63a012a 138 SendCharCan(0x35,tab[num_ca]);
gein 31:dd39b63a012a 139 }
ekey29 33:c8d5b8d1ffc0 140 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
gein 31:dd39b63a012a 141 break;
gein 31:dd39b63a012a 142
gein 31:dd39b63a012a 143
gein 31:dd39b63a012a 144 case bascule:
gein 31:dd39b63a012a 145
ares1999 35:89c1a096e896 146 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 147 positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 148 positionControl(ID_HAUT-choix_color*2, 119, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 149 positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 150 positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 151 positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 152 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 153
gein 31:dd39b63a012a 154
gein 31:dd39b63a012a 155 etat=pretest;//peut etre a changer
gein 31:dd39b63a012a 156
gein 31:dd39b63a012a 157 break;
gein 31:dd39b63a012a 158
gein 31:dd39b63a012a 159 case pretest:
ares1999 35:89c1a096e896 160 positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL); // on positionne la base 770
ares1999 35:89c1a096e896 161 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
ekey29 33:c8d5b8d1ffc0 162 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ekey29 33:c8d5b8d1ffc0 163 etat=init;
gein 31:dd39b63a012a 164
gein 31:dd39b63a012a 165 break;
gein 31:dd39b63a012a 166 }
gein 31:dd39b63a012a 167 return 0;
gein 31:dd39b63a012a 168 }
gein 31:dd39b63a012a 169 void set_color(bool color)
gein 31:dd39b63a012a 170 {
gein 31:dd39b63a012a 171 choix_color=color;
ekey29 33:c8d5b8d1ffc0 172 pc.printf("ident haut : %X \n",ID_HAUT - choix_color*2);
gein 31:dd39b63a012a 173 }
gein 31:dd39b63a012a 174 void BF_test_mesure(bool BJ)
gein 31:dd39b63a012a 175 {
ares1999 35:89c1a096e896 176 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 177 positionControl(ID_MILLIEU-choix_color*2, 602, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 178 if(mesure_resistor(releve,BJ)==1) {
ares1999 35:89c1a096e896 179 positionControl(ID_HAUT-choix_color*2, 119, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 180 positionControl(ID_HAUT-choix_color*2, 612, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 181 positionControl(ID_MILLIEU-choix_color*2, 450, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 182 positionControl(ID_MILLIEU-choix_color*2, 700, 20, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 183 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 184 SendCharCan(0x35,0x01);
gein 31:dd39b63a012a 185
gein 31:dd39b63a012a 186 } else if(mesure_resistor(releve,BJ)==0) {
ares1999 35:89c1a096e896 187 positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 188 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 189 SendCharCan(0x35,0x00);
gein 31:dd39b63a012a 190
gein 31:dd39b63a012a 191
gein 31:dd39b63a012a 192 } else if(mesure_resistor(releve,BJ)==2) {
ares1999 35:89c1a096e896 193 positionControl(ID_MILLIEU-choix_color*2, 700, 40, GLED_ON, SERIAL_SPECIAL);
ares1999 35:89c1a096e896 194 positionControl(ID_HAUT - choix_color*2, 214, 40, GLED_ON, SERIAL_SPECIAL);
gein 31:dd39b63a012a 195 SendCharCan(0x35,0x02);
gein 31:dd39b63a012a 196
gein 31:dd39b63a012a 197
gein 31:dd39b63a012a 198 }
gein 31:dd39b63a012a 199
gein 31:dd39b63a012a 200
gein 31:dd39b63a012a 201
gein 31:dd39b63a012a 202 }
gein 31:dd39b63a012a 203
gein 31:dd39b63a012a 204 int algo_carre (int nombre)
gein 31:dd39b63a012a 205 {
gein 31:dd39b63a012a 206 switch (nombre) {
gein 31:dd39b63a012a 207 case 0:
gein 31:dd39b63a012a 208 if(resultat==Mycolor) {
gein 31:dd39b63a012a 209
gein 31:dd39b63a012a 210 tab[2]=rouge;
gein 31:dd39b63a012a 211 } else {
gein 31:dd39b63a012a 212 tab[2]=Mycolor;
gein 31:dd39b63a012a 213 }
gein 31:dd39b63a012a 214 break;
gein 31:dd39b63a012a 215
gein 31:dd39b63a012a 216 case 2:
gein 31:dd39b63a012a 217
gein 31:dd39b63a012a 218 if(resultat==Mycolor) {
gein 31:dd39b63a012a 219
gein 31:dd39b63a012a 220 tab[0]=rouge;
gein 31:dd39b63a012a 221 } else {
gein 31:dd39b63a012a 222 tab[1]=Mycolor;
gein 31:dd39b63a012a 223 }
gein 31:dd39b63a012a 224 break;
gein 31:dd39b63a012a 225 case 3:
gein 31:dd39b63a012a 226
gein 31:dd39b63a012a 227 if(resultat==Mycolor) {
gein 31:dd39b63a012a 228 tab[4]=Enemy_color;
gein 31:dd39b63a012a 229 tab[5]=Enemy_color;
gein 31:dd39b63a012a 230 tab[6]=Mycolor;
gein 31:dd39b63a012a 231 } else {
gein 31:dd39b63a012a 232 tab[4]=Mycolor;
gein 31:dd39b63a012a 233 tab[5]=Mycolor;
gein 31:dd39b63a012a 234 tab[6]=Enemy_color;
gein 31:dd39b63a012a 235 }
gein 31:dd39b63a012a 236 break;
gein 31:dd39b63a012a 237 case 4:
gein 31:dd39b63a012a 238
gein 31:dd39b63a012a 239 if(resultat==Mycolor) {
gein 31:dd39b63a012a 240 tab[3]=Enemy_color;
gein 31:dd39b63a012a 241 tab[5]=Mycolor;
gein 31:dd39b63a012a 242 tab[6]=Enemy_color;
gein 31:dd39b63a012a 243 } else {
gein 31:dd39b63a012a 244 tab[3]=Mycolor;
gein 31:dd39b63a012a 245 tab[5]=Enemy_color;
gein 31:dd39b63a012a 246 tab[6]=Mycolor;
gein 31:dd39b63a012a 247 }
gein 31:dd39b63a012a 248 break;
gein 31:dd39b63a012a 249 case 5:
gein 31:dd39b63a012a 250
gein 31:dd39b63a012a 251 if(resultat==Mycolor) {
gein 31:dd39b63a012a 252 tab[3]=Enemy_color;
gein 31:dd39b63a012a 253 tab[4]=Mycolor;
gein 31:dd39b63a012a 254 tab[6]=Enemy_color;
gein 31:dd39b63a012a 255 } else {
gein 31:dd39b63a012a 256
gein 31:dd39b63a012a 257 tab[3]=Mycolor;
gein 31:dd39b63a012a 258 tab[4]=Enemy_color;
gein 31:dd39b63a012a 259 tab[6]=Mycolor;
gein 31:dd39b63a012a 260 }
gein 31:dd39b63a012a 261
gein 31:dd39b63a012a 262 break;
gein 31:dd39b63a012a 263 case 6:
gein 31:dd39b63a012a 264
gein 31:dd39b63a012a 265 if(resultat==Mycolor) {
gein 31:dd39b63a012a 266
gein 31:dd39b63a012a 267 tab[3]=Mycolor;
gein 31:dd39b63a012a 268 tab[4]=Enemy_color;
gein 31:dd39b63a012a 269 tab[5]=Enemy_color;
gein 31:dd39b63a012a 270 } else {
gein 31:dd39b63a012a 271
gein 31:dd39b63a012a 272 tab[3]=Enemy_color;
gein 31:dd39b63a012a 273 tab[4]=Mycolor;
gein 31:dd39b63a012a 274 tab[5]=Mycolor;
gein 31:dd39b63a012a 275 }
gein 31:dd39b63a012a 276 break;
gein 31:dd39b63a012a 277 }
gein 31:dd39b63a012a 278
gein 31:dd39b63a012a 279 return 0;
ares1999 34:2d7d09f9965c 280 }
ares1999 34:2d7d09f9965c 281
ares1999 34:2d7d09f9965c 282 ////////////////////////////////////////////////BRAS 2022/////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 283
ares1999 34:2d7d09f9965c 284 //Variables "data" fonction positionControl_Mul_ensemble_complex
ares1999 34:2d7d09f9965c 285 uint8_t servos_BAV[6] = {RLED_ON, BAV_BASE, GLED_ON, BAV_MILIEU, BLED_ON, BAV_HAUT};
ares1999 34:2d7d09f9965c 286 uint8_t servos_BAR[6] = {RLED_ON, BAR_BASE, GLED_ON, BAR_MILIEU, BLED_ON, BAR_HAUT};
ares1999 34:2d7d09f9965c 287 uint8_t servos_HAV[6] = {RLED_ON, HAV_BASE, GLED_ON, HAV_MILIEU, BLED_ON, HAV_HAUT};
ares1999 34:2d7d09f9965c 288 uint8_t servos_HAR[6] = {RLED_ON, HAR_BASE, GLED_ON, HAR_MILIEU, BLED_ON, HAR_HAUT};
ares1999 34:2d7d09f9965c 289
ares1999 34:2d7d09f9965c 290 //Variables position des bras
ares1999 34:2d7d09f9965c 291 //bras Bas Gauche
ares1999 34:2d7d09f9965c 292 uint16_t rangement_BAV[3] = {950,75,850};
ares1999 34:2d7d09f9965c 293 uint16_t pre_prise_BAV[3] = {790,210,790};
ares1999 35:89c1a096e896 294 uint16_t prise_BAV[3] = {560,330,660};
ares1999 34:2d7d09f9965c 295 uint16_t rangement2_BAV[3] = {950,75,510};
ares1999 34:2d7d09f9965c 296 //Positions intermédiaires bas
ares1999 34:2d7d09f9965c 297 uint16_t pre_passe_BAV[3] = {725,335,420};
ares1999 34:2d7d09f9965c 298 uint16_t passe_BAV[3] = {365,755,310};
ares1999 34:2d7d09f9965c 299 uint16_t pre_rangement_BAV[3] = {325,745,510};
ares1999 34:2d7d09f9965c 300 //bras Haut Gauche
ares1999 34:2d7d09f9965c 301 uint16_t rangement_HAV[3] = {100,100,175};
ares1999 34:2d7d09f9965c 302 uint16_t pre_prise_HAV[3] = {660,660,490};
ares1999 34:2d7d09f9965c 303 uint16_t prise_HAV[3] = {570,610,710};
ares1999 34:2d7d09f9965c 304 uint16_t rangement2_HAV[3] = {100,100,230};
ares1999 34:2d7d09f9965c 305 //Positions intermédiaires haut
ares1999 35:89c1a096e896 306 uint16_t pose_HAV[3] = {520,500,570};
ares1999 34:2d7d09f9965c 307
ares1999 34:2d7d09f9965c 308 //bras Bas Droite
ares1999 34:2d7d09f9965c 309 uint16_t rangement_BAR[3] = {75,950,175};
ares1999 34:2d7d09f9965c 310 uint16_t pre_prise_BAR[3] = {210,790,210};
ares1999 35:89c1a096e896 311 uint16_t prise_BAR[3] = {460,690,340}; //{450,675,345};
ares1999 34:2d7d09f9965c 312 uint16_t rangement2_BAR[3] = {75,950,510};
ares1999 34:2d7d09f9965c 313 //Positions intermédiaires
ares1999 34:2d7d09f9965c 314 uint16_t pre_passe_BAR[3] = {300,690,600};
ares1999 34:2d7d09f9965c 315 uint16_t passe_BAR[3] = {660,270,715};
ares1999 35:89c1a096e896 316 uint16_t pre_rangement_BAR[3] = {700,280,490};
ares1999 34:2d7d09f9965c 317 //bras Haut Droite
ares1999 34:2d7d09f9965c 318 uint16_t rangement_HAR[3] = {925,925,850};
ares1999 34:2d7d09f9965c 319 uint16_t pre_prise_HAR[3] = {365,365,535};
ares1999 35:89c1a096e896 320 uint16_t prise_HAR[3] = {500,450,300};
ares1999 34:2d7d09f9965c 321 uint16_t rangement2_HAR[3] = {925,925,790};
ares1999 34:2d7d09f9965c 322 //Positions intermédiaires haut
ares1999 35:89c1a096e896 323 uint16_t pose_HAR[3] = {500,500,450};
ares1999 34:2d7d09f9965c 324
ares1999 34:2d7d09f9965c 325 //bras Bas Milieu --> identique AV/AR
ares1999 34:2d7d09f9965c 326 uint16_t rangement_BM[3] = {80,90,175};
ares1999 34:2d7d09f9965c 327 uint16_t pre_prise_BM[3] = {250,190,330};
ares1999 35:89c1a096e896 328 uint16_t prise_BM[3] = {290,140,420};
ares1999 34:2d7d09f9965c 329 uint16_t rangement2_BM[3] = {80,90,510};
ares1999 34:2d7d09f9965c 330 //Positions intermédiaires
ares1999 34:2d7d09f9965c 331 uint16_t pre_passe_BM[3] = {290,300,600};
ares1999 34:2d7d09f9965c 332 uint16_t passe_BM[3] = {625,690,780};
ares1999 34:2d7d09f9965c 333 uint16_t pre_rangement_BM[3] = {675,690,510};
ares1999 34:2d7d09f9965c 334 //bras Haut Milieu
ares1999 34:2d7d09f9965c 335 uint16_t rangement_HM[3] = {895,900,845};
ares1999 34:2d7d09f9965c 336 uint16_t pre_prise_HM[3] = {335,330,490};
ares1999 34:2d7d09f9965c 337 uint16_t prise_HM[3] = {425,380,300};
ares1999 34:2d7d09f9965c 338 uint16_t rangement2_HM[3] = {895,900,790};
ares1999 34:2d7d09f9965c 339 //Positions intermédiaires haut
ares1999 35:89c1a096e896 340 uint16_t pose_HM[3] = {500,500,570};
ares1999 34:2d7d09f9965c 341
ares1999 34:2d7d09f9965c 342
ares1999 34:2d7d09f9965c 343 char aut_rangement = 0, aut_CN = 0;
ares1999 34:2d7d09f9965c 344 char aut_prise_bas = 0, aut_passe = 0, aut_relache_bas = 0, aut_pose_haut = 0;
ares1999 34:2d7d09f9965c 345 char aut_1_bras_prise_bas = 0, aut_1_bras_passe = 0, aut_1_bras_relache_bas = 0, aut_1_bras_pose_haut = 0;
ares1999 34:2d7d09f9965c 346 char aut_2_bras_prise_bas = 0, aut_2_bras_passe = 0, aut_2_bras_relache_bas = 0, aut_2_bras_pose_haut = 0;
ares1999 34:2d7d09f9965c 347 char aut_3_bras_prise_bas = 0, aut_3_bras_passe = 0, aut_3_bras_relache_bas = 0, aut_3_bras_pose_haut = 0;
ares1999 34:2d7d09f9965c 348
ares1999 34:2d7d09f9965c 349
ares1999 34:2d7d09f9965c 350
ares1999 34:2d7d09f9965c 351 ////////////////////////////////////////////////////////////////Automate pose_haut////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 352 void automate_pose_haut_1(void)
ares1999 34:2d7d09f9965c 353 {
ares1999 34:2d7d09f9965c 354 typedef enum {init, rangement2, pose, rangement} type_etat;
ares1999 34:2d7d09f9965c 355 static type_etat etat = init;
ares1999 34:2d7d09f9965c 356 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 357
ares1999 34:2d7d09f9965c 358 switch(etat) {
ares1999 34:2d7d09f9965c 359 case init:
ares1999 34:2d7d09f9965c 360 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 361 etat=rangement2;
ares1999 34:2d7d09f9965c 362 break;
ares1999 34:2d7d09f9965c 363 case rangement2:
ares1999 34:2d7d09f9965c 364 if(bras_choix==0)
ares1999 34:2d7d09f9965c 365 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 366 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 367 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 368 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 369 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 370 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 371 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 372 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 373 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 374 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 375 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 376 //verification();
ares1999 34:2d7d09f9965c 377 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 378 etat = pose;
ares1999 34:2d7d09f9965c 379 break;
ares1999 34:2d7d09f9965c 380 case pose:
ares1999 34:2d7d09f9965c 381 if(bras_choix==0)
ares1999 34:2d7d09f9965c 382 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 383 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 384 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 385 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 386 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 387 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 388 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pose_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 389 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 390 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 391 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 392 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pose_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 393 //verification();
ares1999 35:89c1a096e896 394 wait_ms(2000);
ares1999 35:89c1a096e896 395 aut_pose_haut = 0;
ares1999 35:89c1a096e896 396 etat = init;
ares1999 35:89c1a096e896 397 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 398 break;
ares1999 34:2d7d09f9965c 399 case rangement:
ares1999 34:2d7d09f9965c 400 if(bras_choix==0)
ares1999 34:2d7d09f9965c 401 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 402 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 403 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 404 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 405 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 406 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 407 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 408 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 409 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 410 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 411 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 412 //verification();
ares1999 34:2d7d09f9965c 413 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 414 aut_1_bras_pose_haut=0;
ares1999 34:2d7d09f9965c 415 aut_pose_haut = 0;
ares1999 34:2d7d09f9965c 416 etat = init;
ares1999 34:2d7d09f9965c 417 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 418 break;
ares1999 34:2d7d09f9965c 419 default:
ares1999 34:2d7d09f9965c 420 break;
ares1999 34:2d7d09f9965c 421 }
ares1999 34:2d7d09f9965c 422 }
ares1999 34:2d7d09f9965c 423
ares1999 34:2d7d09f9965c 424 void automate_pose_haut_2(void)
ares1999 34:2d7d09f9965c 425 {}
ares1999 34:2d7d09f9965c 426 void automate_pose_haut_3(void)
ares1999 34:2d7d09f9965c 427 {}
ares1999 34:2d7d09f9965c 428
ares1999 34:2d7d09f9965c 429 ////////////////////////////////////////////////////////////////Automate relache bas////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 430 void automate_relache_bas_1(void)
ares1999 34:2d7d09f9965c 431 {
ares1999 34:2d7d09f9965c 432 typedef enum {init, rangement2, pre_prise, prise} type_etat;
ares1999 34:2d7d09f9965c 433 static type_etat etat = init;
ares1999 34:2d7d09f9965c 434 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 435
ares1999 34:2d7d09f9965c 436 switch(etat) {
ares1999 34:2d7d09f9965c 437 case init:
ares1999 34:2d7d09f9965c 438 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 439 etat=rangement2;
ares1999 34:2d7d09f9965c 440 break;
ares1999 34:2d7d09f9965c 441 case rangement2:
ares1999 34:2d7d09f9965c 442 if(bras_choix==0)
ares1999 34:2d7d09f9965c 443 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 444 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 445 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 446 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 447 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 448 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 449 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 450 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 451 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 452 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 453 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 454 //verification();
ares1999 34:2d7d09f9965c 455 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 456 etat = pre_prise;
ares1999 34:2d7d09f9965c 457 break;
ares1999 34:2d7d09f9965c 458 case pre_prise:
ares1999 34:2d7d09f9965c 459 if(bras_choix==0)
ares1999 34:2d7d09f9965c 460 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 461 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 462 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 463 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 464 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 465 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 466 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 467 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 468 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 469 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 470 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 471 //verification();
ares1999 35:89c1a096e896 472 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 473 etat = prise;
ares1999 34:2d7d09f9965c 474 break;
ares1999 34:2d7d09f9965c 475 case prise:
ares1999 34:2d7d09f9965c 476 if(bras_choix==0)
ares1999 34:2d7d09f9965c 477 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 478 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 479 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 480 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 481 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 482 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 483 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 484 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 485 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 486 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 487 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 488 //verification();
ares1999 34:2d7d09f9965c 489 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 490 aut_1_bras_relache_bas=0;
ares1999 34:2d7d09f9965c 491 aut_relache_bas = 0;
ares1999 34:2d7d09f9965c 492 etat = init;
ares1999 34:2d7d09f9965c 493 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 494 break;
ares1999 34:2d7d09f9965c 495 default:
ares1999 34:2d7d09f9965c 496 break;
ares1999 34:2d7d09f9965c 497 }
ares1999 34:2d7d09f9965c 498 }
ares1999 34:2d7d09f9965c 499
ares1999 34:2d7d09f9965c 500 void automate_relache_bas_2(void)
ares1999 35:89c1a096e896 501 {
ares1999 34:2d7d09f9965c 502 typedef enum {init, rangement2, pre_prise, prise} type_etat;
ares1999 34:2d7d09f9965c 503 static type_etat etat = init;
ares1999 34:2d7d09f9965c 504 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 505
ares1999 34:2d7d09f9965c 506 switch(etat) {
ares1999 34:2d7d09f9965c 507 case init:
ares1999 34:2d7d09f9965c 508 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 509 etat=rangement2;
ares1999 34:2d7d09f9965c 510 break;
ares1999 34:2d7d09f9965c 511 case rangement2:
ares1999 35:89c1a096e896 512 if(bras_choix==21){
ares1999 34:2d7d09f9965c 513 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 514 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 515 }
ares1999 35:89c1a096e896 516 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 517 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 518 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 519 }
ares1999 35:89c1a096e896 520 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 521 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 522 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 523 }
ares1999 35:89c1a096e896 524 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 525 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 526 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 527 }
ares1999 35:89c1a096e896 528 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 529 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 530 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 531 }
ares1999 35:89c1a096e896 532 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 533 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 534 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 535 }
ares1999 34:2d7d09f9965c 536 //verification();
ares1999 34:2d7d09f9965c 537 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 538 etat = pre_prise;
ares1999 34:2d7d09f9965c 539 break;
ares1999 34:2d7d09f9965c 540 case pre_prise:
ares1999 35:89c1a096e896 541 if(bras_choix==21){
ares1999 34:2d7d09f9965c 542 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 543 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 544 }
ares1999 35:89c1a096e896 545 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 546 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 547 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 548 }
ares1999 35:89c1a096e896 549 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 550 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 551 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 552 }
ares1999 35:89c1a096e896 553 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 554 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 555 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 556 }
ares1999 35:89c1a096e896 557 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 558 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 559 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 560 }
ares1999 35:89c1a096e896 561 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 562 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 563 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 564 }
ares1999 34:2d7d09f9965c 565 //verification();
ares1999 35:89c1a096e896 566 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 567 etat = prise;
ares1999 34:2d7d09f9965c 568 break;
ares1999 34:2d7d09f9965c 569 case prise:
ares1999 35:89c1a096e896 570 if(bras_choix==21){
ares1999 34:2d7d09f9965c 571 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 572 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 573 }
ares1999 35:89c1a096e896 574 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 575 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 576 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 577 }
ares1999 35:89c1a096e896 578 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 579 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 580 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 581 }
ares1999 35:89c1a096e896 582 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 583 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 584 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 585 }
ares1999 35:89c1a096e896 586 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 587 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 588 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 589 }
ares1999 35:89c1a096e896 590 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 591 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 592 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 593 }
ares1999 34:2d7d09f9965c 594 //verification();
ares1999 34:2d7d09f9965c 595 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 596 aut_2_bras_relache_bas=0;
ares1999 34:2d7d09f9965c 597 aut_relache_bas = 0;
ares1999 34:2d7d09f9965c 598 etat = init;
ares1999 34:2d7d09f9965c 599 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 600 break;
ares1999 34:2d7d09f9965c 601 default:
ares1999 34:2d7d09f9965c 602 break;
ares1999 35:89c1a096e896 603 }
ares1999 34:2d7d09f9965c 604 }
ares1999 34:2d7d09f9965c 605 void automate_relache_bas_3(void)
ares1999 35:89c1a096e896 606 {
ares1999 34:2d7d09f9965c 607 typedef enum {init, rangement2, pre_prise, prise} type_etat;
ares1999 34:2d7d09f9965c 608 static type_etat etat = init;
ares1999 34:2d7d09f9965c 609 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 610
ares1999 34:2d7d09f9965c 611 switch(etat) {
ares1999 34:2d7d09f9965c 612 case init:
ares1999 34:2d7d09f9965c 613 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 614 etat=rangement2;
ares1999 34:2d7d09f9965c 615 break;
ares1999 34:2d7d09f9965c 616 case rangement2:
ares1999 35:89c1a096e896 617 if(bras_choix==210){
ares1999 34:2d7d09f9965c 618 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 619 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 620 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 621 }
ares1999 35:89c1a096e896 622 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 623 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 624 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 625 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 626 }
ares1999 34:2d7d09f9965c 627 //verification();
ares1999 34:2d7d09f9965c 628 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 629 etat = pre_prise;
ares1999 34:2d7d09f9965c 630 break;
ares1999 34:2d7d09f9965c 631 case pre_prise:
ares1999 35:89c1a096e896 632 if(bras_choix==210){
ares1999 34:2d7d09f9965c 633 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 634 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 635 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 636 }
ares1999 35:89c1a096e896 637 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 638 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 639 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 640 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 641 }
ares1999 34:2d7d09f9965c 642 //verification();
ares1999 35:89c1a096e896 643 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 644 etat = prise;
ares1999 34:2d7d09f9965c 645 break;
ares1999 34:2d7d09f9965c 646 case prise:
ares1999 35:89c1a096e896 647 if(bras_choix==210){
ares1999 34:2d7d09f9965c 648 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 649 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 650 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 651 }
ares1999 35:89c1a096e896 652 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 653 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 654 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 655 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 656 }
ares1999 34:2d7d09f9965c 657 //verification();
ares1999 34:2d7d09f9965c 658 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 659 aut_3_bras_relache_bas=0;
ares1999 34:2d7d09f9965c 660 aut_relache_bas = 0;
ares1999 34:2d7d09f9965c 661 etat = init;
ares1999 34:2d7d09f9965c 662 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 663 break;
ares1999 34:2d7d09f9965c 664 default:
ares1999 34:2d7d09f9965c 665 break;
ares1999 35:89c1a096e896 666 }
ares1999 34:2d7d09f9965c 667 }
ares1999 34:2d7d09f9965c 668
ares1999 34:2d7d09f9965c 669 ////////////////////////////////////////////////////////////////Automate passe////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 670 void automate_passe_1(void)
ares1999 34:2d7d09f9965c 671 {
ares1999 34:2d7d09f9965c 672 typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
ares1999 34:2d7d09f9965c 673 static type_etat etat = init;
ares1999 34:2d7d09f9965c 674 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 675
ares1999 34:2d7d09f9965c 676 switch(etat) {
ares1999 34:2d7d09f9965c 677 case init:
ares1999 34:2d7d09f9965c 678 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 679 etat=pre_passe;
ares1999 34:2d7d09f9965c 680 break;
ares1999 34:2d7d09f9965c 681 case pre_passe:
ares1999 34:2d7d09f9965c 682 if(bras_choix==0){
ares1999 34:2d7d09f9965c 683 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 684 }
ares1999 34:2d7d09f9965c 685 else if(bras_choix==1){
ares1999 34:2d7d09f9965c 686 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 687 }
ares1999 34:2d7d09f9965c 688 else if(bras_choix==2){
ares1999 34:2d7d09f9965c 689 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 690 }
ares1999 34:2d7d09f9965c 691 else if(bras_choix==3){
ares1999 34:2d7d09f9965c 692 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 693 }
ares1999 34:2d7d09f9965c 694 else if(bras_choix==4){
ares1999 34:2d7d09f9965c 695 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 696 }
ares1999 34:2d7d09f9965c 697 else if(bras_choix==5){
ares1999 34:2d7d09f9965c 698 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 699 }
ares1999 34:2d7d09f9965c 700 //verification();
ares1999 34:2d7d09f9965c 701 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 702 etat = passe;
ares1999 34:2d7d09f9965c 703 break;
ares1999 34:2d7d09f9965c 704 case passe:
ares1999 34:2d7d09f9965c 705 if(bras_choix==0)
ares1999 34:2d7d09f9965c 706 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 707 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 708 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 709 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 710 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 711 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 712 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 713 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 714 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 715 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 716 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 717 //verification();
ares1999 34:2d7d09f9965c 718 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 719 etat = pre_prise_haut;
ares1999 34:2d7d09f9965c 720 break;
ares1999 34:2d7d09f9965c 721 case pre_prise_haut:
ares1999 34:2d7d09f9965c 722 if(bras_choix==0)
ares1999 34:2d7d09f9965c 723 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 724 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 725 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 726 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 727 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 728 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 729 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 730 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 731 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 732 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 733 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 734 //verification();
ares1999 34:2d7d09f9965c 735 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 736 etat = prise_haut;
ares1999 34:2d7d09f9965c 737 break;
ares1999 34:2d7d09f9965c 738 case prise_haut:
ares1999 35:89c1a096e896 739 if(bras_choix==0){
ares1999 35:89c1a096e896 740 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 741 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 742 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 743 }
ares1999 35:89c1a096e896 744 else if(bras_choix==1){
ares1999 35:89c1a096e896 745 positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 746 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 747 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 748 }
ares1999 35:89c1a096e896 749 else if(bras_choix==2){
ares1999 35:89c1a096e896 750 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 751 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 752 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
ares1999 35:89c1a096e896 753 }
ares1999 35:89c1a096e896 754 else if(bras_choix==3){
ares1999 35:89c1a096e896 755 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 756 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 757 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
ares1999 35:89c1a096e896 758 }
ares1999 35:89c1a096e896 759 else if(bras_choix==4){
ares1999 35:89c1a096e896 760 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 761 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 762 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
ares1999 35:89c1a096e896 763 }
ares1999 35:89c1a096e896 764 else if(bras_choix==5){
ares1999 35:89c1a096e896 765 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 766 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 767 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 768 }
ares1999 34:2d7d09f9965c 769 //verification();
ares1999 34:2d7d09f9965c 770 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 771 etat = pre_rangement_bas;
ares1999 34:2d7d09f9965c 772 break;
ares1999 34:2d7d09f9965c 773 case pre_rangement_bas:
ares1999 34:2d7d09f9965c 774 if(bras_choix==0){
ares1999 34:2d7d09f9965c 775 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 776 }
ares1999 34:2d7d09f9965c 777 else if(bras_choix==1){
ares1999 34:2d7d09f9965c 778 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 779 }
ares1999 34:2d7d09f9965c 780 else if(bras_choix==2){
ares1999 34:2d7d09f9965c 781 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 782 }
ares1999 34:2d7d09f9965c 783 else if(bras_choix==3){
ares1999 34:2d7d09f9965c 784 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 785 }
ares1999 34:2d7d09f9965c 786 else if(bras_choix==4){
ares1999 34:2d7d09f9965c 787 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 788 }
ares1999 34:2d7d09f9965c 789 else if(bras_choix==5){
ares1999 34:2d7d09f9965c 790 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 791 }
ares1999 34:2d7d09f9965c 792 //verification();
ares1999 34:2d7d09f9965c 793 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 794 etat = rangement_haut2;
ares1999 34:2d7d09f9965c 795 break;
ares1999 34:2d7d09f9965c 796 case rangement_haut2:
ares1999 34:2d7d09f9965c 797 if(bras_choix==0){
ares1999 34:2d7d09f9965c 798 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 799 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 800 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 801 }
ares1999 34:2d7d09f9965c 802 else if(bras_choix==1){
ares1999 34:2d7d09f9965c 803 positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 804 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 805 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 806 }
ares1999 34:2d7d09f9965c 807 else if(bras_choix==2){
ares1999 34:2d7d09f9965c 808 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 809 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 810 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 811 }
ares1999 34:2d7d09f9965c 812 else if(bras_choix==3){
ares1999 34:2d7d09f9965c 813 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 814 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 815 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 816 }
ares1999 34:2d7d09f9965c 817 else if(bras_choix==4){
ares1999 34:2d7d09f9965c 818 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 819 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 820 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 821 }
ares1999 34:2d7d09f9965c 822 else if(bras_choix==5){
ares1999 34:2d7d09f9965c 823 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 824 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 825 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 826 }
ares1999 34:2d7d09f9965c 827 //verification();
ares1999 34:2d7d09f9965c 828 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 829 etat = rangement_bas;
ares1999 34:2d7d09f9965c 830 break;
ares1999 34:2d7d09f9965c 831 case rangement_bas:
ares1999 34:2d7d09f9965c 832 if(bras_choix==0)
ares1999 34:2d7d09f9965c 833 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 834 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 835 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 836 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 837 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 838 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 839 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 840 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 841 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 842 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 843 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 844 //verification();
ares1999 34:2d7d09f9965c 845 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 846 aut_1_bras_passe=0;
ares1999 34:2d7d09f9965c 847 aut_passe = 0;
ares1999 34:2d7d09f9965c 848 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 849 etat = init;
ares1999 34:2d7d09f9965c 850 break;
ares1999 34:2d7d09f9965c 851 }
ares1999 34:2d7d09f9965c 852 }
ares1999 34:2d7d09f9965c 853
ares1999 34:2d7d09f9965c 854 void automate_passe_2(void)
ares1999 34:2d7d09f9965c 855 {
ares1999 34:2d7d09f9965c 856 typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
ares1999 34:2d7d09f9965c 857 static type_etat etat = init;
ares1999 34:2d7d09f9965c 858 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 859
ares1999 34:2d7d09f9965c 860 switch(etat) {
ares1999 34:2d7d09f9965c 861 case init:
ares1999 34:2d7d09f9965c 862 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 863 etat=pre_passe;
ares1999 34:2d7d09f9965c 864 break;
ares1999 34:2d7d09f9965c 865 case pre_passe:
ares1999 34:2d7d09f9965c 866 if(bras_choix==21){
ares1999 34:2d7d09f9965c 867 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 868 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 869 }
ares1999 34:2d7d09f9965c 870 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 871 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 872 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 873 }
ares1999 34:2d7d09f9965c 874 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 875 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 876 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 877 }
ares1999 34:2d7d09f9965c 878 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 879 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 880 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 881 }
ares1999 34:2d7d09f9965c 882 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 883 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 884 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 885 }
ares1999 34:2d7d09f9965c 886 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 887 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 888 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 889 }
ares1999 34:2d7d09f9965c 890 //verification();
ares1999 34:2d7d09f9965c 891 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 892 etat = passe;
ares1999 34:2d7d09f9965c 893 break;
ares1999 34:2d7d09f9965c 894 case passe:
ares1999 34:2d7d09f9965c 895 if(bras_choix==21){
ares1999 34:2d7d09f9965c 896 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 897 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 898 }
ares1999 34:2d7d09f9965c 899 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 900 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 901 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 902 }
ares1999 34:2d7d09f9965c 903 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 904 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 905 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 906 }
ares1999 34:2d7d09f9965c 907 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 908 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 909 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 910 }
ares1999 34:2d7d09f9965c 911 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 912 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 913 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 914 }
ares1999 34:2d7d09f9965c 915 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 916 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 917 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 918 }
ares1999 34:2d7d09f9965c 919 //verification();
ares1999 34:2d7d09f9965c 920 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 921 etat = pre_prise_haut;
ares1999 34:2d7d09f9965c 922 break;
ares1999 34:2d7d09f9965c 923 case pre_prise_haut:
ares1999 34:2d7d09f9965c 924 if(bras_choix==21){
ares1999 34:2d7d09f9965c 925 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 926 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 927 }
ares1999 34:2d7d09f9965c 928 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 929 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 930 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 931 }
ares1999 34:2d7d09f9965c 932 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 933 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 934 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 935 }
ares1999 34:2d7d09f9965c 936 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 937 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 938 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 939 }
ares1999 34:2d7d09f9965c 940 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 941 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 942 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 943 }
ares1999 34:2d7d09f9965c 944 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 945 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 946 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 947 }
ares1999 34:2d7d09f9965c 948 //verification();
ares1999 34:2d7d09f9965c 949 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 950 etat = prise_haut;
ares1999 34:2d7d09f9965c 951 break;
ares1999 34:2d7d09f9965c 952 case prise_haut:
ares1999 34:2d7d09f9965c 953 if(bras_choix==21){
ares1999 35:89c1a096e896 954 positionControl(HAR_HAUT, 290, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 955 positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 956 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 957 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 958 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 959 }
ares1999 34:2d7d09f9965c 960 else if(bras_choix==20){
ares1999 35:89c1a096e896 961 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 962 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 963 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 964 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 965 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 966 }
ares1999 34:2d7d09f9965c 967 else if(bras_choix==10){
ares1999 35:89c1a096e896 968 positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 969 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 970 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 971 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 972 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 973 }
ares1999 34:2d7d09f9965c 974 else if(bras_choix==43){
ares1999 35:89c1a096e896 975 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 976 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 977 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 978 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 979 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 980 }
ares1999 34:2d7d09f9965c 981 else if(bras_choix==53){
ares1999 35:89c1a096e896 982 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 983 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 984 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 985 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 986 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 987 }
ares1999 34:2d7d09f9965c 988 else if(bras_choix==54){
ares1999 35:89c1a096e896 989 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 990 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 991 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 992 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 993 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 994 }
ares1999 34:2d7d09f9965c 995 //verification();
ares1999 34:2d7d09f9965c 996 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 997 etat = pre_rangement_bas;
ares1999 34:2d7d09f9965c 998 break;
ares1999 34:2d7d09f9965c 999 case pre_rangement_bas:
ares1999 34:2d7d09f9965c 1000 if(bras_choix==21){
ares1999 34:2d7d09f9965c 1001 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1002 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1003 }
ares1999 34:2d7d09f9965c 1004 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 1005 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1006 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1007 }
ares1999 34:2d7d09f9965c 1008 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 1009 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1010 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1011 }
ares1999 34:2d7d09f9965c 1012 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 1013 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1014 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1015 }
ares1999 34:2d7d09f9965c 1016 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 1017 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1018 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1019 }
ares1999 34:2d7d09f9965c 1020 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 1021 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1022 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1023 }
ares1999 34:2d7d09f9965c 1024 //verification();
ares1999 34:2d7d09f9965c 1025 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1026 etat = rangement_haut2;
ares1999 34:2d7d09f9965c 1027 break;
ares1999 34:2d7d09f9965c 1028 case rangement_haut2:
ares1999 34:2d7d09f9965c 1029 if(bras_choix==21){
ares1999 34:2d7d09f9965c 1030 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1031 positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 1032 wait_ms(200);
ares1999 34:2d7d09f9965c 1033 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1034 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1035 }
ares1999 34:2d7d09f9965c 1036 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 1037 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1038 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 1039 wait_ms(200);
ares1999 34:2d7d09f9965c 1040 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1041 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1042 }
ares1999 34:2d7d09f9965c 1043 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 1044 positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1045 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 1046 wait_ms(200);
ares1999 34:2d7d09f9965c 1047 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1048 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1049 }
ares1999 34:2d7d09f9965c 1050 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 1051 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1052 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 1053 wait_ms(200);
ares1999 34:2d7d09f9965c 1054 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1055 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1056 }
ares1999 34:2d7d09f9965c 1057 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 1058 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1059 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 1060 wait_ms(200);
ares1999 34:2d7d09f9965c 1061 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1062 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1063 }
ares1999 34:2d7d09f9965c 1064 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 1065 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1066 positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 1067 wait_ms(200);
ares1999 34:2d7d09f9965c 1068 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1069 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1070 }
ares1999 34:2d7d09f9965c 1071 //verification();
ares1999 34:2d7d09f9965c 1072 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1073 etat = rangement_bas;
ares1999 34:2d7d09f9965c 1074 break;
ares1999 34:2d7d09f9965c 1075 case rangement_bas:
ares1999 34:2d7d09f9965c 1076 if(bras_choix==21){
ares1999 34:2d7d09f9965c 1077 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1078 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1079 }
ares1999 34:2d7d09f9965c 1080 else if(bras_choix==20){
ares1999 34:2d7d09f9965c 1081 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1082 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1083 }
ares1999 34:2d7d09f9965c 1084 else if(bras_choix==10){
ares1999 34:2d7d09f9965c 1085 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1086 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1087 }
ares1999 34:2d7d09f9965c 1088 else if(bras_choix==43){
ares1999 34:2d7d09f9965c 1089 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1090 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1091 }
ares1999 34:2d7d09f9965c 1092 else if(bras_choix==53){
ares1999 34:2d7d09f9965c 1093 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1094 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1095 }
ares1999 34:2d7d09f9965c 1096 else if(bras_choix==54){
ares1999 34:2d7d09f9965c 1097 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1098 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1099 }
ares1999 34:2d7d09f9965c 1100 //verification();
ares1999 34:2d7d09f9965c 1101 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1102 aut_1_bras_passe=0;
ares1999 34:2d7d09f9965c 1103 aut_passe = 0;
ares1999 34:2d7d09f9965c 1104 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1105 etat = init;
ares1999 34:2d7d09f9965c 1106 break;
ares1999 34:2d7d09f9965c 1107 }
ares1999 34:2d7d09f9965c 1108 }
ares1999 34:2d7d09f9965c 1109
ares1999 34:2d7d09f9965c 1110 void automate_passe_3(void)
ares1999 34:2d7d09f9965c 1111 {
ares1999 34:2d7d09f9965c 1112 typedef enum {init, pre_passe, passe, pre_prise_haut, prise_haut, pre_rangement_bas, rangement_haut2, rangement_bas} type_etat;
ares1999 34:2d7d09f9965c 1113 static type_etat etat = init;
ares1999 34:2d7d09f9965c 1114 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 1115
ares1999 34:2d7d09f9965c 1116 switch(etat) {
ares1999 34:2d7d09f9965c 1117 case init:
ares1999 34:2d7d09f9965c 1118 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 1119 etat=pre_passe;
ares1999 34:2d7d09f9965c 1120 break;
ares1999 34:2d7d09f9965c 1121 case pre_passe:
ares1999 34:2d7d09f9965c 1122 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1123 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1124 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1125 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1126 }
ares1999 34:2d7d09f9965c 1127 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1128 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1129 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1130 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1131 }
ares1999 34:2d7d09f9965c 1132 //verification();
ares1999 34:2d7d09f9965c 1133 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1134 etat = passe;
ares1999 34:2d7d09f9965c 1135 break;
ares1999 34:2d7d09f9965c 1136 case passe:
ares1999 34:2d7d09f9965c 1137 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1138 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1139 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1140 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1141 }
ares1999 34:2d7d09f9965c 1142 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1143 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1144 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1145 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1146 }
ares1999 34:2d7d09f9965c 1147 //verification();
ares1999 34:2d7d09f9965c 1148 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1149 etat = pre_prise_haut;
ares1999 34:2d7d09f9965c 1150 break;
ares1999 34:2d7d09f9965c 1151 case pre_prise_haut:
ares1999 34:2d7d09f9965c 1152 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1153 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1154 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1155 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1156 }
ares1999 34:2d7d09f9965c 1157 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1158 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1159 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1160 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1161 }
ares1999 34:2d7d09f9965c 1162 //verification();
ares1999 34:2d7d09f9965c 1163 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1164 etat = prise_haut;
ares1999 34:2d7d09f9965c 1165 break;
ares1999 34:2d7d09f9965c 1166 case prise_haut:
ares1999 34:2d7d09f9965c 1167 if(bras_choix==210){
ares1999 35:89c1a096e896 1168 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 1169 positionControl(HAV_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 1170 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 1171 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1172 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1173 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1174 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1175 }
ares1999 34:2d7d09f9965c 1176 else if(bras_choix==66){
ares1999 35:89c1a096e896 1177 positionControl(HAV_HAUT, 710, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 35:89c1a096e896 1178 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 35:89c1a096e896 1179 positionControl(HAR_HAUT, 300, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 35:89c1a096e896 1180 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1181 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1182 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1183 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1184 }
ares1999 34:2d7d09f9965c 1185 //verification();
ares1999 34:2d7d09f9965c 1186 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 1187 etat = pre_rangement_bas;
ares1999 34:2d7d09f9965c 1188 break;
ares1999 34:2d7d09f9965c 1189 case pre_rangement_bas:
ares1999 34:2d7d09f9965c 1190 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1191 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1192 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1193 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1194 }
ares1999 34:2d7d09f9965c 1195 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1196 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1197 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1198 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1199 }
ares1999 34:2d7d09f9965c 1200 //verification();
ares1999 34:2d7d09f9965c 1201 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1202 etat = rangement_haut2;
ares1999 34:2d7d09f9965c 1203 break;
ares1999 34:2d7d09f9965c 1204 case rangement_haut2:
ares1999 34:2d7d09f9965c 1205 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1206 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1207 positionControl(HAV_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1208 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1209 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1210 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1211 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1212 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1213 }
ares1999 34:2d7d09f9965c 1214 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1215 positionControl(HAV_HAUT, 230, PLAYTIME, BLED_ON, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1216 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1217 positionControl(HAR_HAUT, 790, PLAYTIME, BLED_ON, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1218 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1219 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1220 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1221 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1222 }
ares1999 34:2d7d09f9965c 1223 //verification();
ares1999 34:2d7d09f9965c 1224 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1225 etat = rangement_bas;
ares1999 34:2d7d09f9965c 1226 break;
ares1999 34:2d7d09f9965c 1227 case rangement_bas:
ares1999 34:2d7d09f9965c 1228 if(bras_choix==210){
ares1999 34:2d7d09f9965c 1229 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1230 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1231 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1232 }
ares1999 34:2d7d09f9965c 1233 else if(bras_choix==66){
ares1999 34:2d7d09f9965c 1234 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1235 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1236 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1237 }
ares1999 34:2d7d09f9965c 1238 //verification();
ares1999 34:2d7d09f9965c 1239 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1240 aut_3_bras_passe=0;
ares1999 34:2d7d09f9965c 1241 aut_passe = 0;
ares1999 34:2d7d09f9965c 1242 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1243 etat = init;
ares1999 34:2d7d09f9965c 1244 break;
ares1999 34:2d7d09f9965c 1245 }
ares1999 34:2d7d09f9965c 1246 }
ares1999 34:2d7d09f9965c 1247
ares1999 34:2d7d09f9965c 1248 ////////////////////////////////////////////////////////////////Automate prise bas////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1249 void automate_prise_bas_1(void)
ares1999 34:2d7d09f9965c 1250 {
ares1999 34:2d7d09f9965c 1251 typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
ares1999 34:2d7d09f9965c 1252 static type_etat etat = init;
ares1999 34:2d7d09f9965c 1253 //static bool sens=0; //si = 0 le bras sélectionné est à l'avant si = 1 à l'arriere
ares1999 34:2d7d09f9965c 1254
ares1999 34:2d7d09f9965c 1255 switch(etat) {
ares1999 34:2d7d09f9965c 1256 case init:
ares1999 34:2d7d09f9965c 1257 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 1258 etat=rangement;
ares1999 34:2d7d09f9965c 1259 break;
ares1999 34:2d7d09f9965c 1260 case rangement:
ares1999 34:2d7d09f9965c 1261 if(bras_choix==0)
ares1999 34:2d7d09f9965c 1262 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1263 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 1264 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1265 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 1266 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1267 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 1268 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1269 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 1270 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1271 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 1272 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1273 //verification();
ares1999 34:2d7d09f9965c 1274 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1275 etat = pre_prise;
ares1999 34:2d7d09f9965c 1276 break;
ares1999 34:2d7d09f9965c 1277 case pre_prise:
ares1999 34:2d7d09f9965c 1278 if(bras_choix==0)
ares1999 34:2d7d09f9965c 1279 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1280 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 1281 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1282 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 1283 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1284 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 1285 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1286 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 1287 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1288 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 1289 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1290 //verification();
ares1999 34:2d7d09f9965c 1291 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1292 etat = prise;
ares1999 34:2d7d09f9965c 1293 break;
ares1999 34:2d7d09f9965c 1294 case prise:
ares1999 34:2d7d09f9965c 1295 if(bras_choix==0)
ares1999 34:2d7d09f9965c 1296 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1297 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 1298 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1299 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 1300 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1301 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 1302 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1303 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 1304 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1305 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 1306 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1307 //verification();
ares1999 34:2d7d09f9965c 1308 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 1309 etat = rangement2;
ares1999 34:2d7d09f9965c 1310 break;
ares1999 34:2d7d09f9965c 1311 case rangement2:
ares1999 34:2d7d09f9965c 1312 if(bras_choix==0)
ares1999 34:2d7d09f9965c 1313 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1314 else if(bras_choix==1)
ares1999 34:2d7d09f9965c 1315 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1316 else if(bras_choix==2)
ares1999 34:2d7d09f9965c 1317 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1318 else if(bras_choix==3)
ares1999 34:2d7d09f9965c 1319 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1320 else if(bras_choix==4)
ares1999 34:2d7d09f9965c 1321 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1322 else if(bras_choix==5)
ares1999 34:2d7d09f9965c 1323 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1324 //verification();
ares1999 34:2d7d09f9965c 1325 wait_ms(TEMPO);
ares1999 35:89c1a096e896 1326 aut_1_bras_prise_bas=0;
ares1999 34:2d7d09f9965c 1327 aut_prise_bas = 0;
ares1999 34:2d7d09f9965c 1328 etat = init;
ares1999 34:2d7d09f9965c 1329 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1330 break;
ares1999 34:2d7d09f9965c 1331 default:
ares1999 34:2d7d09f9965c 1332 break;
ares1999 34:2d7d09f9965c 1333 }
ares1999 34:2d7d09f9965c 1334 }
ares1999 34:2d7d09f9965c 1335
ares1999 34:2d7d09f9965c 1336 void automate_prise_bas_2(void)
ares1999 34:2d7d09f9965c 1337 {
ares1999 34:2d7d09f9965c 1338 typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
ares1999 34:2d7d09f9965c 1339 static type_etat etat = init;
ares1999 34:2d7d09f9965c 1340
ares1999 34:2d7d09f9965c 1341 switch(etat) {
ares1999 34:2d7d09f9965c 1342 case init:
ares1999 34:2d7d09f9965c 1343 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 1344 etat=rangement;
ares1999 34:2d7d09f9965c 1345 break;
ares1999 34:2d7d09f9965c 1346 case rangement:
ares1999 34:2d7d09f9965c 1347 if(bras_choix == 21) {
ares1999 34:2d7d09f9965c 1348 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1349 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1350 } else if(bras_choix == 20) {
ares1999 34:2d7d09f9965c 1351 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1352 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1353 } else if(bras_choix == 10) {
ares1999 34:2d7d09f9965c 1354 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1355 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1356 } else if(bras_choix == 43) {
ares1999 34:2d7d09f9965c 1357 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1358 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1359 } else if(bras_choix == 53) {
ares1999 34:2d7d09f9965c 1360 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1361 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1362 } else if(bras_choix == 54) {
ares1999 34:2d7d09f9965c 1363 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1364 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1365 }
ares1999 34:2d7d09f9965c 1366 //verification();
ares1999 34:2d7d09f9965c 1367 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1368 etat = pre_prise;
ares1999 34:2d7d09f9965c 1369 break;
ares1999 34:2d7d09f9965c 1370
ares1999 34:2d7d09f9965c 1371 case pre_prise:
ares1999 34:2d7d09f9965c 1372 if(bras_choix == 21) {
ares1999 34:2d7d09f9965c 1373 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1374 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1375 } else if(bras_choix == 20) {
ares1999 34:2d7d09f9965c 1376 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1377 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1378 } else if(bras_choix == 10) {
ares1999 34:2d7d09f9965c 1379 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1380 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1381 } else if(bras_choix == 43) {
ares1999 34:2d7d09f9965c 1382 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1383 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1384 } else if(bras_choix == 53) {
ares1999 34:2d7d09f9965c 1385 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1386 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1387 } else if(bras_choix == 54) {
ares1999 34:2d7d09f9965c 1388 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1389 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1390 }
ares1999 34:2d7d09f9965c 1391 //verification();
ares1999 34:2d7d09f9965c 1392 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1393 etat = prise;
ares1999 34:2d7d09f9965c 1394 break;
ares1999 34:2d7d09f9965c 1395
ares1999 34:2d7d09f9965c 1396 case prise:
ares1999 34:2d7d09f9965c 1397 if(bras_choix == 21) {
ares1999 34:2d7d09f9965c 1398 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1399 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1400 } else if(bras_choix == 20) {
ares1999 34:2d7d09f9965c 1401 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1402 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1403 } else if(bras_choix == 10) {
ares1999 34:2d7d09f9965c 1404 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1405 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1406 } else if(bras_choix == 43) {
ares1999 34:2d7d09f9965c 1407 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1408 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1409 } else if(bras_choix == 53) {
ares1999 34:2d7d09f9965c 1410 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1411 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1412 } else if(bras_choix == 54) {
ares1999 34:2d7d09f9965c 1413 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1414 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1415 }
ares1999 34:2d7d09f9965c 1416 //verification();
ares1999 34:2d7d09f9965c 1417 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 1418 etat = rangement2;
ares1999 34:2d7d09f9965c 1419 break;
ares1999 34:2d7d09f9965c 1420
ares1999 34:2d7d09f9965c 1421 case rangement2:
ares1999 34:2d7d09f9965c 1422 if(bras_choix == 21) {
ares1999 34:2d7d09f9965c 1423 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1424 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1425 } else if(bras_choix == 20) {
ares1999 34:2d7d09f9965c 1426 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1427 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1428 } else if(bras_choix == 10) {
ares1999 34:2d7d09f9965c 1429 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1430 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1431 } else if(bras_choix == 43) {
ares1999 34:2d7d09f9965c 1432 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1433 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1434 } else if(bras_choix == 53) {
ares1999 34:2d7d09f9965c 1435 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1436 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1437 } else if(bras_choix == 54) {
ares1999 34:2d7d09f9965c 1438 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1439 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1440 }
ares1999 34:2d7d09f9965c 1441 //verification();
ares1999 34:2d7d09f9965c 1442 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1443 aut_2_bras_prise_bas = 0;
ares1999 34:2d7d09f9965c 1444 aut_prise_bas = 0 ;
ares1999 34:2d7d09f9965c 1445 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1446 etat = init;
ares1999 34:2d7d09f9965c 1447 break;
ares1999 34:2d7d09f9965c 1448
ares1999 34:2d7d09f9965c 1449 default:
ares1999 34:2d7d09f9965c 1450 break;
ares1999 34:2d7d09f9965c 1451 }
ares1999 34:2d7d09f9965c 1452 }
ares1999 34:2d7d09f9965c 1453
ares1999 34:2d7d09f9965c 1454 void automate_prise_bas_3(void)
ares1999 34:2d7d09f9965c 1455 {
ares1999 34:2d7d09f9965c 1456 typedef enum {init, rangement, pre_prise, prise, rangement2} type_etat;
ares1999 34:2d7d09f9965c 1457 static type_etat etat = init;
ares1999 34:2d7d09f9965c 1458
ares1999 34:2d7d09f9965c 1459 switch(etat) {
ares1999 34:2d7d09f9965c 1460 case init:
ares1999 34:2d7d09f9965c 1461 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 1462 etat=rangement;
ares1999 34:2d7d09f9965c 1463 break;
ares1999 34:2d7d09f9965c 1464 case rangement:
ares1999 34:2d7d09f9965c 1465 if(bras_choix == 210) {
ares1999 34:2d7d09f9965c 1466 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1467 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1468 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1469 } else if(bras_choix == 66) {
ares1999 34:2d7d09f9965c 1470 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1471 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1472 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1473 }
ares1999 34:2d7d09f9965c 1474 //verification();
ares1999 34:2d7d09f9965c 1475 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1476 etat = pre_prise;
ares1999 34:2d7d09f9965c 1477 break;
ares1999 34:2d7d09f9965c 1478
ares1999 34:2d7d09f9965c 1479 case pre_prise:
ares1999 34:2d7d09f9965c 1480 if(bras_choix == 210) {
ares1999 34:2d7d09f9965c 1481 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1482 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1483 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1484 } else if(bras_choix == 66) {
ares1999 34:2d7d09f9965c 1485 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1486 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1487 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1488 }
ares1999 34:2d7d09f9965c 1489 //verification();
ares1999 34:2d7d09f9965c 1490 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1491 etat = prise;
ares1999 34:2d7d09f9965c 1492 break;
ares1999 34:2d7d09f9965c 1493
ares1999 34:2d7d09f9965c 1494 case prise:
ares1999 34:2d7d09f9965c 1495 if(bras_choix == 210) {
ares1999 34:2d7d09f9965c 1496 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1497 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1498 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1499 } else if(bras_choix == 66) {
ares1999 34:2d7d09f9965c 1500 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1501 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1502 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1503 }
ares1999 34:2d7d09f9965c 1504 //verification();
ares1999 34:2d7d09f9965c 1505 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 1506 etat = rangement2;
ares1999 34:2d7d09f9965c 1507 break;
ares1999 34:2d7d09f9965c 1508
ares1999 34:2d7d09f9965c 1509 case rangement2:
ares1999 34:2d7d09f9965c 1510 if(bras_choix == 210) {
ares1999 34:2d7d09f9965c 1511 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1512 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1513 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1514 } else if(bras_choix == 66) {
ares1999 34:2d7d09f9965c 1515 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1516 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1517 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1518 }
ares1999 34:2d7d09f9965c 1519 //verification();
ares1999 34:2d7d09f9965c 1520 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1521 aut_3_bras_prise_bas = 0;
ares1999 34:2d7d09f9965c 1522 aut_prise_bas = 0 ;
ares1999 34:2d7d09f9965c 1523 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1524 etat = init;
ares1999 34:2d7d09f9965c 1525 break;
ares1999 34:2d7d09f9965c 1526
ares1999 34:2d7d09f9965c 1527 default:
ares1999 34:2d7d09f9965c 1528 break;
ares1999 34:2d7d09f9965c 1529 }
ares1999 34:2d7d09f9965c 1530 }
ares1999 34:2d7d09f9965c 1531
ares1999 34:2d7d09f9965c 1532 ////////////////////////////////////////////////////////////////Gestion Chasse Neige////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1533 void Gestion_CN(void)
ares1999 34:2d7d09f9965c 1534 {
ares1999 34:2d7d09f9965c 1535 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
ares1999 34:2d7d09f9965c 1536 switch(ouv_ferm_CN)
ares1999 34:2d7d09f9965c 1537 {
ares1999 34:2d7d09f9965c 1538 case 0: //ouverture
ares1999 34:2d7d09f9965c 1539 positionControl_Mul_ensemble(CN_GAUCHE_AV, 790, PLAYTIME, RLED_ON, CN_DROITE_AV, 240, RLED_ON, SERIAL_SPECIAL);
ares1999 34:2d7d09f9965c 1540 break;
ares1999 34:2d7d09f9965c 1541
ares1999 34:2d7d09f9965c 1542 case 1: //fermeture
ares1999 34:2d7d09f9965c 1543 positionControl_Mul_ensemble(CN_GAUCHE_AV, 240, PLAYTIME, RLED_ON, CN_DROITE_AV, 790, RLED_ON, SERIAL_SPECIAL);
ares1999 34:2d7d09f9965c 1544 break;
ares1999 34:2d7d09f9965c 1545
ares1999 34:2d7d09f9965c 1546 default:
ares1999 34:2d7d09f9965c 1547 break;
ares1999 34:2d7d09f9965c 1548 }
ares1999 34:2d7d09f9965c 1549 //verification();
ares1999 34:2d7d09f9965c 1550 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1551 ouv_ferm_CN = 0;
ares1999 34:2d7d09f9965c 1552 aut_CN = 0;
ares1999 34:2d7d09f9965c 1553 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1554 }
ares1999 34:2d7d09f9965c 1555
ares1999 34:2d7d09f9965c 1556 ////////////////////////////////////////////////////////////////Selection prise bas////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1557 void selection_bras_prise_bas(void)
ares1999 34:2d7d09f9965c 1558 {
ares1999 34:2d7d09f9965c 1559
ares1999 34:2d7d09f9965c 1560 if(bras_choix<6)
ares1999 34:2d7d09f9965c 1561 {
ares1999 34:2d7d09f9965c 1562 aut_1_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1563 automate_prise_bas_1();
ares1999 34:2d7d09f9965c 1564 }
ares1999 34:2d7d09f9965c 1565 else if(bras_choix>5)
ares1999 34:2d7d09f9965c 1566 {
ares1999 34:2d7d09f9965c 1567 switch(bras_choix)
ares1999 34:2d7d09f9965c 1568 {
ares1999 34:2d7d09f9965c 1569 case 10:
ares1999 34:2d7d09f9965c 1570 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1571 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1572 break;
ares1999 34:2d7d09f9965c 1573
ares1999 34:2d7d09f9965c 1574 case 20:
ares1999 34:2d7d09f9965c 1575 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1576 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1577 break;
ares1999 34:2d7d09f9965c 1578
ares1999 34:2d7d09f9965c 1579 case 21:
ares1999 34:2d7d09f9965c 1580 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1581 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1582 break;
ares1999 34:2d7d09f9965c 1583
ares1999 34:2d7d09f9965c 1584 case 210:
ares1999 34:2d7d09f9965c 1585 aut_3_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1586 automate_prise_bas_3();
ares1999 34:2d7d09f9965c 1587 break;
ares1999 34:2d7d09f9965c 1588
ares1999 34:2d7d09f9965c 1589 case 43:
ares1999 34:2d7d09f9965c 1590 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1591 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1592 break;
ares1999 34:2d7d09f9965c 1593
ares1999 34:2d7d09f9965c 1594 case 53:
ares1999 34:2d7d09f9965c 1595 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1596 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1597 break;
ares1999 34:2d7d09f9965c 1598
ares1999 34:2d7d09f9965c 1599 case 54:
ares1999 34:2d7d09f9965c 1600 aut_2_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1601 automate_prise_bas_2();
ares1999 34:2d7d09f9965c 1602 break;
ares1999 34:2d7d09f9965c 1603
ares1999 34:2d7d09f9965c 1604 case 66:
ares1999 34:2d7d09f9965c 1605 aut_3_bras_prise_bas = 1;
ares1999 34:2d7d09f9965c 1606 automate_prise_bas_3();
ares1999 34:2d7d09f9965c 1607 break;
ares1999 34:2d7d09f9965c 1608
ares1999 34:2d7d09f9965c 1609 default:
ares1999 34:2d7d09f9965c 1610 break;
ares1999 34:2d7d09f9965c 1611 }
ares1999 34:2d7d09f9965c 1612 }
ares1999 34:2d7d09f9965c 1613 }
ares1999 34:2d7d09f9965c 1614
ares1999 34:2d7d09f9965c 1615 ////////////////////////////////////////////////////////////////Selection passe////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1616 void selection_bras_passe(void)
ares1999 34:2d7d09f9965c 1617 {
ares1999 34:2d7d09f9965c 1618
ares1999 34:2d7d09f9965c 1619 if(bras_choix<6)
ares1999 34:2d7d09f9965c 1620 {
ares1999 34:2d7d09f9965c 1621 aut_1_bras_passe = 1;
ares1999 34:2d7d09f9965c 1622 automate_passe_1();
ares1999 34:2d7d09f9965c 1623 }
ares1999 34:2d7d09f9965c 1624 else if(bras_choix>5)
ares1999 34:2d7d09f9965c 1625 {
ares1999 34:2d7d09f9965c 1626 switch(bras_choix)
ares1999 34:2d7d09f9965c 1627 {
ares1999 34:2d7d09f9965c 1628 case 10:
ares1999 34:2d7d09f9965c 1629 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1630 automate_passe_2();
ares1999 34:2d7d09f9965c 1631 break;
ares1999 34:2d7d09f9965c 1632
ares1999 34:2d7d09f9965c 1633 case 20:
ares1999 34:2d7d09f9965c 1634 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1635 automate_passe_2();
ares1999 34:2d7d09f9965c 1636 break;
ares1999 34:2d7d09f9965c 1637
ares1999 34:2d7d09f9965c 1638 case 21:
ares1999 34:2d7d09f9965c 1639 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1640 automate_passe_2();
ares1999 34:2d7d09f9965c 1641 break;
ares1999 34:2d7d09f9965c 1642
ares1999 34:2d7d09f9965c 1643 case 210:
ares1999 34:2d7d09f9965c 1644 aut_3_bras_passe = 1;
ares1999 34:2d7d09f9965c 1645 automate_passe_3();
ares1999 34:2d7d09f9965c 1646 break;
ares1999 34:2d7d09f9965c 1647
ares1999 34:2d7d09f9965c 1648 case 43:
ares1999 34:2d7d09f9965c 1649 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1650 automate_passe_2();
ares1999 34:2d7d09f9965c 1651 break;
ares1999 34:2d7d09f9965c 1652
ares1999 34:2d7d09f9965c 1653 case 53:
ares1999 34:2d7d09f9965c 1654 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1655 automate_passe_2();
ares1999 34:2d7d09f9965c 1656 break;
ares1999 34:2d7d09f9965c 1657
ares1999 34:2d7d09f9965c 1658 case 54:
ares1999 34:2d7d09f9965c 1659 aut_2_bras_passe = 1;
ares1999 34:2d7d09f9965c 1660 automate_passe_2();
ares1999 34:2d7d09f9965c 1661 break;
ares1999 34:2d7d09f9965c 1662
ares1999 34:2d7d09f9965c 1663 case 66:
ares1999 34:2d7d09f9965c 1664 aut_3_bras_passe = 1;
ares1999 34:2d7d09f9965c 1665 automate_passe_3();
ares1999 34:2d7d09f9965c 1666 break;
ares1999 34:2d7d09f9965c 1667
ares1999 34:2d7d09f9965c 1668 default:
ares1999 34:2d7d09f9965c 1669 break;
ares1999 34:2d7d09f9965c 1670 }
ares1999 34:2d7d09f9965c 1671 }
ares1999 34:2d7d09f9965c 1672 }
ares1999 34:2d7d09f9965c 1673
ares1999 34:2d7d09f9965c 1674 ////////////////////////////////////////////////////////////////Selection relache bas////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1675 void selection_bras_relache_bas(void)
ares1999 34:2d7d09f9965c 1676 {
ares1999 34:2d7d09f9965c 1677
ares1999 34:2d7d09f9965c 1678 if(bras_choix<6)
ares1999 34:2d7d09f9965c 1679 {
ares1999 34:2d7d09f9965c 1680 aut_1_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1681 automate_relache_bas_1();
ares1999 34:2d7d09f9965c 1682 }
ares1999 34:2d7d09f9965c 1683 else if(bras_choix>5)
ares1999 34:2d7d09f9965c 1684 {
ares1999 34:2d7d09f9965c 1685 switch(bras_choix)
ares1999 34:2d7d09f9965c 1686 {
ares1999 34:2d7d09f9965c 1687 case 10:
ares1999 34:2d7d09f9965c 1688 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1689 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1690 break;
ares1999 34:2d7d09f9965c 1691
ares1999 34:2d7d09f9965c 1692 case 20:
ares1999 34:2d7d09f9965c 1693 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1694 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1695 break;
ares1999 34:2d7d09f9965c 1696
ares1999 34:2d7d09f9965c 1697 case 21:
ares1999 34:2d7d09f9965c 1698 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1699 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1700 break;
ares1999 34:2d7d09f9965c 1701
ares1999 34:2d7d09f9965c 1702 case 210:
ares1999 34:2d7d09f9965c 1703 aut_3_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1704 automate_relache_bas_3();
ares1999 34:2d7d09f9965c 1705 break;
ares1999 34:2d7d09f9965c 1706
ares1999 34:2d7d09f9965c 1707 case 43:
ares1999 34:2d7d09f9965c 1708 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1709 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1710 break;
ares1999 34:2d7d09f9965c 1711
ares1999 34:2d7d09f9965c 1712 case 53:
ares1999 34:2d7d09f9965c 1713 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1714 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1715 break;
ares1999 34:2d7d09f9965c 1716
ares1999 34:2d7d09f9965c 1717 case 54:
ares1999 34:2d7d09f9965c 1718 aut_2_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1719 automate_relache_bas_2();
ares1999 34:2d7d09f9965c 1720 break;
ares1999 34:2d7d09f9965c 1721
ares1999 34:2d7d09f9965c 1722 case 66:
ares1999 34:2d7d09f9965c 1723 aut_3_bras_relache_bas = 1;
ares1999 34:2d7d09f9965c 1724 automate_relache_bas_3();
ares1999 34:2d7d09f9965c 1725 break;
ares1999 34:2d7d09f9965c 1726
ares1999 34:2d7d09f9965c 1727 default:
ares1999 34:2d7d09f9965c 1728 break;
ares1999 34:2d7d09f9965c 1729 }
ares1999 34:2d7d09f9965c 1730 }
ares1999 34:2d7d09f9965c 1731 }
ares1999 34:2d7d09f9965c 1732 ////////////////////////////////////////////////////////////////Selection pose haut////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1733 void selection_bras_pose_haut(void)
ares1999 34:2d7d09f9965c 1734 {
ares1999 34:2d7d09f9965c 1735
ares1999 34:2d7d09f9965c 1736 if(bras_choix<6)
ares1999 34:2d7d09f9965c 1737 {
ares1999 34:2d7d09f9965c 1738 aut_1_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1739 automate_pose_haut_1();
ares1999 34:2d7d09f9965c 1740 }
ares1999 34:2d7d09f9965c 1741 else if(bras_choix>5)
ares1999 34:2d7d09f9965c 1742 {
ares1999 34:2d7d09f9965c 1743 switch(bras_choix)
ares1999 34:2d7d09f9965c 1744 {
ares1999 34:2d7d09f9965c 1745 case 10:
ares1999 34:2d7d09f9965c 1746 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1747 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1748 break;
ares1999 34:2d7d09f9965c 1749
ares1999 34:2d7d09f9965c 1750 case 20:
ares1999 34:2d7d09f9965c 1751 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1752 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1753 break;
ares1999 34:2d7d09f9965c 1754
ares1999 34:2d7d09f9965c 1755 case 21:
ares1999 34:2d7d09f9965c 1756 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1757 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1758 break;
ares1999 34:2d7d09f9965c 1759
ares1999 34:2d7d09f9965c 1760 case 210:
ares1999 34:2d7d09f9965c 1761 aut_3_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1762 automate_pose_haut_3();
ares1999 34:2d7d09f9965c 1763 break;
ares1999 34:2d7d09f9965c 1764
ares1999 34:2d7d09f9965c 1765 case 43:
ares1999 34:2d7d09f9965c 1766 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1767 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1768 break;
ares1999 34:2d7d09f9965c 1769
ares1999 34:2d7d09f9965c 1770 case 53:
ares1999 34:2d7d09f9965c 1771 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1772 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1773 break;
ares1999 34:2d7d09f9965c 1774
ares1999 34:2d7d09f9965c 1775 case 54:
ares1999 34:2d7d09f9965c 1776 aut_2_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1777 automate_pose_haut_2();
ares1999 34:2d7d09f9965c 1778 break;
ares1999 34:2d7d09f9965c 1779
ares1999 34:2d7d09f9965c 1780 case 66:
ares1999 34:2d7d09f9965c 1781 aut_3_bras_pose_haut = 1;
ares1999 34:2d7d09f9965c 1782 automate_pose_haut_3();
ares1999 34:2d7d09f9965c 1783 break;
ares1999 34:2d7d09f9965c 1784
ares1999 34:2d7d09f9965c 1785 default:
ares1999 34:2d7d09f9965c 1786 break;
ares1999 34:2d7d09f9965c 1787 }
ares1999 34:2d7d09f9965c 1788 }
ares1999 34:2d7d09f9965c 1789 }
ares1999 34:2d7d09f9965c 1790
ares1999 34:2d7d09f9965c 1791 ////////////////////////////////////////////////////////////////Selection rangement////////////////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 1792 void selection_bras_rangement(void)
ares1999 34:2d7d09f9965c 1793 {
ares1999 34:2d7d09f9965c 1794 switch(bras_choix) {
ares1999 34:2d7d09f9965c 1795 case 0:
ares1999 34:2d7d09f9965c 1796 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1797 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1798 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1799 break;
ares1999 34:2d7d09f9965c 1800
ares1999 34:2d7d09f9965c 1801 case 1:
ares1999 34:2d7d09f9965c 1802 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1803 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1804 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1805 break;
ares1999 34:2d7d09f9965c 1806
ares1999 34:2d7d09f9965c 1807 case 2:
ares1999 34:2d7d09f9965c 1808 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1809 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1810 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1811 break;
ares1999 34:2d7d09f9965c 1812
ares1999 34:2d7d09f9965c 1813 case 3:
ares1999 34:2d7d09f9965c 1814 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1815 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1816 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1817 break;
ares1999 34:2d7d09f9965c 1818
ares1999 34:2d7d09f9965c 1819 case 4:
ares1999 34:2d7d09f9965c 1820 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1821 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1822 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1823 break;
ares1999 34:2d7d09f9965c 1824
ares1999 34:2d7d09f9965c 1825 case 5:
ares1999 34:2d7d09f9965c 1826 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1827 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1828 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1829 break;
ares1999 34:2d7d09f9965c 1830
ares1999 34:2d7d09f9965c 1831 case 10:
ares1999 34:2d7d09f9965c 1832 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1833 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1834 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1835 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1836 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1837 break;
ares1999 34:2d7d09f9965c 1838
ares1999 34:2d7d09f9965c 1839 case 20:
ares1999 34:2d7d09f9965c 1840 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1841 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1842 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1843 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1844 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1845 break;
ares1999 34:2d7d09f9965c 1846
ares1999 34:2d7d09f9965c 1847 case 21:
ares1999 34:2d7d09f9965c 1848 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1849 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1850 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1851 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1852 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1853 break;
ares1999 34:2d7d09f9965c 1854
ares1999 34:2d7d09f9965c 1855 case 210:
ares1999 34:2d7d09f9965c 1856 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1857 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1858 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1859 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1860 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1861 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1862 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1863 break;
ares1999 34:2d7d09f9965c 1864
ares1999 34:2d7d09f9965c 1865 case 43:
ares1999 34:2d7d09f9965c 1866 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1867 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1868 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1869 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1870 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1871 break;
ares1999 34:2d7d09f9965c 1872
ares1999 34:2d7d09f9965c 1873 case 53:
ares1999 34:2d7d09f9965c 1874 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1875 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1876 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1877 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1878 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1879 break;
ares1999 34:2d7d09f9965c 1880
ares1999 34:2d7d09f9965c 1881 case 54:
ares1999 34:2d7d09f9965c 1882 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1883 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1884 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1885 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1886 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1887 break;
ares1999 34:2d7d09f9965c 1888
ares1999 34:2d7d09f9965c 1889 case 66: //3 4 5
ares1999 34:2d7d09f9965c 1890 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1891 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1892 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1893 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1894 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 1895 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 1896 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 1897 break;
ares1999 34:2d7d09f9965c 1898 default:
ares1999 34:2d7d09f9965c 1899 break;
ares1999 34:2d7d09f9965c 1900 }
ares1999 34:2d7d09f9965c 1901 aut_rangement=0;
ares1999 34:2d7d09f9965c 1902 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
ares1999 34:2d7d09f9965c 1903 }
ares1999 34:2d7d09f9965c 1904
ares1999 34:2d7d09f9965c 1905
ares1999 34:2d7d09f9965c 1906 /*void fct_bras_Gauche(char serial_select)
ares1999 34:2d7d09f9965c 1907 {
ares1999 34:2d7d09f9965c 1908 //Rangement bras bas
ares1999 34:2d7d09f9965c 1909 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
ares1999 34:2d7d09f9965c 1910 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1911 //Rangement bras haut
ares1999 34:2d7d09f9965c 1912 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HAV, serial_select);
ares1999 34:2d7d09f9965c 1913 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1914 //Pré Prise bras bas
ares1999 34:2d7d09f9965c 1915 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, serial_select);
ares1999 34:2d7d09f9965c 1916 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1917 //Prise bras bas
ares1999 34:2d7d09f9965c 1918 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, serial_select);
ares1999 34:2d7d09f9965c 1919 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1920 //Pré passe bras bas
ares1999 34:2d7d09f9965c 1921 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BAV, serial_select);
ares1999 34:2d7d09f9965c 1922 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1923 positionControl(BAV_HAUT, 295, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 1924 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1925 //Passe bras bas
ares1999 34:2d7d09f9965c 1926 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BAV, serial_select);
ares1999 34:2d7d09f9965c 1927 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1928 //Pré prise bras haut
ares1999 34:2d7d09f9965c 1929 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HAV, serial_select);
ares1999 34:2d7d09f9965c 1930 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1931 //Prise bras haut
ares1999 34:2d7d09f9965c 1932 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HAV, serial_select);
ares1999 34:2d7d09f9965c 1933 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1934 //Pré rangement bras bas
ares1999 34:2d7d09f9965c 1935 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BAV, serial_select);
ares1999 34:2d7d09f9965c 1936 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1937 //Rangement bras haut
ares1999 34:2d7d09f9965c 1938 positionControl(BAV_HAUT, 535, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 1939 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1940 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HAV, serial_select);
ares1999 34:2d7d09f9965c 1941 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1942 //Rangement bras bas
ares1999 34:2d7d09f9965c 1943 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
ares1999 34:2d7d09f9965c 1944 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1945 }
ares1999 34:2d7d09f9965c 1946
ares1999 34:2d7d09f9965c 1947 void fct_bras_Droite(char serial_select)
ares1999 34:2d7d09f9965c 1948 {
ares1999 34:2d7d09f9965c 1949 //Rangement bras bas
ares1999 34:2d7d09f9965c 1950 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
ares1999 34:2d7d09f9965c 1951 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1952 //Rangement bras haut
ares1999 34:2d7d09f9965c 1953 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HAR, serial_select);
ares1999 34:2d7d09f9965c 1954 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1955 //Pré Prise bras bas
ares1999 34:2d7d09f9965c 1956 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, serial_select);
ares1999 34:2d7d09f9965c 1957 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1958 //Prise bras bas
ares1999 34:2d7d09f9965c 1959 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, serial_select);
ares1999 34:2d7d09f9965c 1960 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1961 //Pré passe bras bas
ares1999 34:2d7d09f9965c 1962 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BAR, serial_select);
ares1999 34:2d7d09f9965c 1963 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1964 positionControl(BAR_HAUT, 730, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 1965 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1966 //Passe bras bas
ares1999 34:2d7d09f9965c 1967 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BAR, serial_select);
ares1999 34:2d7d09f9965c 1968 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1969 //Pré prise bras haut
ares1999 34:2d7d09f9965c 1970 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HAR, serial_select);
ares1999 34:2d7d09f9965c 1971 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1972 //Prise bras haut
ares1999 34:2d7d09f9965c 1973 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HAR, serial_select);
ares1999 34:2d7d09f9965c 1974 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1975 //Pré rangement bras bas
ares1999 34:2d7d09f9965c 1976 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BAR, serial_select);
ares1999 34:2d7d09f9965c 1977 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1978 //Rangement bras haut
ares1999 34:2d7d09f9965c 1979 positionControl(BAR_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 1980 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1981 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HAR, serial_select);
ares1999 34:2d7d09f9965c 1982 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1983 //Rangement bras bas
ares1999 34:2d7d09f9965c 1984 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
ares1999 34:2d7d09f9965c 1985 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 1986 }
ares1999 34:2d7d09f9965c 1987
ares1999 34:2d7d09f9965c 1988 void fct_bras_AV_Milieu(char serial_select)
ares1999 34:2d7d09f9965c 1989 {
ares1999 34:2d7d09f9965c 1990 //Rangement bras bas
ares1999 34:2d7d09f9965c 1991 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 1992 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1993 //Rangement bras haut
ares1999 34:2d7d09f9965c 1994 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement_HM, serial_select);
ares1999 34:2d7d09f9965c 1995 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1996 //Pré Prise bras bas
ares1999 34:2d7d09f9965c 1997 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, serial_select);
ares1999 34:2d7d09f9965c 1998 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 1999 //Prise bras bas
ares1999 34:2d7d09f9965c 2000 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2001 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2002 //Pré passe bras bas
ares1999 34:2d7d09f9965c 2003 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_passe_BM, serial_select);
ares1999 34:2d7d09f9965c 2004 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2005 positionControl(BAV_HAUT, 785, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 2006 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2007 //Passe bras bas
ares1999 34:2d7d09f9965c 2008 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, passe_BM, serial_select);
ares1999 34:2d7d09f9965c 2009 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2010 //Pré prise bras haut
ares1999 34:2d7d09f9965c 2011 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, pre_prise_HM, serial_select);
ares1999 34:2d7d09f9965c 2012 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2013 //Prise bras haut
ares1999 34:2d7d09f9965c 2014 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, prise_HM, serial_select);
ares1999 34:2d7d09f9965c 2015 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2016 //Pré rangement bras bas
ares1999 34:2d7d09f9965c 2017 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2018 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2019 //Rangement bras haut
ares1999 34:2d7d09f9965c 2020 positionControl(BAV_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 2021 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2022 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAV, rangement2_HM, serial_select);
ares1999 34:2d7d09f9965c 2023 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2024 //Rangement bras bas
ares1999 34:2d7d09f9965c 2025 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2026 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2027 }
ares1999 34:2d7d09f9965c 2028
ares1999 34:2d7d09f9965c 2029 void fct_bras_AR_Milieu(char serial_select)
ares1999 34:2d7d09f9965c 2030 {
ares1999 34:2d7d09f9965c 2031 //Rangement bras bas
ares1999 34:2d7d09f9965c 2032 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2033 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2034 //Rangement bras haut
ares1999 34:2d7d09f9965c 2035 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement_HM, serial_select);
ares1999 34:2d7d09f9965c 2036 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2037 //Pré Prise bras bas
ares1999 34:2d7d09f9965c 2038 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2039 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2040 //Prise bras bas
ares1999 34:2d7d09f9965c 2041 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2042 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2043 //Pré passe bras bas
ares1999 34:2d7d09f9965c 2044 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_passe_BM, serial_select);
ares1999 34:2d7d09f9965c 2045 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2046 positionControl(BAR_HAUT, 785, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 2047 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2048 //Passe bras bas
ares1999 34:2d7d09f9965c 2049 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, passe_BM, serial_select);
ares1999 34:2d7d09f9965c 2050 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2051 //Pré prise bras haut
ares1999 34:2d7d09f9965c 2052 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, pre_prise_HM, serial_select);
ares1999 34:2d7d09f9965c 2053 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2054 //Prise bras haut
ares1999 34:2d7d09f9965c 2055 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, prise_HM, serial_select);
ares1999 34:2d7d09f9965c 2056 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2057 //Pré rangement bras bas
ares1999 34:2d7d09f9965c 2058 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2059 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2060 //Rangement bras haut
ares1999 34:2d7d09f9965c 2061 positionControl(BAR_HAUT, 485, PLAYTIME, BLED_ON, serial_select);
ares1999 34:2d7d09f9965c 2062 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2063 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_HAR, rangement2_HM, serial_select);
ares1999 34:2d7d09f9965c 2064 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2065 //Rangement bras bas
ares1999 34:2d7d09f9965c 2066 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2067 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2068 }
ares1999 34:2d7d09f9965c 2069
ares1999 34:2d7d09f9965c 2070 void fct_prise_tous_AV(void)
ares1999 34:2d7d09f9965c 2071 {
ares1999 34:2d7d09f9965c 2072 typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
ares1999 34:2d7d09f9965c 2073 static type_etat etat = rangement;
ares1999 34:2d7d09f9965c 2074 for(int i=1;i<=4;i++){
ares1999 34:2d7d09f9965c 2075 switch(etat) {
ares1999 34:2d7d09f9965c 2076 case rangement:
ares1999 34:2d7d09f9965c 2077 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 2078 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 2079 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 2080 //verification();
ares1999 34:2d7d09f9965c 2081 pc.printf("\n1\n\r");
ares1999 34:2d7d09f9965c 2082 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2083 etat = pre_prise;
ares1999 34:2d7d09f9965c 2084 break;
ares1999 34:2d7d09f9965c 2085 case pre_prise:
ares1999 34:2d7d09f9965c 2086 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 2087 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 2088 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 2089 //verification();
ares1999 34:2d7d09f9965c 2090 pc.printf("\n2\n\r");
ares1999 34:2d7d09f9965c 2091 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2092 etat = prise;
ares1999 34:2d7d09f9965c 2093 break;
ares1999 34:2d7d09f9965c 2094 case prise:
ares1999 34:2d7d09f9965c 2095 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 2096 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 2097 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 2098 //verification();
ares1999 34:2d7d09f9965c 2099 pc.printf("\n3\n\r");
ares1999 34:2d7d09f9965c 2100 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 2101 etat = rangement_2;
ares1999 34:2d7d09f9965c 2102 break;
ares1999 34:2d7d09f9965c 2103 case rangement_2:
ares1999 34:2d7d09f9965c 2104 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, SERIAL_GAUCHE);
ares1999 34:2d7d09f9965c 2105 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, SERIAL_MILIEU);
ares1999 34:2d7d09f9965c 2106 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, SERIAL_DROITE);
ares1999 34:2d7d09f9965c 2107 //verification();
ares1999 34:2d7d09f9965c 2108 pc.printf("\n4\n\r");
ares1999 34:2d7d09f9965c 2109 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2110 etat = rangement;
ares1999 34:2d7d09f9965c 2111 break;
ares1999 34:2d7d09f9965c 2112 }
ares1999 34:2d7d09f9965c 2113 }
ares1999 34:2d7d09f9965c 2114 }
ares1999 34:2d7d09f9965c 2115
ares1999 34:2d7d09f9965c 2116 void fct_prise_Gauche(char serial_select) //servo 1,2,3
ares1999 34:2d7d09f9965c 2117 {
ares1999 34:2d7d09f9965c 2118 for(int i=1;i<=4;i++){
ares1999 34:2d7d09f9965c 2119 typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
ares1999 34:2d7d09f9965c 2120 static type_etat etat = rangement;
ares1999 34:2d7d09f9965c 2121
ares1999 34:2d7d09f9965c 2122 switch(etat) {
ares1999 34:2d7d09f9965c 2123 case rangement:
ares1999 34:2d7d09f9965c 2124 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BAV, serial_select);
ares1999 34:2d7d09f9965c 2125 //verification();
ares1999 34:2d7d09f9965c 2126 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2127 etat = pre_prise;
ares1999 34:2d7d09f9965c 2128 break;
ares1999 34:2d7d09f9965c 2129 case pre_prise:
ares1999 34:2d7d09f9965c 2130 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BAV, serial_select);
ares1999 34:2d7d09f9965c 2131 //verification();
ares1999 34:2d7d09f9965c 2132 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2133 etat = prise;
ares1999 34:2d7d09f9965c 2134 break;
ares1999 34:2d7d09f9965c 2135 case prise:
ares1999 34:2d7d09f9965c 2136 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BAV, serial_select);
ares1999 34:2d7d09f9965c 2137 //verification();
ares1999 34:2d7d09f9965c 2138 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 2139 etat = rangement_2;
ares1999 34:2d7d09f9965c 2140 break;
ares1999 34:2d7d09f9965c 2141 case rangement_2:
ares1999 34:2d7d09f9965c 2142 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BAV, serial_select);
ares1999 34:2d7d09f9965c 2143 //verification();
ares1999 34:2d7d09f9965c 2144 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2145 etat = rangement;
ares1999 34:2d7d09f9965c 2146 break;
ares1999 34:2d7d09f9965c 2147 }
ares1999 34:2d7d09f9965c 2148 }
ares1999 34:2d7d09f9965c 2149 }
ares1999 34:2d7d09f9965c 2150
ares1999 34:2d7d09f9965c 2151 void fct_prise_Droite(char serial_select) //servo 4,5,6
ares1999 34:2d7d09f9965c 2152 {
ares1999 34:2d7d09f9965c 2153 for(int i=1;i<=4;i++){
ares1999 34:2d7d09f9965c 2154 typedef enum {rangement, pre_prise, prise, rangement_2} type_etat;
ares1999 34:2d7d09f9965c 2155 static type_etat etat = rangement;
ares1999 34:2d7d09f9965c 2156
ares1999 34:2d7d09f9965c 2157 switch(etat) {
ares1999 34:2d7d09f9965c 2158 case rangement:
ares1999 34:2d7d09f9965c 2159 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BAR, serial_select);
ares1999 34:2d7d09f9965c 2160 //verification();
ares1999 34:2d7d09f9965c 2161 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2162 etat = pre_prise;
ares1999 34:2d7d09f9965c 2163 break;
ares1999 34:2d7d09f9965c 2164 case pre_prise:
ares1999 34:2d7d09f9965c 2165 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BAR, serial_select);
ares1999 34:2d7d09f9965c 2166 //verification();
ares1999 34:2d7d09f9965c 2167 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2168 etat = prise;
ares1999 34:2d7d09f9965c 2169 break;
ares1999 34:2d7d09f9965c 2170 case prise:
ares1999 34:2d7d09f9965c 2171 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BAR, serial_select);
ares1999 34:2d7d09f9965c 2172 //verification();
ares1999 34:2d7d09f9965c 2173 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 2174 etat = rangement_2;
ares1999 34:2d7d09f9965c 2175 break;
ares1999 34:2d7d09f9965c 2176 case rangement_2:
ares1999 34:2d7d09f9965c 2177 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BAR, serial_select);
ares1999 34:2d7d09f9965c 2178 //verification();
ares1999 34:2d7d09f9965c 2179 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2180 etat = rangement;
ares1999 34:2d7d09f9965c 2181 break;
ares1999 34:2d7d09f9965c 2182 }
ares1999 34:2d7d09f9965c 2183 }
ares1999 34:2d7d09f9965c 2184 }
ares1999 34:2d7d09f9965c 2185
ares1999 34:2d7d09f9965c 2186 void fct_prise_AV_milieu(char serial_select)
ares1999 34:2d7d09f9965c 2187 {
ares1999 34:2d7d09f9965c 2188 //Rangement
ares1999 34:2d7d09f9965c 2189 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2190 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2191 //Pré Prise
ares1999 34:2d7d09f9965c 2192 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, pre_prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2193 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2194 //Prise
ares1999 34:2d7d09f9965c 2195 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2196 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 2197 //Rangement avec echantillon
ares1999 34:2d7d09f9965c 2198 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAV, rangement2_BM, serial_select);
ares1999 34:2d7d09f9965c 2199 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2200 }
ares1999 34:2d7d09f9965c 2201
ares1999 34:2d7d09f9965c 2202 void fct_prise_AR_milieu(char serial_select)
ares1999 34:2d7d09f9965c 2203 {
ares1999 34:2d7d09f9965c 2204 //Rangement
ares1999 34:2d7d09f9965c 2205 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement_BM, serial_select);
ares1999 34:2d7d09f9965c 2206 wait_ms(TEMPO_LONGUE);
ares1999 34:2d7d09f9965c 2207 //Pré Prise
ares1999 34:2d7d09f9965c 2208 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, pre_prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2209 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2210 //Prise
ares1999 34:2d7d09f9965c 2211 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, prise_BM, serial_select);
ares1999 34:2d7d09f9965c 2212 wait_ms(TEMPO_POMPE);
ares1999 34:2d7d09f9965c 2213 //Rangement avec echantillon
ares1999 34:2d7d09f9965c 2214 positionControl_Mul_ensemble_complex(3, PLAYTIME, servos_BAR, rangement2_BM, serial_select);
ares1999 34:2d7d09f9965c 2215 wait_ms(TEMPO);
ares1999 34:2d7d09f9965c 2216 }
ares1999 34:2d7d09f9965c 2217
ares1999 34:2d7d09f9965c 2218 ////////////////////////////////////////////////CHASSE-NEIGE/////////////////////////////////////////////////////
ares1999 34:2d7d09f9965c 2219 void ouverture_CN(void)
ares1999 34:2d7d09f9965c 2220 {
ares1999 34:2d7d09f9965c 2221 positionControl_Mul_ensemble(CN_GAUCHE, 790, PLAYTIME, RLED_ON, CN_DROITE, 240, RLED_ON, SERIAL_SPECIAL);
ares1999 34:2d7d09f9965c 2222 }
ares1999 34:2d7d09f9965c 2223
ares1999 34:2d7d09f9965c 2224 void fermeture_CN(void)
ares1999 34:2d7d09f9965c 2225 {
ares1999 34:2d7d09f9965c 2226 positionControl_Mul_ensemble(CN_GAUCHE, 240, PLAYTIME, RLED_ON, CN_DROITE, 790, RLED_ON, SERIAL_SPECIAL);
ares1999 34:2d7d09f9965c 2227 }*/