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