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@15:c0f578f34434, 2019-05-27 (annotated)

Committer:
Artiom
Date:
Mon May 27 19:18:43 2019 +0000
Revision:
15:c0f578f34434
Parent:
14:0eaeb3af639d
Child:
16:77e352ca3035
accelerateur avant arriere symetrie fini;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:42544b5e8b9f 1 #include "actions_Pr.h"
Artiom 0:42544b5e8b9f 2
Artiom 0:42544b5e8b9f 3 #ifdef ROBOT_SMALL
Artiom 0:42544b5e8b9f 4 #define MASK_PRESENTOIR_AV 0x07
Artiom 0:42544b5e8b9f 5 #define MASK_PRESENTOIR_AR 0x70
Artiom 0:42544b5e8b9f 6
Artiom 0:42544b5e8b9f 7 #define MASK_GOLDENIUM_AV 0x02
Artiom 0:42544b5e8b9f 8 #define MASK_GOLDENIUM_AR 0x20
Artiom 0:42544b5e8b9f 9
Artiom 0:42544b5e8b9f 10 #define MASK_SOL_AV 0x08
Artiom 0:42544b5e8b9f 11 #define MASK_SOL_AR 0x80
Artiom 0:42544b5e8b9f 12
Artiom 0:42544b5e8b9f 13 #define MASK_AV_DROIT 0x01
Artiom 0:42544b5e8b9f 14 #define MASK_AR_DROIT 0x10
Artiom 0:42544b5e8b9f 15
Artiom 1:67fe131ff7e2 16 #define MASK_AV_GAUCHE 0x04
Artiom 1:67fe131ff7e2 17 #define MASK_AR_GAUCHE 0x40
Artiom 1:67fe131ff7e2 18
Artiom 0:42544b5e8b9f 19 #define MASK_AV_DROIT_GAUCHE 0x05
Artiom 0:42544b5e8b9f 20 #define MASK_AR_DROIT_GAUCHE 0x50
Artiom 0:42544b5e8b9f 21
Artiom 12:15fd3bb5adb5 22 #define TIMEOUT_ACTION_PRESENTOIR 5000
Artiom 0:42544b5e8b9f 23 char status_pompe=0;
Artiom 0:42544b5e8b9f 24
Artiom 0:42544b5e8b9f 25
Artiom 0:42544b5e8b9f 26
Artiom 0:42544b5e8b9f 27 char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom 0:42544b5e8b9f 28 char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom 0:42544b5e8b9f 29 char fsol_avant=0,fsol_arriere=0;
Artiom 0:42544b5e8b9f 30 char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom 0:42544b5e8b9f 31 char fbalance_avant=0,fbalance_arriere=0;
Artiom 0:42544b5e8b9f 32 char favant_relache=0,farriere_relache=0;
Artiom 0:42544b5e8b9f 33 char faccelerateur_avant=0,faccelerateur_arriere=0;
Artiom 0:42544b5e8b9f 34
Artiom 12:15fd3bb5adb5 35 Timer timeout;
Artiom 0:42544b5e8b9f 36
Artiom 6:dd02e432eb8e 37
kyxstark 4:21ff54400895 38 void gabarit_robot(void)
Artiom 0:42544b5e8b9f 39 {
Artiom 0:42544b5e8b9f 40
Artiom 0:42544b5e8b9f 41 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:42544b5e8b9f 42 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:42544b5e8b9f 43 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:42544b5e8b9f 44
Artiom 0:42544b5e8b9f 45 uint16_t pos_av_gauche[2] = {520,780};//epaule,poigne520,780
Artiom 0:42544b5e8b9f 46 uint16_t pos_av_centre[2] = {550,600};//470,350
Artiom 0:42544b5e8b9f 47 uint16_t pos_av_droite[2] = {550,270};
Artiom 0:42544b5e8b9f 48
Artiom 0:42544b5e8b9f 49 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:42544b5e8b9f 50 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:42544b5e8b9f 51 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:42544b5e8b9f 52
Artiom 0:42544b5e8b9f 53 uint16_t pos_ar_gauche[2] = {530,780};
Artiom 0:42544b5e8b9f 54 uint16_t pos_ar_centre[2] = {600,500};//500,250
Artiom 0:42544b5e8b9f 55 uint16_t pos_ar_droite[2] = {430,200};
Artiom 0:42544b5e8b9f 56
Artiom 0:42544b5e8b9f 57 uint8_t servos_sol[4] = {GLED_ON, AR_sol, GLED_ON, AV_sol};
Artiom 0:42544b5e8b9f 58 uint16_t pos_sol[2] = {230,750}; //200 ,800 (90°)
Artiom 0:42544b5e8b9f 59
Artiom 0:42544b5e8b9f 60 int speed=100;
Artiom 1:67fe131ff7e2 61
Artiom 0:42544b5e8b9f 62 SendRawId(HACHEUR_RELEASE_AV);
Artiom 0:42544b5e8b9f 63 SendRawId(HACHEUR_RELEASE_AR);
Artiom 0:42544b5e8b9f 64 wait(0.5);
Artiom 0:42544b5e8b9f 65
Artiom 0:42544b5e8b9f 66 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:42544b5e8b9f 67 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:42544b5e8b9f 68 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 0:42544b5e8b9f 69
Artiom 0:42544b5e8b9f 70 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:42544b5e8b9f 71 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:42544b5e8b9f 72 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:42544b5e8b9f 73
Artiom 0:42544b5e8b9f 74 positionControl_Mul_ensemble_complex(2,speed,servos_sol, pos_sol,4);
Artiom 1:67fe131ff7e2 75
Artiom 0:42544b5e8b9f 76 verification();
Artiom 0:42544b5e8b9f 77 }
Artiom 14:0eaeb3af639d 78 /////////////////////////////////////////PRESENTOIR//////////////////////////////////////////////
Artiom 0:42544b5e8b9f 79 void presentoir_avant(void)
Artiom 0:42544b5e8b9f 80 {
Artiom 0:42544b5e8b9f 81 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:42544b5e8b9f 82 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:42544b5e8b9f 83 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:42544b5e8b9f 84
Artiom 0:42544b5e8b9f 85 uint16_t pos_av_gauche[2] = {520,490};
Artiom 0:42544b5e8b9f 86 uint16_t pos_av_centre[2] = {470,512};
Artiom 0:42544b5e8b9f 87 uint16_t pos_av_droite[2] = {550,540};
Artiom 0:42544b5e8b9f 88
Artiom 0:42544b5e8b9f 89 int speed=1;
Artiom 0:42544b5e8b9f 90
Artiom 0:42544b5e8b9f 91 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:42544b5e8b9f 92 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:42544b5e8b9f 93 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 0:42544b5e8b9f 94 verification();
Artiom 0:42544b5e8b9f 95 }
Artiom 0:42544b5e8b9f 96
Artiom 0:42544b5e8b9f 97 void presentoir_arriere(void)
Artiom 0:42544b5e8b9f 98 {
Artiom 0:42544b5e8b9f 99 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:42544b5e8b9f 100 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:42544b5e8b9f 101 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:42544b5e8b9f 102
Artiom 0:42544b5e8b9f 103 uint16_t pos_ar_gauche[2] = {530,512};
Artiom 0:42544b5e8b9f 104 uint16_t pos_ar_centre[2] = {500,430};
Artiom 0:42544b5e8b9f 105 uint16_t pos_ar_droite[2] = {430,470};
Artiom 0:42544b5e8b9f 106
Artiom 0:42544b5e8b9f 107 int speed=1;
Artiom 0:42544b5e8b9f 108
Artiom 0:42544b5e8b9f 109 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:42544b5e8b9f 110 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:42544b5e8b9f 111 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:42544b5e8b9f 112 verification();
Artiom 0:42544b5e8b9f 113
Artiom 0:42544b5e8b9f 114
Artiom 0:42544b5e8b9f 115 }
Artiom 14:0eaeb3af639d 116 /////////////////////////////////////////////////BALANCE/////////////////////////////////////////////
kyxstark 4:21ff54400895 117 void balance_avant(void)
Artiom 0:42544b5e8b9f 118 {
Artiom 1:67fe131ff7e2 119 int speed=1;
Artiom 0:42544b5e8b9f 120 if(cote==0) {
Artiom 0:42544b5e8b9f 121 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:42544b5e8b9f 122 uint16_t pos_av_droite[2] = {250,270};
Artiom 1:67fe131ff7e2 123
Artiom 0:42544b5e8b9f 124 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 0:42544b5e8b9f 125 verification();
Artiom 0:42544b5e8b9f 126 } else {
Artiom 0:42544b5e8b9f 127 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:42544b5e8b9f 128 uint16_t pos_av_gauche[2] = {800,780};
Artiom 1:67fe131ff7e2 129 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:42544b5e8b9f 130 verification();
Artiom 0:42544b5e8b9f 131 }
Artiom 0:42544b5e8b9f 132 }
kyxstark 4:21ff54400895 133 void balance_arriere(void)
Artiom 0:42544b5e8b9f 134 {
Artiom 1:67fe131ff7e2 135 int speed=1;
Artiom 0:42544b5e8b9f 136 if(cote==0) {
Artiom 0:42544b5e8b9f 137 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:42544b5e8b9f 138 uint16_t pos_ar_droite[2] = {150,200};
Artiom 0:42544b5e8b9f 139 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:42544b5e8b9f 140 verification();
Artiom 0:42544b5e8b9f 141 } else {
Artiom 1:67fe131ff7e2 142 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 1:67fe131ff7e2 143 uint16_t pos_ar_gauche[2] = {820,780};
Artiom 1:67fe131ff7e2 144 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 1:67fe131ff7e2 145 verification();
Artiom 0:42544b5e8b9f 146 }
Artiom 0:42544b5e8b9f 147 }
Artiom 0:42544b5e8b9f 148
Artiom 14:0eaeb3af639d 149 ////////////////////////////////////////////GOLDENIUM//////////////////////////////////////////
Artiom 0:42544b5e8b9f 150 void goldenium_avant(void)
Artiom 0:42544b5e8b9f 151 {
Artiom 0:42544b5e8b9f 152 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 1:67fe131ff7e2 153 uint16_t pos_av_centre[2] = {180,220};
Artiom 0:42544b5e8b9f 154 int speed=25;
Artiom 0:42544b5e8b9f 155
Artiom 0:42544b5e8b9f 156 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:42544b5e8b9f 157 verification();
Artiom 0:42544b5e8b9f 158 }
Artiom 0:42544b5e8b9f 159
Artiom 0:42544b5e8b9f 160 void goldenium_arriere(void)
Artiom 0:42544b5e8b9f 161 {
Artiom 0:42544b5e8b9f 162 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:42544b5e8b9f 163 uint16_t pos_ar_centre[2] = {230,150};
Artiom 0:42544b5e8b9f 164 int speed=25;
Artiom 0:42544b5e8b9f 165
Artiom 0:42544b5e8b9f 166 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:42544b5e8b9f 167 verification();
Artiom 0:42544b5e8b9f 168 }
Artiom 0:42544b5e8b9f 169
Artiom 14:0eaeb3af639d 170 /////////////////////////////////////////////ACCELERATEUR///////////////////////////////////////
Artiom 0:42544b5e8b9f 171 void accelerateur_avant(void)
Artiom 0:42544b5e8b9f 172 {
Artiom 0:42544b5e8b9f 173 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:42544b5e8b9f 174 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:42544b5e8b9f 175 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 0:42544b5e8b9f 176
Artiom 13:c459edbdaf9c 177 uint16_t pos_av_gauche[2] = {800,470};//800,490
Artiom 13:c459edbdaf9c 178 uint16_t pos_av_centre[2] = {200,512};
Artiom 14:0eaeb3af639d 179 uint16_t pos_av_droite[2] = {270,540};
Artiom 0:42544b5e8b9f 180
Artiom 0:42544b5e8b9f 181 int speed=25;
Artiom 0:42544b5e8b9f 182
Artiom 0:42544b5e8b9f 183 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 0:42544b5e8b9f 184 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 0:42544b5e8b9f 185 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 0:42544b5e8b9f 186
Artiom 0:42544b5e8b9f 187 verification();
Artiom 0:42544b5e8b9f 188 }
Artiom 9:40ac0603f9df 189 void accelerateur_avant_insertion(void)
Artiom 9:40ac0603f9df 190 {
Artiom 14:0eaeb3af639d 191 if(cote==0) {
Artiom 14:0eaeb3af639d 192 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 14:0eaeb3af639d 193 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 14:0eaeb3af639d 194 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 14:0eaeb3af639d 195
Artiom 14:0eaeb3af639d 196 uint16_t pos_av_gauche[2] = {750,490};//800,490
Artiom 14:0eaeb3af639d 197 uint16_t pos_av_centre[2] = {200,512};//200,220
Artiom 14:0eaeb3af639d 198 uint16_t pos_av_droite[2] = {470,540};//250,540
Artiom 14:0eaeb3af639d 199
Artiom 14:0eaeb3af639d 200 int speed=150;
Artiom 14:0eaeb3af639d 201
Artiom 14:0eaeb3af639d 202 positionControl_Mul_ensemble_complex(2,75,servos_av_gauche, pos_av_gauche,3);
Artiom 14:0eaeb3af639d 203 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 14:0eaeb3af639d 204 positionControl_Mul_ensemble_complex(2,speed,servos_av_droite, pos_av_droite,1);
Artiom 14:0eaeb3af639d 205 } else {
Artiom 9:40ac0603f9df 206
Artiom 14:0eaeb3af639d 207 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 14:0eaeb3af639d 208 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 14:0eaeb3af639d 209 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 14:0eaeb3af639d 210
Artiom 14:0eaeb3af639d 211 uint16_t pos_av_gauche[2] = {600,490};//800,490
Artiom 14:0eaeb3af639d 212 uint16_t pos_av_centre[2] = {200,512};//200,220
Artiom 14:0eaeb3af639d 213 uint16_t pos_av_droite[2] = {300,540};//250,540
Artiom 9:40ac0603f9df 214
Artiom 14:0eaeb3af639d 215 int speed=150;
Artiom 9:40ac0603f9df 216
Artiom 14:0eaeb3af639d 217 positionControl_Mul_ensemble_complex(2,75,servos_av_droite, pos_av_droite,1);
Artiom 14:0eaeb3af639d 218 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
Artiom 14:0eaeb3af639d 219 positionControl_Mul_ensemble_complex(2,speed,servos_av_gauche, pos_av_gauche,3);
Artiom 14:0eaeb3af639d 220 verification();
Artiom 14:0eaeb3af639d 221 }
Artiom 9:40ac0603f9df 222 }
Artiom 0:42544b5e8b9f 223 void accelerateur_arriere(void)
Artiom 0:42544b5e8b9f 224 {
Artiom 0:42544b5e8b9f 225 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:42544b5e8b9f 226 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:42544b5e8b9f 227 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:42544b5e8b9f 228
Artiom 0:42544b5e8b9f 229 uint16_t pos_ar_gauche[2] = {820,512};
Artiom 14:0eaeb3af639d 230 uint16_t pos_ar_centre[2] = {230,400};
Artiom 0:42544b5e8b9f 231 uint16_t pos_ar_droite[2] = {150,470};
Artiom 0:42544b5e8b9f 232
Artiom 0:42544b5e8b9f 233 int speed=25;
Artiom 0:42544b5e8b9f 234
Artiom 0:42544b5e8b9f 235 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:42544b5e8b9f 236 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 0:42544b5e8b9f 237 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 0:42544b5e8b9f 238 verification();
Artiom 0:42544b5e8b9f 239
Artiom 0:42544b5e8b9f 240 }
Artiom 13:c459edbdaf9c 241 void accelerateur_arriere_insertion(void)
Artiom 13:c459edbdaf9c 242 {
Artiom 14:0eaeb3af639d 243 if(cote==0) {
Artiom 14:0eaeb3af639d 244 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 14:0eaeb3af639d 245 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 14:0eaeb3af639d 246 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 0:42544b5e8b9f 247
Artiom 14:0eaeb3af639d 248 uint16_t pos_ar_gauche[2] = {770,512};//820,512
Artiom 14:0eaeb3af639d 249 uint16_t pos_ar_centre[2] = {230,400};//230,150
Artiom 14:0eaeb3af639d 250 uint16_t pos_ar_droite[2] = {300,470};//150,470
Artiom 14:0eaeb3af639d 251
Artiom 14:0eaeb3af639d 252 int speed=150;
Artiom 0:42544b5e8b9f 253
Artiom 14:0eaeb3af639d 254 positionControl_Mul_ensemble_complex(2,75,servos_ar_gauche, pos_ar_gauche,1);
Artiom 14:0eaeb3af639d 255 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 14:0eaeb3af639d 256 positionControl_Mul_ensemble_complex(2,speed,servos_ar_droite, pos_ar_droite,3);
Artiom 14:0eaeb3af639d 257 } else {
Artiom 14:0eaeb3af639d 258 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 14:0eaeb3af639d 259 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 14:0eaeb3af639d 260 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 13:c459edbdaf9c 261
Artiom 15:c0f578f34434 262 uint16_t pos_ar_gauche[2] = {630,512};//820,512
Artiom 14:0eaeb3af639d 263 uint16_t pos_ar_centre[2] = {230,400};//230,150
Artiom 14:0eaeb3af639d 264 uint16_t pos_ar_droite[2] = {250,470};//150,470
Artiom 14:0eaeb3af639d 265
Artiom 14:0eaeb3af639d 266 int speed=150;
Artiom 14:0eaeb3af639d 267
Artiom 14:0eaeb3af639d 268 positionControl_Mul_ensemble_complex(2,75,servos_ar_droite, pos_ar_droite,3);
Artiom 14:0eaeb3af639d 269 positionControl_Mul_ensemble_complex(2,speed,servos_ar_centre, pos_ar_centre,2);
Artiom 14:0eaeb3af639d 270 positionControl_Mul_ensemble_complex(2,speed,servos_ar_gauche, pos_ar_gauche,1);
Artiom 14:0eaeb3af639d 271
Artiom 14:0eaeb3af639d 272
Artiom 13:c459edbdaf9c 273 }
Artiom 14:0eaeb3af639d 274 }
Artiom 14:0eaeb3af639d 275 ////////////////////////////////////////SOL//////////////////////////////////////////////////////////////
Artiom 0:42544b5e8b9f 276 void sol_avant_baisser(void)
Artiom 0:42544b5e8b9f 277 {
Artiom 0:42544b5e8b9f 278 int speed=1;
Artiom 0:42544b5e8b9f 279 positionControl(AV_sol,480,speed,BLED_ON,4);//descend
Artiom 0:42544b5e8b9f 280 }
Artiom 14:0eaeb3af639d 281
Artiom 0:42544b5e8b9f 282 void sol_avant_remonter(void)
Artiom 0:42544b5e8b9f 283 {
Artiom 0:42544b5e8b9f 284 int speed=1;
Artiom 0:42544b5e8b9f 285 positionControl(AV_sol,800,speed,BLED_ON,4);//remonte
Artiom 0:42544b5e8b9f 286 }
Artiom 0:42544b5e8b9f 287
Artiom 0:42544b5e8b9f 288 void sol_arriere_baisser(void)
Artiom 0:42544b5e8b9f 289 {
Artiom 0:42544b5e8b9f 290 int speed=1;
Artiom 0:42544b5e8b9f 291 positionControl(AR_sol,535,speed,BLED_ON,4);//descend
Artiom 0:42544b5e8b9f 292 }
Artiom 14:0eaeb3af639d 293
Artiom 0:42544b5e8b9f 294 void sol_arriere_remonter(void)
Artiom 0:42544b5e8b9f 295 {
Artiom 0:42544b5e8b9f 296 int speed=1;
Artiom 0:42544b5e8b9f 297 positionControl(AR_sol,200,speed,BLED_ON,4);//remonte
Artiom 0:42544b5e8b9f 298 }
Artiom 14:0eaeb3af639d 299 ///////////////////////////////////////////////////RECROQUEVILLER////////////////////////////////////////////////
Artiom 0:42544b5e8b9f 300
Artiom 0:42544b5e8b9f 301 void recroqueviller_avant(void)
Artiom 0:42544b5e8b9f 302 {
Artiom 0:42544b5e8b9f 303 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 0:42544b5e8b9f 304 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:42544b5e8b9f 305 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 6:dd02e432eb8e 306 uint16_t pos_av_gauche[2] = {370,490};//350,490
Artiom 6:dd02e432eb8e 307 uint16_t pos_av_centre[2] = {270,320};//180,220
Artiom 6:dd02e432eb8e 308 uint16_t pos_av_droite[2] = {700,540};//700,540
Artiom 0:42544b5e8b9f 309
Artiom 0:42544b5e8b9f 310 positionControl_Mul_ensemble_complex(2,40,servos_av_gauche, pos_av_gauche,3);
Artiom 0:42544b5e8b9f 311 positionControl_Mul_ensemble_complex(2,1,servos_av_centre, pos_av_centre,2);
Artiom 0:42544b5e8b9f 312 positionControl_Mul_ensemble_complex(2,25,servos_av_droite, pos_av_droite,1);
Artiom 0:42544b5e8b9f 313 }
Artiom 0:42544b5e8b9f 314
Artiom 0:42544b5e8b9f 315 void recroqueviller_arriere(void)
Artiom 0:42544b5e8b9f 316 {
Artiom 0:42544b5e8b9f 317 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 0:42544b5e8b9f 318 uint8_t servos_ar_centre[4] = {GLED_ON, AR_EP_C, GLED_ON, AR_poigne_C};
Artiom 0:42544b5e8b9f 319 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 6:dd02e432eb8e 320 uint16_t pos_ar_gauche[2] = {400,510};//400,490
Artiom 6:dd02e432eb8e 321 uint16_t pos_ar_centre[2] = {310,220};//230,150
Artiom 6:dd02e432eb8e 322 uint16_t pos_ar_droite[2] = {550,470};
Artiom 0:42544b5e8b9f 323
Artiom 0:42544b5e8b9f 324 positionControl_Mul_ensemble_complex(2,40,servos_ar_gauche, pos_ar_gauche,1);
Artiom 0:42544b5e8b9f 325 positionControl_Mul_ensemble_complex(2,1,servos_ar_centre, pos_ar_centre,2);
Artiom 0:42544b5e8b9f 326 positionControl_Mul_ensemble_complex(2,25,servos_ar_droite, pos_ar_droite,3);
Artiom 0:42544b5e8b9f 327 }
Artiom 14:0eaeb3af639d 328 //////////////////////////////////ACCELERATEUR POUSSER PALET///////////////////////////////////////////////////
kyxstark 4:21ff54400895 329 void accelerateur_insertion_avant_gauche(void)
Artiom 0:42544b5e8b9f 330 {
Artiom 2:e667255cd5b0 331 if(cote==0) {
Artiom 2:e667255cd5b0 332 uint8_t servos_av_gauche[4] = {RLED_ON, AV_EP_G, RLED_ON, AV_poigne_G};
Artiom 2:e667255cd5b0 333 uint16_t pos_av_gauche[2] = {800,780};
Artiom 2:e667255cd5b0 334 positionControl_Mul_ensemble_complex(2,1,servos_av_gauche, pos_av_gauche,3);
Artiom 2:e667255cd5b0 335 } else {
Artiom 2:e667255cd5b0 336 uint8_t servos_av_droite[4] = {BLED_ON, AV_EP_D, BLED_ON, AV_poigne_D};
Artiom 2:e667255cd5b0 337 uint16_t pos_av_droite[2] = {250,270};
Artiom 2:e667255cd5b0 338 positionControl_Mul_ensemble_complex(2,25,servos_av_droite, pos_av_droite,1);
Artiom 2:e667255cd5b0 339 }
Artiom 0:42544b5e8b9f 340 verification();
Artiom 0:42544b5e8b9f 341 }
Artiom 0:42544b5e8b9f 342
kyxstark 4:21ff54400895 343 void accelerateur_insertion_arriere_gauche(void)
Artiom 0:42544b5e8b9f 344 {
Artiom 2:e667255cd5b0 345 if(cote==0) {
Artiom 2:e667255cd5b0 346 uint8_t servos_ar_gauche[4] = {RLED_ON, AR_EP_G, RLED_ON, AR_poigne_G};
Artiom 2:e667255cd5b0 347 uint16_t pos_ar_gauche[2] = {820,780};
Artiom 2:e667255cd5b0 348 positionControl_Mul_ensemble_complex(2,1,servos_ar_gauche, pos_ar_gauche,1);
Artiom 2:e667255cd5b0 349 } else {
Artiom 2:e667255cd5b0 350 uint8_t servos_ar_droite[4] = {BLED_ON, AR_EP_D, BLED_ON, AR_poigne_D};
Artiom 2:e667255cd5b0 351 uint16_t pos_ar_droite[2] = {150,200};
Artiom 2:e667255cd5b0 352 positionControl_Mul_ensemble_complex(2,25,servos_ar_droite, pos_ar_droite,3);
Artiom 2:e667255cd5b0 353 }
Artiom 0:42544b5e8b9f 354 verification();
Artiom 0:42544b5e8b9f 355 }
Artiom 0:42544b5e8b9f 356
Artiom 14:0eaeb3af639d 357 ///////////////////////////////////////////////AUTOMATE PRESENTOIR////////////////////////////////////
Artiom 0:42544b5e8b9f 358 void automate_ventouse_presentoir_avant (void)
Artiom 0:42544b5e8b9f 359 {
Artiom 0:42544b5e8b9f 360 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 361 static type_etat etat = init;
Artiom 14:0eaeb3af639d 362
Artiom 0:42544b5e8b9f 363 switch(etat) {
Artiom 0:42544b5e8b9f 364 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 365 if(fpresentoir_avant)
Artiom 0:42544b5e8b9f 366 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 367 break;
Artiom 0:42544b5e8b9f 368
Artiom 0:42544b5e8b9f 369 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 370 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 371 presentoir_avant();
Artiom 0:42544b5e8b9f 372 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
Artiom 0:42544b5e8b9f 373 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 374 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 375 break;
Artiom 0:42544b5e8b9f 376
Artiom 0:42544b5e8b9f 377 case attente_ack_ventouse:
Artiom 12:15fd3bb5adb5 378 if(((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV)) {
Artiom 0:42544b5e8b9f 379 fpresentoir_avant=0;
Artiom 0:42544b5e8b9f 380 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 381 etat = init;
Artiom 0:42544b5e8b9f 382 }
Artiom 0:42544b5e8b9f 383 break;
Artiom 0:42544b5e8b9f 384 }
Artiom 0:42544b5e8b9f 385 }
Artiom 14:0eaeb3af639d 386
Artiom 0:42544b5e8b9f 387 void automate_ventouse_presentoir_arriere (void)
Artiom 0:42544b5e8b9f 388 {
Artiom 0:42544b5e8b9f 389
Artiom 0:42544b5e8b9f 390 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 391 static type_etat etat = init;
Artiom 0:42544b5e8b9f 392
Artiom 0:42544b5e8b9f 393 switch(etat) {
Artiom 0:42544b5e8b9f 394 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 395 if(fpresentoir_arriere)
Artiom 0:42544b5e8b9f 396 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 397 break;
Artiom 0:42544b5e8b9f 398
Artiom 0:42544b5e8b9f 399 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 400 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 401 presentoir_arriere();
Artiom 0:42544b5e8b9f 402 SendRawId(HACHEUR_GET_PRESENTOIR_AR);
Artiom 0:42544b5e8b9f 403 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 404 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 405 break;
Artiom 0:42544b5e8b9f 406
Artiom 0:42544b5e8b9f 407 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 408 if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
Artiom 0:42544b5e8b9f 409 fpresentoir_arriere=0;
Artiom 0:42544b5e8b9f 410 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 411 etat = init;
Artiom 0:42544b5e8b9f 412 }
Artiom 0:42544b5e8b9f 413 break;
Artiom 0:42544b5e8b9f 414 }
Artiom 0:42544b5e8b9f 415 }
Artiom 14:0eaeb3af639d 416 //////////////////////////////////////////////AUTOMATE GOLDENIUM////////////////////////////////////////
Artiom 0:42544b5e8b9f 417 void automate_ventouse_goldenium_avant (void)
Artiom 0:42544b5e8b9f 418 {
Artiom 0:42544b5e8b9f 419
Artiom 0:42544b5e8b9f 420 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 421 static type_etat etat = init;
Artiom 0:42544b5e8b9f 422
Artiom 0:42544b5e8b9f 423 switch(etat) {
Artiom 0:42544b5e8b9f 424 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 425 if(fgoldenium_avant)
Artiom 0:42544b5e8b9f 426 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 427 break;
Artiom 0:42544b5e8b9f 428
Artiom 0:42544b5e8b9f 429 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 430 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 431 goldenium_avant();
Artiom 0:42544b5e8b9f 432 char pompe=AV_CENTRE;
Artiom 0:42544b5e8b9f 433 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 0:42544b5e8b9f 434 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 6:dd02e432eb8e 435 GoStraight(distance_goldenium,0,0,0);
Artiom 0:42544b5e8b9f 436 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 437 break;
Artiom 0:42544b5e8b9f 438
Artiom 0:42544b5e8b9f 439 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 440 if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
Artiom 5:37015374ef10 441 wait(1);
Artiom 6:dd02e432eb8e 442 GoStraight(-distance_goldenium,0,0,0);
Artiom 5:37015374ef10 443 positionControl(AV_poigne_C,512,100,BLED_ON,2);//forklift
Artiom 3:cfd2b0d98c42 444 verification();
Artiom 0:42544b5e8b9f 445 fgoldenium_avant=0;
Artiom 0:42544b5e8b9f 446 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 447 etat = init;
Artiom 0:42544b5e8b9f 448 }
Artiom 0:42544b5e8b9f 449 break;
Artiom 0:42544b5e8b9f 450 }
Artiom 0:42544b5e8b9f 451 }
Artiom 0:42544b5e8b9f 452
Artiom 0:42544b5e8b9f 453 void automate_ventouse_goldenium_arriere (void)
Artiom 0:42544b5e8b9f 454 {
Artiom 0:42544b5e8b9f 455
Artiom 0:42544b5e8b9f 456 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 457 static type_etat etat = init;
Artiom 0:42544b5e8b9f 458
Artiom 0:42544b5e8b9f 459 switch(etat) {
Artiom 0:42544b5e8b9f 460 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 461 if(fgoldenium_arriere)
Artiom 0:42544b5e8b9f 462 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 463 break;
Artiom 0:42544b5e8b9f 464
Artiom 0:42544b5e8b9f 465 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 466 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 467 goldenium_arriere();
Artiom 0:42544b5e8b9f 468 char pompe=AR_CENTRE;
Artiom 0:42544b5e8b9f 469 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 0:42544b5e8b9f 470 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 5:37015374ef10 471 GoStraight(-100,0,0,0);
Artiom 0:42544b5e8b9f 472 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 473 break;
Artiom 0:42544b5e8b9f 474
Artiom 0:42544b5e8b9f 475 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 476 if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
Artiom 5:37015374ef10 477 wait(1);
Artiom 5:37015374ef10 478 GoStraight(100,0,0,0);
Artiom 5:37015374ef10 479 positionControl(AR_poigne_C,430,100,BLED_ON,2);//forklift
Artiom 6:dd02e432eb8e 480 verification();
Artiom 0:42544b5e8b9f 481 fgoldenium_arriere=0;
Artiom 0:42544b5e8b9f 482 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 483 etat = init;
Artiom 0:42544b5e8b9f 484 }
Artiom 0:42544b5e8b9f 485 break;
Artiom 0:42544b5e8b9f 486 }
Artiom 0:42544b5e8b9f 487 }
Artiom 14:0eaeb3af639d 488 ////////////////////////////////////////////////////AUTOMATE SOL///////////////////////////////////
Artiom 0:42544b5e8b9f 489 void automate_ventouse_sol_avant (void)
Artiom 0:42544b5e8b9f 490 {
Artiom 0:42544b5e8b9f 491
Artiom 0:42544b5e8b9f 492 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 493 static type_etat etat = init;
Artiom 0:42544b5e8b9f 494
Artiom 0:42544b5e8b9f 495 switch(etat) {
Artiom 0:42544b5e8b9f 496 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 497 if(fsol_avant)
Artiom 0:42544b5e8b9f 498 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 499 break;
Artiom 0:42544b5e8b9f 500
Artiom 0:42544b5e8b9f 501 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 502 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 503 sol_avant_baisser();
Artiom 0:42544b5e8b9f 504 char pompe=AV_BAS;
Artiom 0:42544b5e8b9f 505 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 0:42544b5e8b9f 506 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 507 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 508 break;
Artiom 0:42544b5e8b9f 509
Artiom 0:42544b5e8b9f 510 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 511 if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
Artiom 0:42544b5e8b9f 512 sol_avant_remonter();
Artiom 0:42544b5e8b9f 513 fsol_avant=0;
Artiom 0:42544b5e8b9f 514 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 515 etat = init;
Artiom 0:42544b5e8b9f 516 }
Artiom 0:42544b5e8b9f 517 break;
Artiom 0:42544b5e8b9f 518 }
Artiom 14:0eaeb3af639d 519 }
Artiom 0:42544b5e8b9f 520
Artiom 0:42544b5e8b9f 521 void automate_ventouse_sol_arriere (void)
Artiom 0:42544b5e8b9f 522 {
Artiom 0:42544b5e8b9f 523
Artiom 0:42544b5e8b9f 524 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 525 static type_etat etat = init;
Artiom 0:42544b5e8b9f 526
Artiom 0:42544b5e8b9f 527 switch(etat) {
Artiom 0:42544b5e8b9f 528 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 529 if(fsol_arriere)
Artiom 0:42544b5e8b9f 530 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 531 break;
Artiom 0:42544b5e8b9f 532
Artiom 0:42544b5e8b9f 533 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 534 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 535 sol_arriere_baisser();
Artiom 0:42544b5e8b9f 536 char pompe=AR_BAS;
Artiom 0:42544b5e8b9f 537 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
Artiom 0:42544b5e8b9f 538 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 539 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 540 break;
Artiom 0:42544b5e8b9f 541
Artiom 0:42544b5e8b9f 542 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 543 if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
Artiom 0:42544b5e8b9f 544 sol_arriere_remonter();
Artiom 0:42544b5e8b9f 545 fsol_arriere=0;
Artiom 0:42544b5e8b9f 546 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 547 etat = init;
Artiom 0:42544b5e8b9f 548 }
Artiom 0:42544b5e8b9f 549 break;
Artiom 14:0eaeb3af639d 550 }
Artiom 14:0eaeb3af639d 551 }
Artiom 14:0eaeb3af639d 552 void automate_ventouse_sol_avant_relache (void)
Artiom 14:0eaeb3af639d 553 {
Artiom 14:0eaeb3af639d 554 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 14:0eaeb3af639d 555 static type_etat etat = init;
Artiom 0:42544b5e8b9f 556
Artiom 14:0eaeb3af639d 557 switch(etat) {
Artiom 14:0eaeb3af639d 558 case init: //attente d'initialisation
Artiom 14:0eaeb3af639d 559 if(fsol_avant_relache)
Artiom 14:0eaeb3af639d 560 etat=envoi_instruction;
Artiom 14:0eaeb3af639d 561 break;
Artiom 14:0eaeb3af639d 562
Artiom 14:0eaeb3af639d 563 case envoi_instruction://envoi instruction
Artiom 14:0eaeb3af639d 564 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:0eaeb3af639d 565 positionControl(AV_sol,520,1,BLED_ON,4);//baisser
Artiom 14:0eaeb3af639d 566 verification();
Artiom 14:0eaeb3af639d 567 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 14:0eaeb3af639d 568 etat = attente_ack_ventouse;
Artiom 14:0eaeb3af639d 569 break;
Artiom 14:0eaeb3af639d 570
Artiom 14:0eaeb3af639d 571 case attente_ack_ventouse:
Artiom 14:0eaeb3af639d 572 char pompe=AV_BAS;
Artiom 14:0eaeb3af639d 573 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:0eaeb3af639d 574 if((status_pompe&MASK_SOL_AV)== 0) {
Artiom 14:0eaeb3af639d 575 sol_avant_remonter();
Artiom 14:0eaeb3af639d 576 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:0eaeb3af639d 577 fsol_avant_relache=0;
Artiom 14:0eaeb3af639d 578 etat = init;
Artiom 14:0eaeb3af639d 579 }
Artiom 14:0eaeb3af639d 580 break;
Artiom 0:42544b5e8b9f 581 }
Artiom 0:42544b5e8b9f 582
Artiom 14:0eaeb3af639d 583 }
Artiom 0:42544b5e8b9f 584
Artiom 14:0eaeb3af639d 585 void automate_ventouse_sol_arriere_relache (void)
Artiom 14:0eaeb3af639d 586 {
Artiom 14:0eaeb3af639d 587 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 14:0eaeb3af639d 588 static type_etat etat = init;
Artiom 14:0eaeb3af639d 589
Artiom 14:0eaeb3af639d 590 switch(etat) {
Artiom 14:0eaeb3af639d 591 case init: //attente d'initialisation
Artiom 14:0eaeb3af639d 592 if(fsol_arriere_relache)
Artiom 14:0eaeb3af639d 593 etat=envoi_instruction;
Artiom 14:0eaeb3af639d 594 break;
Artiom 14:0eaeb3af639d 595
Artiom 14:0eaeb3af639d 596 case envoi_instruction://envoi instruction
Artiom 14:0eaeb3af639d 597 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 14:0eaeb3af639d 598 positionControl(AR_sol,480,1,BLED_ON,4);//baisser
Artiom 14:0eaeb3af639d 599 verification();
Artiom 14:0eaeb3af639d 600 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 14:0eaeb3af639d 601 etat = attente_ack_ventouse;
Artiom 14:0eaeb3af639d 602 break;
Artiom 14:0eaeb3af639d 603
Artiom 14:0eaeb3af639d 604 case attente_ack_ventouse:
Artiom 14:0eaeb3af639d 605 char pompe=AR_BAS;
Artiom 14:0eaeb3af639d 606 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 14:0eaeb3af639d 607 if((status_pompe&MASK_SOL_AR)== 0) {
Artiom 14:0eaeb3af639d 608 sol_arriere_remonter();
Artiom 14:0eaeb3af639d 609 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 14:0eaeb3af639d 610 fsol_arriere_relache=0;
Artiom 14:0eaeb3af639d 611 etat = init;
Artiom 14:0eaeb3af639d 612 }
Artiom 14:0eaeb3af639d 613 break;
Artiom 14:0eaeb3af639d 614 }
Artiom 0:42544b5e8b9f 615 }
Artiom 14:0eaeb3af639d 616 /////////////////////////////////////////////////////AUTOMATE BALANCE////////////////////////////////////////
Artiom 0:42544b5e8b9f 617 void automate_ventouse_balance_avant (void)
Artiom 0:42544b5e8b9f 618 {
Artiom 1:67fe131ff7e2 619 typedef enum {init,envoi_instruction,attente_ack_ventouse,reset} type_etat;
Artiom 0:42544b5e8b9f 620 static type_etat etat = init;
Artiom 2:e667255cd5b0 621 static char pompe;
Artiom 0:42544b5e8b9f 622 switch(etat) {
Artiom 0:42544b5e8b9f 623 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 624 if(fbalance_avant)
Artiom 0:42544b5e8b9f 625 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 626 break;
Artiom 0:42544b5e8b9f 627
Artiom 0:42544b5e8b9f 628 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 629 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 630 presentoir_avant();
Artiom 0:42544b5e8b9f 631 recroqueviller_arriere();
Artiom 0:42544b5e8b9f 632 //verification();
Artiom 6:dd02e432eb8e 633 char angle=200;
Artiom 1:67fe131ff7e2 634 if(cote==0) {
Artiom 6:dd02e432eb8e 635 Rotate(angle);
Artiom 6:dd02e432eb8e 636 wait(0.5);
kyxstark 4:21ff54400895 637 balance_avant();
Artiom 6:dd02e432eb8e 638 Rotate(-angle);
Artiom 6:dd02e432eb8e 639 wait(0.5);
Artiom 1:67fe131ff7e2 640 } else {
Artiom 6:dd02e432eb8e 641 Rotate(-angle);
Artiom 6:dd02e432eb8e 642 wait(0.5);
kyxstark 4:21ff54400895 643 balance_avant();
Artiom 6:dd02e432eb8e 644 Rotate(angle);
Artiom 6:dd02e432eb8e 645 wait(0.5);
Artiom 1:67fe131ff7e2 646 }
Artiom 0:42544b5e8b9f 647 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 648 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 649 break;
Artiom 0:42544b5e8b9f 650
Artiom 0:42544b5e8b9f 651 case attente_ack_ventouse:
Artiom 2:e667255cd5b0 652
Artiom 1:67fe131ff7e2 653 if(cote==0) {
Artiom 1:67fe131ff7e2 654 pompe=AV_DROIT;
Artiom 1:67fe131ff7e2 655 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 1:67fe131ff7e2 656 if((status_pompe&MASK_AV_DROIT)== 0) etat = reset;
Artiom 1:67fe131ff7e2 657 } else {
Artiom 1:67fe131ff7e2 658 pompe=AV_GAUCHE;
Artiom 1:67fe131ff7e2 659 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 1:67fe131ff7e2 660 if((status_pompe&MASK_AV_GAUCHE)== 0) etat = reset;
Artiom 0:42544b5e8b9f 661 }
Artiom 0:42544b5e8b9f 662 break;
Artiom 0:42544b5e8b9f 663
Artiom 1:67fe131ff7e2 664 case reset:
Artiom 1:67fe131ff7e2 665 fbalance_avant=0;
Artiom 1:67fe131ff7e2 666 presentoir_arriere();
Artiom 1:67fe131ff7e2 667 presentoir_avant();
Artiom 1:67fe131ff7e2 668 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 1:67fe131ff7e2 669 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 1:67fe131ff7e2 670 etat = init;
Artiom 1:67fe131ff7e2 671 break;
Artiom 0:42544b5e8b9f 672 }
Artiom 14:0eaeb3af639d 673 }
Artiom 0:42544b5e8b9f 674
Artiom 0:42544b5e8b9f 675 void automate_ventouse_balance_arriere (void)
Artiom 0:42544b5e8b9f 676 {
Artiom 1:67fe131ff7e2 677 typedef enum {init,envoi_instruction,attente_ack_ventouse,reset} type_etat;
Artiom 0:42544b5e8b9f 678 static type_etat etat = init;
Artiom 2:e667255cd5b0 679 char pompe;
Artiom 0:42544b5e8b9f 680 switch(etat) {
Artiom 0:42544b5e8b9f 681 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 682 if(fbalance_arriere)
Artiom 0:42544b5e8b9f 683 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 684 break;
Artiom 0:42544b5e8b9f 685
Artiom 0:42544b5e8b9f 686 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 687 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 688 presentoir_arriere();
Artiom 0:42544b5e8b9f 689 recroqueviller_avant();
Artiom 0:42544b5e8b9f 690 //verification();
Artiom 6:dd02e432eb8e 691 char angle=200;
Artiom 1:67fe131ff7e2 692 if(cote==0) {
Artiom 6:dd02e432eb8e 693 Rotate(angle);
Artiom 6:dd02e432eb8e 694 wait(0.5);
kyxstark 4:21ff54400895 695 balance_arriere();
Artiom 6:dd02e432eb8e 696 Rotate(-angle);
Artiom 6:dd02e432eb8e 697 wait(0.5);
Artiom 1:67fe131ff7e2 698 } else {
Artiom 6:dd02e432eb8e 699 Rotate(-angle);
Artiom 6:dd02e432eb8e 700 wait(0.5);
kyxstark 4:21ff54400895 701 balance_arriere();
Artiom 6:dd02e432eb8e 702 Rotate(angle);
Artiom 6:dd02e432eb8e 703 wait(0.5);
Artiom 1:67fe131ff7e2 704 }
Artiom 0:42544b5e8b9f 705 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 706 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 707 break;
Artiom 0:42544b5e8b9f 708
Artiom 0:42544b5e8b9f 709 case attente_ack_ventouse:
Artiom 1:67fe131ff7e2 710 if(cote==0) {
Artiom 1:67fe131ff7e2 711 pompe=AR_DROIT;
Artiom 1:67fe131ff7e2 712 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 1:67fe131ff7e2 713 if((status_pompe&MASK_AR_DROIT)== 0) etat=reset;
Artiom 1:67fe131ff7e2 714 } else {
Artiom 1:67fe131ff7e2 715 pompe=AR_GAUCHE;
Artiom 1:67fe131ff7e2 716 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 1:67fe131ff7e2 717 if((status_pompe&MASK_AR_GAUCHE)== 0) etat=reset;
Artiom 0:42544b5e8b9f 718 }
Artiom 0:42544b5e8b9f 719 break;
Artiom 0:42544b5e8b9f 720
Artiom 1:67fe131ff7e2 721 case reset:
Artiom 1:67fe131ff7e2 722 fbalance_arriere=0;
Artiom 1:67fe131ff7e2 723 presentoir_arriere();
Artiom 1:67fe131ff7e2 724 presentoir_avant();
Artiom 1:67fe131ff7e2 725 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
Artiom 1:67fe131ff7e2 726 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 1:67fe131ff7e2 727 etat = init;
Artiom 1:67fe131ff7e2 728 break;
Artiom 0:42544b5e8b9f 729 }
Artiom 0:42544b5e8b9f 730 }
Artiom 14:0eaeb3af639d 731 ///////////////////////////////////////////////////AUTOMATE RELACHE FACE//////////////////////////////////////////
Artiom 0:42544b5e8b9f 732 void automate_ventouse_relache_avant (void)
Artiom 0:42544b5e8b9f 733 {
Artiom 0:42544b5e8b9f 734 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 735 static type_etat etat = init;
Artiom 0:42544b5e8b9f 736
Artiom 0:42544b5e8b9f 737 switch(etat) {
Artiom 0:42544b5e8b9f 738 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 739 if(favant_relache)
Artiom 0:42544b5e8b9f 740 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 741 break;
Artiom 0:42544b5e8b9f 742
Artiom 0:42544b5e8b9f 743 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 744 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 745 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 746 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 747 break;
Artiom 0:42544b5e8b9f 748
Artiom 0:42544b5e8b9f 749 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 750 SendRawId(HACHEUR_RELEASE_AV);
Artiom 0:42544b5e8b9f 751 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
Artiom 0:42544b5e8b9f 752 favant_relache=0;
Artiom 0:42544b5e8b9f 753 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 754 etat = init;
Artiom 0:42544b5e8b9f 755 }
Artiom 0:42544b5e8b9f 756 break;
Artiom 0:42544b5e8b9f 757 }
Artiom 14:0eaeb3af639d 758 }
Artiom 0:42544b5e8b9f 759
Artiom 0:42544b5e8b9f 760 void automate_ventouse_relache_arriere (void)
Artiom 0:42544b5e8b9f 761 {
Artiom 0:42544b5e8b9f 762 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
Artiom 0:42544b5e8b9f 763 static type_etat etat = init;
Artiom 0:42544b5e8b9f 764
Artiom 0:42544b5e8b9f 765 switch(etat) {
Artiom 0:42544b5e8b9f 766 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 767 if(farriere_relache)
Artiom 0:42544b5e8b9f 768 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 769 break;
Artiom 0:42544b5e8b9f 770
Artiom 0:42544b5e8b9f 771 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 772 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 773 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 774 etat = attente_ack_ventouse;
Artiom 0:42544b5e8b9f 775 break;
Artiom 0:42544b5e8b9f 776
Artiom 0:42544b5e8b9f 777 case attente_ack_ventouse:
Artiom 0:42544b5e8b9f 778 SendRawId(HACHEUR_RELEASE_AR);
Artiom 0:42544b5e8b9f 779 if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
Artiom 0:42544b5e8b9f 780 farriere_relache=0;
Artiom 0:42544b5e8b9f 781 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 782 etat = init;
Artiom 0:42544b5e8b9f 783 }
Artiom 0:42544b5e8b9f 784 break;
Artiom 0:42544b5e8b9f 785 }
Artiom 0:42544b5e8b9f 786
Artiom 0:42544b5e8b9f 787 }
Artiom 14:0eaeb3af639d 788 ////////////////////////////////////////////////////////AUTOMATE LACHER DANS L'ACCELERATEUR/////////////////////////////////////////
Artiom 0:42544b5e8b9f 789 void automate_ventouse_accelerateur_avant (void)
Artiom 0:42544b5e8b9f 790 {
Artiom 9:40ac0603f9df 791 typedef enum {init,envoi_instruction,insertion,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom 0:42544b5e8b9f 792 static type_etat etat = init;
Artiom 6:dd02e432eb8e 793 static char pompe;
Artiom 13:c459edbdaf9c 794 static char distance_palet_milieu=35;
Artiom 0:42544b5e8b9f 795 switch(etat) {
Artiom 0:42544b5e8b9f 796 case init: //attente d'initialisation
Artiom 0:42544b5e8b9f 797 if(faccelerateur_avant)
Artiom 0:42544b5e8b9f 798 etat=envoi_instruction;
Artiom 0:42544b5e8b9f 799 break;
Artiom 0:42544b5e8b9f 800
Artiom 0:42544b5e8b9f 801 case envoi_instruction://envoi instruction
Artiom 0:42544b5e8b9f 802 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 0:42544b5e8b9f 803 accelerateur_avant();
Artiom 6:dd02e432eb8e 804 GoStraight(-100,1,distance_recalage,0);//100 distance recalage,1->reclage en x 2->reclage en y
Artiom 6:dd02e432eb8e 805 wait(3);
Artiom 9:40ac0603f9df 806 accelerateur_avant_insertion();
Artiom 9:40ac0603f9df 807 etat=insertion;
Artiom 9:40ac0603f9df 808 break;
Artiom 9:40ac0603f9df 809 case insertion:
Artiom 11:3288ca4adb8f 810 verification();
Artiom 11:3288ca4adb8f 811 pompe=AV_DROIT;
Artiom 11:3288ca4adb8f 812 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 11:3288ca4adb8f 813 pompe=AV_GAUCHE;
Artiom 11:3288ca4adb8f 814 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 11:3288ca4adb8f 815 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 11:3288ca4adb8f 816 etat = attente_ack_ventouse_droite_gauche;
Artiom 11:3288ca4adb8f 817
Artiom 0:42544b5e8b9f 818 break;
Artiom 0:42544b5e8b9f 819
Artiom 0:42544b5e8b9f 820 case attente_ack_ventouse_droite_gauche:
Artiom 0:42544b5e8b9f 821 if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
Artiom 6:dd02e432eb8e 822 GoStraight(-distance_palet_milieu,0,0,0);
Artiom 0:42544b5e8b9f 823 wait(1);
Artiom 13:c459edbdaf9c 824 positionControl(AV_poigne_C,220,100,BLED_ON,2);
Artiom 9:40ac0603f9df 825 verification();
Artiom 6:dd02e432eb8e 826 pompe=AV_CENTRE;
Artiom 6:dd02e432eb8e 827 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 6:dd02e432eb8e 828 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 0:42544b5e8b9f 829 etat = attente_ack_ventouse_centre;
Artiom 0:42544b5e8b9f 830 }
Artiom 0:42544b5e8b9f 831 case attente_ack_ventouse_centre:
Artiom 6:dd02e432eb8e 832 if((status_pompe&MASK_GOLDENIUM_AV)== 0) {
Artiom 6:dd02e432eb8e 833 GoStraight(-(distance_revenir-(distance_recalage+distance_palet_milieu)),0,0,0);
Artiom 6:dd02e432eb8e 834 wait(3);
Artiom 14:0eaeb3af639d 835 presentoir_avant();
Artiom 14:0eaeb3af639d 836 SendRawId(HACHEUR_RELEASE_AV);
Artiom 14:0eaeb3af639d 837 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 0:42544b5e8b9f 838 faccelerateur_avant=0;
Artiom 0:42544b5e8b9f 839 etat = init;
Artiom 0:42544b5e8b9f 840 }
Artiom 0:42544b5e8b9f 841 break;
Artiom 0:42544b5e8b9f 842 }
Artiom 0:42544b5e8b9f 843 }
Artiom 0:42544b5e8b9f 844
Artiom 0:42544b5e8b9f 845
Artiom 0:42544b5e8b9f 846 void automate_ventouse_accelerateur_arriere (void)
Artiom 0:42544b5e8b9f 847 {
Artiom 13:c459edbdaf9c 848 typedef enum {init,envoi_instruction,insertion,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
Artiom 13:c459edbdaf9c 849 static type_etat etat = init;
Artiom 13:c459edbdaf9c 850 static char pompe;
Artiom 14:0eaeb3af639d 851 static char distance_palet_milieu=40;
Artiom 13:c459edbdaf9c 852 switch(etat) {
Artiom 13:c459edbdaf9c 853 case init: //attente d'initialisation
Artiom 13:c459edbdaf9c 854 if(faccelerateur_arriere)
Artiom 13:c459edbdaf9c 855 etat=envoi_instruction;
Artiom 13:c459edbdaf9c 856 break;
Artiom 13:c459edbdaf9c 857
Artiom 13:c459edbdaf9c 858 case envoi_instruction://envoi instruction
Artiom 13:c459edbdaf9c 859 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
Artiom 13:c459edbdaf9c 860 accelerateur_arriere();
Artiom 13:c459edbdaf9c 861 GoStraight(100,1,distance_recalage,0);//100 distance recalage,1->reclage en x 2->reclage en y
Artiom 13:c459edbdaf9c 862 wait(3);
Artiom 13:c459edbdaf9c 863 accelerateur_arriere_insertion();
Artiom 13:c459edbdaf9c 864 etat=insertion;
Artiom 13:c459edbdaf9c 865 break;
Artiom 13:c459edbdaf9c 866 case insertion:
Artiom 13:c459edbdaf9c 867 verification();
Artiom 13:c459edbdaf9c 868 pompe=AR_DROIT;
Artiom 13:c459edbdaf9c 869 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:c459edbdaf9c 870 pompe=AR_GAUCHE;
Artiom 13:c459edbdaf9c 871 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:c459edbdaf9c 872 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:c459edbdaf9c 873 etat = attente_ack_ventouse_droite_gauche;
Artiom 13:c459edbdaf9c 874
Artiom 13:c459edbdaf9c 875 break;
Artiom 13:c459edbdaf9c 876
Artiom 13:c459edbdaf9c 877 case attente_ack_ventouse_droite_gauche:
Artiom 13:c459edbdaf9c 878 if((status_pompe&MASK_AR_DROIT_GAUCHE)== 0) {
Artiom 13:c459edbdaf9c 879 GoStraight(distance_palet_milieu,0,0,0);
Artiom 13:c459edbdaf9c 880 wait(1);
Artiom 14:0eaeb3af639d 881 positionControl(AR_poigne_C,150,100,BLED_ON,2);
Artiom 13:c459edbdaf9c 882 verification();
Artiom 13:c459edbdaf9c 883 pompe=AR_CENTRE;
Artiom 13:c459edbdaf9c 884 can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
Artiom 13:c459edbdaf9c 885 SendRawId(HACHEUR_STATUT_VENTOUSES);
Artiom 13:c459edbdaf9c 886 etat = attente_ack_ventouse_centre;
Artiom 13:c459edbdaf9c 887 }
Artiom 13:c459edbdaf9c 888 case attente_ack_ventouse_centre:
Artiom 13:c459edbdaf9c 889 if((status_pompe&MASK_GOLDENIUM_AR)== 0) {
Artiom 13:c459edbdaf9c 890 GoStraight((distance_revenir-(distance_recalage+distance_palet_milieu)),0,0,0);
Artiom 13:c459edbdaf9c 891 wait(3);
Artiom 14:0eaeb3af639d 892 presentoir_arriere();
Artiom 14:0eaeb3af639d 893 SendRawId(HACHEUR_RELEASE_AR);
Artiom 14:0eaeb3af639d 894 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
Artiom 13:c459edbdaf9c 895 faccelerateur_avant=0;
Artiom 13:c459edbdaf9c 896 etat = init;
Artiom 13:c459edbdaf9c 897 }
Artiom 13:c459edbdaf9c 898 break;
Artiom 13:c459edbdaf9c 899 }
Artiom 0:42544b5e8b9f 900 }
Artiom 14:0eaeb3af639d 901 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Artiom 0:42544b5e8b9f 902 #endif