CRAC Team / actions_Gr

librairie actions gros robot carte esclave

Dependents:   carte_esclave201_petit_rob carte_esclave2019 carte_esclave_PETIT_ROBOT_2019

actions_Gr.cpp@14:bf2810b95f25, 2019-05-26 (annotated)

Committer:
marwanesaich
Date:
Sun May 26 12:46:41 2019 +0000
Revision:
14:bf2810b95f25
Parent:
13:0b5310c6dc44
Child:
15:1fefa6b1569b
ajout vide convoyeurs sans roulette;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 0:f900f4ea9dfd 1 #include "actions_Gr.h"
Artiom 0:f900f4ea9dfd 2
Artiom 0:f900f4ea9dfd 3
Artiom 0:f900f4ea9dfd 4 #ifdef ROBOT_BIG
Artiom 0:f900f4ea9dfd 5 #define MASK_PRESENTOIR_AV 0x07
Artiom 0:f900f4ea9dfd 6 #define MASK_PRESENTOIR_AR 0x70
Artiom 0:f900f4ea9dfd 7
Artiom 0:f900f4ea9dfd 8 #define MASK_GOLDENIUM_AV 0x02
Artiom 0:f900f4ea9dfd 9 #define MASK_GOLDENIUM_AR 0x20
Artiom 0:f900f4ea9dfd 10
Artiom 0:f900f4ea9dfd 11 #define MASK_SOL_AV 0x08
Artiom 0:f900f4ea9dfd 12 #define MASK_SOL_AR 0x80
Artiom 0:f900f4ea9dfd 13
Artiom 0:f900f4ea9dfd 14 #define MASK_AV_DROIT 0x01
Artiom 0:f900f4ea9dfd 15 #define MASK_AR_DROIT 0x10
Artiom 0:f900f4ea9dfd 16
Artiom 0:f900f4ea9dfd 17 #define MASK_AV_DROIT_GAUCHE 0x05
Artiom 0:f900f4ea9dfd 18 #define MASK_AR_DROIT_GAUCHE 0x50
Artiom 0:f900f4ea9dfd 19
marwanesaich 5:bb533bf81ee6 20 #define MASK_FC_DROIT 0x01
marwanesaich 5:bb533bf81ee6 21 #define MASK_CT_DROIT 0x02
marwanesaich 5:bb533bf81ee6 22
marwanesaich 5:bb533bf81ee6 23 #define MASK_FC_GAUCHE 0x04
marwanesaich 5:bb533bf81ee6 24 #define MASK_CT_GAUCHE 0x08
marwanesaich 5:bb533bf81ee6 25
marwanesaich 6:aa800daf5ff9 26 #define POS_DOIGT_GAUCHE 585
marwanesaich 6:aa800daf5ff9 27 #define POS_DOIGT_DROIT 389
Artiom 0:f900f4ea9dfd 28
Artiom 0:f900f4ea9dfd 29 char status_pompe=0;
Artiom 0:f900f4ea9dfd 30 bool flag_ascenseur = 0;
Artiom 0:f900f4ea9dfd 31
Artiom 0:f900f4ea9dfd 32 char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom 0:f900f4ea9dfd 33 char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom 0:f900f4ea9dfd 34 char fsol_avant=0,fsol_arriere=0;
Artiom 0:f900f4ea9dfd 35 char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom 0:f900f4ea9dfd 36 char fbalance_avant=0,fbalance_arriere=0;
Artiom 0:f900f4ea9dfd 37 char favant_relache=0,farriere_relache=0;
Artiom 0:f900f4ea9dfd 38 char faccelerateur_avant=0,faccelerateur_arriere=0;
Artiom 0:f900f4ea9dfd 39
marwanesaich 14:bf2810b95f25 40 int flag_vide_vert_rouge=0,flag_vide_bleu=0;
Artiom 0:f900f4ea9dfd 41
Artiom 0:f900f4ea9dfd 42 DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
Artiom 0:f900f4ea9dfd 43 DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
Artiom 0:f900f4ea9dfd 44
Artiom 0:f900f4ea9dfd 45 char buffer_couleur_bas[SIZE_FIFO];
Artiom 0:f900f4ea9dfd 46 unsigned char FIFO_couleur_ecriture=0;
Artiom 0:f900f4ea9dfd 47 signed char FIFO_couleur_lecture=0;
marwanesaich 5:bb533bf81ee6 48 char status_contact=0;
Artiom 0:f900f4ea9dfd 49
Artiom 0:f900f4ea9dfd 50
marwanesaich 14:bf2810b95f25 51 void convoyeur_gauche_jaune(void)
marwanesaich 14:bf2810b95f25 52 {
marwanesaich 14:bf2810b95f25 53
marwanesaich 14:bf2810b95f25 54 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ;
marwanesaich 5:bb533bf81ee6 55 static type_etat etat = init;
marwanesaich 11:dfff2e200d0c 56 static int16_t pos = 0, consigne_pos;
marwanesaich 5:bb533bf81ee6 57 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 58 /*if(flag_vide_vert_rouge)
marwanesaich 14:bf2810b95f25 59 {
marwanesaich 14:bf2810b95f25 60 etat = init;
marwanesaich 14:bf2810b95f25 61 flag_vide_vert_rouge = 0;
marwanesaich 14:bf2810b95f25 62 }*/
marwanesaich 14:bf2810b95f25 63
marwanesaich 14:bf2810b95f25 64 switch(etat) {
marwanesaich 14:bf2810b95f25 65
marwanesaich 14:bf2810b95f25 66 case init :
marwanesaich 5:bb533bf81ee6 67 velocityControl(stockage_G,-512,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 68 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 69 if(previous_pos != pos) {
marwanesaich 5:bb533bf81ee6 70 etat = etalonnage;
marwanesaich 14:bf2810b95f25 71 //pc.printf("ETALONNAGE\n");
marwanesaich 14:bf2810b95f25 72
marwanesaich 5:bb533bf81ee6 73 }
marwanesaich 5:bb533bf81ee6 74 break;
Artiom 0:f900f4ea9dfd 75
marwanesaich 5:bb533bf81ee6 76 case etalonnage :
marwanesaich 5:bb533bf81ee6 77 pos = getPos(stockage_G,1);
marwanesaich 11:dfff2e200d0c 78 wait_us(500);
marwanesaich 14:bf2810b95f25 79 if( ( (21<pos && pos<260) /*|| (404<pos && pos<812)*/ ) and (status_contact & MASK_FC_GAUCHE)) {
marwanesaich 5:bb533bf81ee6 80 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 11:dfff2e200d0c 81 wait_ms(300);
marwanesaich 5:bb533bf81ee6 82 pos = getPos(stockage_G,1) ;
marwanesaich 11:dfff2e200d0c 83 consigne_pos = pos + 180;
marwanesaich 11:dfff2e200d0c 84 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 85 etat = tmp_pret;
marwanesaich 14:bf2810b95f25 86 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 87 }
marwanesaich 5:bb533bf81ee6 88 break;
marwanesaich 14:bf2810b95f25 89
marwanesaich 5:bb533bf81ee6 90 case tmp_pret :
marwanesaich 11:dfff2e200d0c 91 pos = getPos(stockage_G,1);
marwanesaich 11:dfff2e200d0c 92 wait_us(500);
marwanesaich 14:bf2810b95f25 93 if(pos> (consigne_pos-5)) {
marwanesaich 13:0b5310c6dc44 94 //pc.printf("pos pret %d\n", pos);
marwanesaich 14:bf2810b95f25 95 etat = pret;
marwanesaich 14:bf2810b95f25 96 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 97
marwanesaich 14:bf2810b95f25 98 }
marwanesaich 5:bb533bf81ee6 99 break;
marwanesaich 14:bf2810b95f25 100
marwanesaich 14:bf2810b95f25 101 case pret :
marwanesaich 14:bf2810b95f25 102 if(flag_vide_vert_rouge) {
marwanesaich 14:bf2810b95f25 103 flag_vide_vert_rouge =0;
marwanesaich 14:bf2810b95f25 104 etat = vide;
marwanesaich 14:bf2810b95f25 105 } else if((status_contact & MASK_CT_GAUCHE)) {
marwanesaich 13:0b5310c6dc44 106 consigne_pos = (getPos(stockage_G,1)+550);
marwanesaich 14:bf2810b95f25 107
marwanesaich 13:0b5310c6dc44 108 //if(consigne_pos>1105) consigne_pos -= 1105;
marwanesaich 13:0b5310c6dc44 109 //pc.printf("consigne pousse %d\n", consigne_pos);
marwanesaich 6:aa800daf5ff9 110 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 111 wait_us(500);
marwanesaich 11:dfff2e200d0c 112 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 13:0b5310c6dc44 113 //compteTour(stockage_G,+1023,0,consigne_pos,BLED_ON,1);
marwanesaich 14:bf2810b95f25 114
marwanesaich 5:bb533bf81ee6 115 etat = pousse;
marwanesaich 14:bf2810b95f25 116 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 117 }
marwanesaich 5:bb533bf81ee6 118 break;
marwanesaich 14:bf2810b95f25 119
marwanesaich 5:bb533bf81ee6 120 case pousse :
marwanesaich 5:bb533bf81ee6 121 pos = getPos(stockage_G,1);
marwanesaich 13:0b5310c6dc44 122 //pc.printf("%d\n", pos);
marwanesaich 11:dfff2e200d0c 123 wait_us(500);
marwanesaich 14:bf2810b95f25 124 if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0) {
marwanesaich 14:bf2810b95f25 125 consigne_pos = pos - 550;
marwanesaich 6:aa800daf5ff9 126 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 127 wait_us(500);
marwanesaich 11:dfff2e200d0c 128 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 129 etat = retour;
marwanesaich 14:bf2810b95f25 130 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 131 }
marwanesaich 14:bf2810b95f25 132
marwanesaich 5:bb533bf81ee6 133 break;
marwanesaich 5:bb533bf81ee6 134 case retour :
marwanesaich 5:bb533bf81ee6 135 pos = getPos(stockage_G,1) ;
marwanesaich 11:dfff2e200d0c 136 wait_us(500);
marwanesaich 14:bf2810b95f25 137 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 138 etat = pret;
marwanesaich 14:bf2810b95f25 139 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 140 }
marwanesaich 14:bf2810b95f25 141 break;
marwanesaich 14:bf2810b95f25 142 case vide:
marwanesaich 14:bf2810b95f25 143 consigne_pos = getPos(stockage_G,1)+500;
marwanesaich 14:bf2810b95f25 144 compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1);
marwanesaich 14:bf2810b95f25 145 etat = fin_vide;
marwanesaich 5:bb533bf81ee6 146 break;
marwanesaich 14:bf2810b95f25 147 case fin_vide:
marwanesaich 14:bf2810b95f25 148 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 149 wait_us(500);
marwanesaich 14:bf2810b95f25 150 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 151 etat = init;
marwanesaich 14:bf2810b95f25 152 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 153 }
marwanesaich 5:bb533bf81ee6 154
marwanesaich 14:bf2810b95f25 155 break;
marwanesaich 14:bf2810b95f25 156 }
marwanesaich 14:bf2810b95f25 157 }
marwanesaich 14:bf2810b95f25 158
marwanesaich 14:bf2810b95f25 159 void convoyeur_droit_jaune(void)
marwanesaich 14:bf2810b95f25 160 {
marwanesaich 14:bf2810b95f25 161
marwanesaich 14:bf2810b95f25 162 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour, vide, fin_vide} type_etat ;
marwanesaich 5:bb533bf81ee6 163 static type_etat etat = init;
marwanesaich 11:dfff2e200d0c 164 static int16_t pos = 0, consigne_pos;
marwanesaich 5:bb533bf81ee6 165 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 166 /*
marwanesaich 14:bf2810b95f25 167 if(flag_vide_bleu){
marwanesaich 14:bf2810b95f25 168 etat = init;
marwanesaich 14:bf2810b95f25 169 flag_vide_bleu = 0;
marwanesaich 14:bf2810b95f25 170 }*/
marwanesaich 14:bf2810b95f25 171
marwanesaich 14:bf2810b95f25 172 switch(etat) {
marwanesaich 14:bf2810b95f25 173
marwanesaich 14:bf2810b95f25 174 case init :
marwanesaich 5:bb533bf81ee6 175 velocityControl(stockage_D,512,BLED_ON,3);
marwanesaich 11:dfff2e200d0c 176 wait_us(500);
marwanesaich 5:bb533bf81ee6 177 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 178 if(previous_pos != pos) {
marwanesaich 5:bb533bf81ee6 179 etat = etalonnage;
marwanesaich 14:bf2810b95f25 180 //pc.printf("ETALONNAGE\n");
marwanesaich 5:bb533bf81ee6 181 }
marwanesaich 5:bb533bf81ee6 182 break;
marwanesaich 5:bb533bf81ee6 183
marwanesaich 5:bb533bf81ee6 184 case etalonnage :
marwanesaich 5:bb533bf81ee6 185 pos = getPos(stockage_D,3);
marwanesaich 11:dfff2e200d0c 186 wait_us(500);
marwanesaich 14:bf2810b95f25 187 if(( /*(21<pos && pos<428) ||*/ (572<pos && pos<700) ) and (status_contact & MASK_FC_DROIT)) {
marwanesaich 5:bb533bf81ee6 188 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 11:dfff2e200d0c 189 wait_ms(300);
marwanesaich 5:bb533bf81ee6 190 pos = getPos(stockage_D,3) ;
marwanesaich 13:0b5310c6dc44 191 //pc.printf("pos %d\n", pos);
marwanesaich 5:bb533bf81ee6 192 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
marwanesaich 13:0b5310c6dc44 193 consigne_pos =pos-550;
marwanesaich 13:0b5310c6dc44 194 //if(consigne_pos<0) consigne_pos += 1105;
marwanesaich 13:0b5310c6dc44 195 //pc.printf("consigne : %d \n",consigne_pos);
marwanesaich 13:0b5310c6dc44 196 compteTour(stockage_D,-1023,1,consigne_pos,BLED_ON,3);
marwanesaich 13:0b5310c6dc44 197 //pc.printf("pos fin tour : %d \n",getPos(stockage_D,3));
marwanesaich 10:fba0699b7a74 198 wait_us(500);
marwanesaich 5:bb533bf81ee6 199 etat = tmp_pret;
marwanesaich 13:0b5310c6dc44 200 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 201 }
marwanesaich 5:bb533bf81ee6 202 break;
marwanesaich 14:bf2810b95f25 203
marwanesaich 5:bb533bf81ee6 204 case tmp_pret :
marwanesaich 5:bb533bf81ee6 205 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 206 wait_us(500);
marwanesaich 14:bf2810b95f25 207 if(pos < (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 208
marwanesaich 13:0b5310c6dc44 209 //pc.printf("pos fin %d\n", pos);
marwanesaich 14:bf2810b95f25 210 etat = pret;
marwanesaich 14:bf2810b95f25 211 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 212 }
marwanesaich 5:bb533bf81ee6 213 break;
marwanesaich 14:bf2810b95f25 214
marwanesaich 14:bf2810b95f25 215 case pret :
marwanesaich 14:bf2810b95f25 216
marwanesaich 14:bf2810b95f25 217 if(flag_vide_bleu) {
marwanesaich 14:bf2810b95f25 218 flag_vide_bleu =0;
marwanesaich 14:bf2810b95f25 219 etat = vide;
marwanesaich 14:bf2810b95f25 220 } else if((status_contact & MASK_CT_DROIT)) {
marwanesaich 5:bb533bf81ee6 221 //pc.printf("%d\n",pos);
marwanesaich 11:dfff2e200d0c 222 consigne_pos = getPos(stockage_D,3)+ 550;
marwanesaich 6:aa800daf5ff9 223 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 224 wait_us(500);
marwanesaich 11:dfff2e200d0c 225 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 226 wait_us(500);
marwanesaich 5:bb533bf81ee6 227 etat = pousse;
marwanesaich 13:0b5310c6dc44 228 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 229 }
marwanesaich 5:bb533bf81ee6 230 break;
marwanesaich 14:bf2810b95f25 231
marwanesaich 5:bb533bf81ee6 232 case pousse :
marwanesaich 5:bb533bf81ee6 233 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 234 wait_us(500);
marwanesaich 14:bf2810b95f25 235 if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0) {
marwanesaich 5:bb533bf81ee6 236 //pc.printf("%d\n",pos);
marwanesaich 14:bf2810b95f25 237 consigne_pos = pos - 550;
marwanesaich 6:aa800daf5ff9 238 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 239 wait_us(500);
marwanesaich 11:dfff2e200d0c 240 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 241 wait_us(500);
marwanesaich 5:bb533bf81ee6 242 etat = retour;
marwanesaich 13:0b5310c6dc44 243 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 244 }
marwanesaich 14:bf2810b95f25 245
marwanesaich 5:bb533bf81ee6 246 break;
marwanesaich 14:bf2810b95f25 247
marwanesaich 5:bb533bf81ee6 248 case retour :
marwanesaich 5:bb533bf81ee6 249 pos = getPos(stockage_D,3) ;
marwanesaich 11:dfff2e200d0c 250 wait_us(500);
marwanesaich 14:bf2810b95f25 251 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 252 etat = pret;
marwanesaich 14:bf2810b95f25 253 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 254 }
marwanesaich 14:bf2810b95f25 255 break;
marwanesaich 14:bf2810b95f25 256
marwanesaich 14:bf2810b95f25 257 case vide:
marwanesaich 14:bf2810b95f25 258 consigne_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 259 compteTour(stockage_D,+1023,2,consigne_pos,BLED_ON,3);
marwanesaich 14:bf2810b95f25 260 etat = fin_vide;
marwanesaich 5:bb533bf81ee6 261 break;
marwanesaich 14:bf2810b95f25 262 case fin_vide:
marwanesaich 14:bf2810b95f25 263 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 264 wait_us(500);
marwanesaich 14:bf2810b95f25 265 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 266 etat = init;
marwanesaich 14:bf2810b95f25 267 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 268 }
marwanesaich 14:bf2810b95f25 269
marwanesaich 14:bf2810b95f25 270 break;
marwanesaich 14:bf2810b95f25 271 }
marwanesaich 5:bb533bf81ee6 272 }
Artiom 0:f900f4ea9dfd 273
marwanesaich 14:bf2810b95f25 274 void convoyeur_gauche_violet(void)
marwanesaich 14:bf2810b95f25 275 {
marwanesaich 14:bf2810b95f25 276
marwanesaich 14:bf2810b95f25 277 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour, vide, fin_vide} type_etat ;
marwanesaich 8:7bd34e838ca3 278 static type_etat etat = init;
marwanesaich 11:dfff2e200d0c 279 static int16_t pos = 0, consigne_pos;
marwanesaich 8:7bd34e838ca3 280 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 281 /*
marwanesaich 14:bf2810b95f25 282 if(flag_vide_bleu){
marwanesaich 14:bf2810b95f25 283 etat = init;
marwanesaich 14:bf2810b95f25 284 flag_vide_bleu = 0;
marwanesaich 14:bf2810b95f25 285 }*/
marwanesaich 14:bf2810b95f25 286 switch(etat) {
marwanesaich 14:bf2810b95f25 287
marwanesaich 14:bf2810b95f25 288 case init :
marwanesaich 8:7bd34e838ca3 289 velocityControl(stockage_G,-512,BLED_ON,1);
marwanesaich 14:bf2810b95f25 290 wait_us(500);
marwanesaich 8:7bd34e838ca3 291 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 292 wait_us(500);
marwanesaich 14:bf2810b95f25 293 if(previous_pos != pos) {
marwanesaich 8:7bd34e838ca3 294 etat = etalonnage;
marwanesaich 14:bf2810b95f25 295 // pc.printf("ETALONNAGE\n");
marwanesaich 8:7bd34e838ca3 296 }
marwanesaich 8:7bd34e838ca3 297 break;
marwanesaich 8:7bd34e838ca3 298
marwanesaich 8:7bd34e838ca3 299 case etalonnage :
marwanesaich 8:7bd34e838ca3 300 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 301 if(((21<pos && pos<228) /*|| (572<pos && pos<980)*/) and (status_contact & MASK_FC_GAUCHE)) { //!!!!!!!!!!!!!changer les bornes
marwanesaich 8:7bd34e838ca3 302 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 8:7bd34e838ca3 303 wait_ms(500);
marwanesaich 8:7bd34e838ca3 304 pos = getPos(stockage_G,1) ;
marwanesaich 13:0b5310c6dc44 305 //pc.printf("pos %d\n",pos);
marwanesaich 10:fba0699b7a74 306 wait_us(500);
marwanesaich 11:dfff2e200d0c 307 consigne_pos = pos + 500;
marwanesaich 14:bf2810b95f25 308
marwanesaich 13:0b5310c6dc44 309 //pc.printf("consigne pos %d\n",consigne_pos);
marwanesaich 11:dfff2e200d0c 310 if(consigne_pos >1105) consigne_pos -= 1105;
marwanesaich 11:dfff2e200d0c 311 compteTour(stockage_G,1023,2,consigne_pos,BLED_ON,1);
marwanesaich 10:fba0699b7a74 312 wait_us(500);
marwanesaich 8:7bd34e838ca3 313 etat = tmp_pret;
marwanesaich 10:fba0699b7a74 314 ////pc.printf("TMP_PRET\n");
marwanesaich 8:7bd34e838ca3 315 }
marwanesaich 8:7bd34e838ca3 316 break;
marwanesaich 14:bf2810b95f25 317
marwanesaich 8:7bd34e838ca3 318 case tmp_pret :
marwanesaich 8:7bd34e838ca3 319 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 320 wait_us(500);
marwanesaich 14:bf2810b95f25 321 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 322 etat = pret;
marwanesaich 14:bf2810b95f25 323 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 324 }
marwanesaich 8:7bd34e838ca3 325 break;
marwanesaich 14:bf2810b95f25 326
marwanesaich 14:bf2810b95f25 327 case pret :
marwanesaich 14:bf2810b95f25 328 if(flag_vide_bleu) {
marwanesaich 14:bf2810b95f25 329 flag_vide_bleu =0;
marwanesaich 14:bf2810b95f25 330 etat = vide;
marwanesaich 14:bf2810b95f25 331 } else if((status_contact & MASK_CT_GAUCHE)) {
marwanesaich 8:7bd34e838ca3 332 //pc.printf("%d\n",pos);
marwanesaich 11:dfff2e200d0c 333 consigne_pos = getPos(stockage_G,1)- 550;
marwanesaich 10:fba0699b7a74 334 wait_us(500);
marwanesaich 8:7bd34e838ca3 335 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 336 wait_us(500);
marwanesaich 11:dfff2e200d0c 337 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 10:fba0699b7a74 338 wait_us(500);
marwanesaich 8:7bd34e838ca3 339 etat = pousse;
marwanesaich 10:fba0699b7a74 340 //pc.printf("POUSSE\n");
marwanesaich 8:7bd34e838ca3 341 }
marwanesaich 8:7bd34e838ca3 342 break;
marwanesaich 14:bf2810b95f25 343
marwanesaich 8:7bd34e838ca3 344 case pousse :
marwanesaich 8:7bd34e838ca3 345 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 346 wait_us(500);
marwanesaich 14:bf2810b95f25 347 if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) {
marwanesaich 13:0b5310c6dc44 348 //pc.printf("%d\n",pos);
marwanesaich 14:bf2810b95f25 349 consigne_pos = pos + 550;
marwanesaich 8:7bd34e838ca3 350 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 351 wait_us(500);
marwanesaich 11:dfff2e200d0c 352 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 10:fba0699b7a74 353 wait_us(500);
marwanesaich 8:7bd34e838ca3 354 etat = retour;
marwanesaich 10:fba0699b7a74 355 //pc.printf("RETOUR\n");
marwanesaich 8:7bd34e838ca3 356 }
marwanesaich 14:bf2810b95f25 357
marwanesaich 8:7bd34e838ca3 358 break;
marwanesaich 14:bf2810b95f25 359
marwanesaich 8:7bd34e838ca3 360 case retour :
marwanesaich 8:7bd34e838ca3 361 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 362 wait_us(500);
marwanesaich 14:bf2810b95f25 363 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 364 etat = pret;
marwanesaich 14:bf2810b95f25 365 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 366 }
marwanesaich 8:7bd34e838ca3 367 break;
marwanesaich 14:bf2810b95f25 368
marwanesaich 14:bf2810b95f25 369 case vide:
marwanesaich 14:bf2810b95f25 370 consigne_pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 371 compteTour(stockage_G,-1023,2,consigne_pos,BLED_ON,1);
marwanesaich 14:bf2810b95f25 372 etat = fin_vide;
marwanesaich 14:bf2810b95f25 373 break;
marwanesaich 14:bf2810b95f25 374 case fin_vide:
marwanesaich 14:bf2810b95f25 375 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 376 wait_us(500);
marwanesaich 14:bf2810b95f25 377 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 378 etat = init;
marwanesaich 14:bf2810b95f25 379 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 380 }
marwanesaich 14:bf2810b95f25 381
marwanesaich 14:bf2810b95f25 382 break;
marwanesaich 14:bf2810b95f25 383 }
marwanesaich 8:7bd34e838ca3 384 }
marwanesaich 8:7bd34e838ca3 385
marwanesaich 14:bf2810b95f25 386 void convoyeur_droit_violet(void)
marwanesaich 14:bf2810b95f25 387 {
marwanesaich 14:bf2810b95f25 388
marwanesaich 14:bf2810b95f25 389 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide} type_etat ;
marwanesaich 9:05da4300730a 390 static type_etat etat = init;
marwanesaich 11:dfff2e200d0c 391 static int16_t pos = 0, consigne_pos;
marwanesaich 9:05da4300730a 392 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 393 /*
marwanesaich 14:bf2810b95f25 394 if(flag_vide_vert_rouge){
marwanesaich 14:bf2810b95f25 395 etat = init;
marwanesaich 14:bf2810b95f25 396 flag_vide_vert_rouge = 0;
marwanesaich 14:bf2810b95f25 397 } */
marwanesaich 14:bf2810b95f25 398
marwanesaich 14:bf2810b95f25 399 switch(etat) {
marwanesaich 14:bf2810b95f25 400
marwanesaich 14:bf2810b95f25 401 case init :
marwanesaich 9:05da4300730a 402 velocityControl(stockage_D,+512,BLED_ON,3);
marwanesaich 14:bf2810b95f25 403 wait_us(500);
marwanesaich 9:05da4300730a 404 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 405 wait_us(500);
marwanesaich 14:bf2810b95f25 406 if(previous_pos != pos) {
marwanesaich 10:fba0699b7a74 407 //pc.printf("ETALONNAGE\n");
marwanesaich 9:05da4300730a 408 etat = etalonnage;
marwanesaich 9:05da4300730a 409 }
marwanesaich 9:05da4300730a 410 break;
Artiom 0:f900f4ea9dfd 411
marwanesaich 9:05da4300730a 412 case etalonnage :
marwanesaich 9:05da4300730a 413 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 414 wait_us(500);
marwanesaich 10:fba0699b7a74 415 //pc.printf("%d\n", pos);
marwanesaich 14:bf2810b95f25 416 if( /*(21<pos && pos<260) ||*/ (754<pos && pos<812) and (status_contact & MASK_FC_DROIT)) { //!!!!!!!!!!!!!changer les bornes
marwanesaich 9:05da4300730a 417 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 9:05da4300730a 418 wait_ms(500);
marwanesaich 11:dfff2e200d0c 419 consigne_pos = getPos(stockage_D,3) - 180 ;
marwanesaich 10:fba0699b7a74 420 wait_us(500);
marwanesaich 11:dfff2e200d0c 421 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 422 wait_us(500);
marwanesaich 9:05da4300730a 423 etat = tmp_pret;
marwanesaich 14:bf2810b95f25 424 // pc.printf("TMP_PRET\n");
marwanesaich 9:05da4300730a 425 }
marwanesaich 9:05da4300730a 426 break;
marwanesaich 14:bf2810b95f25 427
marwanesaich 14:bf2810b95f25 428 case tmp_pret :
marwanesaich 9:05da4300730a 429 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 430 wait_us(500);
marwanesaich 14:bf2810b95f25 431 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 432 etat = pret;
marwanesaich 14:bf2810b95f25 433 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 434 }
marwanesaich 9:05da4300730a 435 break;
marwanesaich 14:bf2810b95f25 436
marwanesaich 14:bf2810b95f25 437 case pret :
marwanesaich 14:bf2810b95f25 438 if(flag_vide_vert_rouge) {
marwanesaich 14:bf2810b95f25 439 flag_vide_vert_rouge =0;
marwanesaich 14:bf2810b95f25 440 etat = vide;
marwanesaich 14:bf2810b95f25 441 } else if((status_contact & MASK_CT_DROIT)) {
marwanesaich 11:dfff2e200d0c 442 consigne_pos = getPos(stockage_D,3) - 550;
marwanesaich 10:fba0699b7a74 443 wait_us(500);
marwanesaich 9:05da4300730a 444 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 445 wait_us(500);
marwanesaich 14:bf2810b95f25 446 //pc.printf("consigne %d", consigne_pos);
marwanesaich 11:dfff2e200d0c 447 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 448 wait_us(500);
marwanesaich 9:05da4300730a 449 etat = pousse;
marwanesaich 14:bf2810b95f25 450 //pc.printf("POUSSE\n");
marwanesaich 9:05da4300730a 451 }
marwanesaich 9:05da4300730a 452 break;
marwanesaich 14:bf2810b95f25 453
marwanesaich 9:05da4300730a 454 case pousse :
marwanesaich 9:05da4300730a 455 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 456 wait_us(500);
marwanesaich 14:bf2810b95f25 457 if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0) {
marwanesaich 14:bf2810b95f25 458 consigne_pos = pos + 550;
marwanesaich 14:bf2810b95f25 459 //pc.printf("consigne %d", consigne_pos);
marwanesaich 9:05da4300730a 460 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 461 wait_us(500);
marwanesaich 11:dfff2e200d0c 462 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 463 wait_us(500);
marwanesaich 9:05da4300730a 464 etat = retour;
marwanesaich 14:bf2810b95f25 465 //pc.printf("RETOUR\n");
marwanesaich 9:05da4300730a 466 }
marwanesaich 14:bf2810b95f25 467
marwanesaich 9:05da4300730a 468 break;
marwanesaich 9:05da4300730a 469 case retour :
marwanesaich 9:05da4300730a 470 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 471 wait_us(500);
marwanesaich 14:bf2810b95f25 472 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 473 etat = pret;
marwanesaich 14:bf2810b95f25 474 // pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 475 }
marwanesaich 14:bf2810b95f25 476 break;
marwanesaich 14:bf2810b95f25 477
marwanesaich 14:bf2810b95f25 478 case vide:
marwanesaich 14:bf2810b95f25 479 consigne_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 480 //pc.printf("consigne %d",consigne_pos);
marwanesaich 14:bf2810b95f25 481 compteTour(stockage_D,-1023,3,consigne_pos,BLED_ON,3);
marwanesaich 14:bf2810b95f25 482 etat = fin_vide;
marwanesaich 9:05da4300730a 483 break;
marwanesaich 14:bf2810b95f25 484
marwanesaich 14:bf2810b95f25 485 case fin_vide:
marwanesaich 14:bf2810b95f25 486 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 487 //pc.printf("pos %d", pos);
marwanesaich 14:bf2810b95f25 488 wait_us(500);
marwanesaich 14:bf2810b95f25 489 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 490 etat = init;
marwanesaich 14:bf2810b95f25 491 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 492 }
marwanesaich 14:bf2810b95f25 493
marwanesaich 14:bf2810b95f25 494 break;
marwanesaich 14:bf2810b95f25 495 }
marwanesaich 14:bf2810b95f25 496 }
kyxstark 3:a630a1ccf5f0 497 void gabarit_robot(void)
Artiom 0:f900f4ea9dfd 498 {
Artiom 0:f900f4ea9dfd 499 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:f900f4ea9dfd 500
kyxstark 3:a630a1ccf5f0 501 uint16_t pos_av_centre[2] = {550,550};//470,350
Artiom 0:f900f4ea9dfd 502
Artiom 0:f900f4ea9dfd 503
Artiom 0:f900f4ea9dfd 504 int speed=100;
Artiom 0:f900f4ea9dfd 505
Artiom 0:f900f4ea9dfd 506 deverouillage_torque();
kyxstark 3:a630a1ccf5f0 507 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 6:aa800daf5ff9 508 wait(0.5);
marwanesaich 14:bf2810b95f25 509
Artiom 0:f900f4ea9dfd 510
Artiom 0:f900f4ea9dfd 511 }
Artiom 0:f900f4ea9dfd 512
Artiom 0:f900f4ea9dfd 513
Artiom 0:f900f4ea9dfd 514 void fifo_couleur(void)
Artiom 0:f900f4ea9dfd 515 {
Artiom 0:f900f4ea9dfd 516
Artiom 0:f900f4ea9dfd 517 typedef enum {n_atome, atome, tmp} type_etat ;
Artiom 0:f900f4ea9dfd 518 static type_etat etat = n_atome;
Artiom 0:f900f4ea9dfd 519
Artiom 0:f900f4ea9dfd 520 int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
Artiom 0:f900f4ea9dfd 521 switch(etat) {
Artiom 0:f900f4ea9dfd 522 case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom 0:f900f4ea9dfd 523 if(etat_cap) {
Artiom 0:f900f4ea9dfd 524 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 525 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 526 if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
marwanesaich 5:bb533bf81ee6 527 oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom 0:f900f4ea9dfd 528 }
marwanesaich 14:bf2810b95f25 529
Artiom 0:f900f4ea9dfd 530 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 531 etat = tmp;
Artiom 0:f900f4ea9dfd 532 }
Artiom 0:f900f4ea9dfd 533 break;
Artiom 0:f900f4ea9dfd 534
Artiom 0:f900f4ea9dfd 535 case tmp : //on attend que l'atome traité soit totalement passé
Artiom 0:f900f4ea9dfd 536 if(!etat_cap) {
Artiom 0:f900f4ea9dfd 537 etat = n_atome;
Artiom 0:f900f4ea9dfd 538 }
Artiom 0:f900f4ea9dfd 539 break;
Artiom 0:f900f4ea9dfd 540
Artiom 0:f900f4ea9dfd 541 }
Artiom 0:f900f4ea9dfd 542
Artiom 0:f900f4ea9dfd 543 }
Artiom 0:f900f4ea9dfd 544
Artiom 0:f900f4ea9dfd 545
Artiom 0:f900f4ea9dfd 546 void ascenseur(void)
Artiom 0:f900f4ea9dfd 547 {
Artiom 0:f900f4ea9dfd 548 typedef enum {init, atome, tmp} type_etat;
Artiom 0:f900f4ea9dfd 549 static type_etat etat = init;
Artiom 0:f900f4ea9dfd 550
Artiom 0:f900f4ea9dfd 551 int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
Artiom 0:f900f4ea9dfd 552 static int flag_sortie = 0;
marwanesaich 14:bf2810b95f25 553
Artiom 0:f900f4ea9dfd 554 switch(etat) {
Artiom 0:f900f4ea9dfd 555 case init :
Artiom 0:f900f4ea9dfd 556 //on attend le premier atome et place le herkulex en fonction
Artiom 0:f900f4ea9dfd 557 if(flag_ascenseur) {
marwanesaich 6:aa800daf5ff9 558 //SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 559 etat = atome;
Artiom 0:f900f4ea9dfd 560 }
Artiom 0:f900f4ea9dfd 561 break;
Artiom 0:f900f4ea9dfd 562
Artiom 0:f900f4ea9dfd 563 case atome :
Artiom 0:f900f4ea9dfd 564 //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
marwanesaich 14:bf2810b95f25 565 if(etat_cap != 0) {
marwanesaich 7:283d66d5c3b9 566 oriente_doigt(etat_cap);
marwanesaich 6:aa800daf5ff9 567 etat = tmp;
marwanesaich 6:aa800daf5ff9 568 }
marwanesaich 10:fba0699b7a74 569 /*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
marwanesaich 10:fba0699b7a74 570 {
marwanesaich 10:fba0699b7a74 571 oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO);
marwanesaich 6:aa800daf5ff9 572 }*/
Artiom 0:f900f4ea9dfd 573 break;
Artiom 0:f900f4ea9dfd 574
Artiom 0:f900f4ea9dfd 575 case tmp :
Artiom 0:f900f4ea9dfd 576 //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom 0:f900f4ea9dfd 577 if(etat_cap == 0) {
marwanesaich 10:fba0699b7a74 578 char memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
Artiom 0:f900f4ea9dfd 579 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
marwanesaich 14:bf2810b95f25 580 if(FIFO_couleur_lecture == FIFO_couleur_ecriture) {
marwanesaich 14:bf2810b95f25 581 //pc.printf("Fifo vide\n");
marwanesaich 10:fba0699b7a74 582 if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
marwanesaich 10:fba0699b7a74 583 oriente_doigt(2);
marwanesaich 10:fba0699b7a74 584 else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
marwanesaich 10:fba0699b7a74 585 oriente_doigt(1);
marwanesaich 10:fba0699b7a74 586 }
Artiom 0:f900f4ea9dfd 587 etat = atome;
Artiom 0:f900f4ea9dfd 588 }
Artiom 0:f900f4ea9dfd 589 break;
Artiom 0:f900f4ea9dfd 590 }
Artiom 0:f900f4ea9dfd 591 }
kyxstark 1:af508f84a079 592
marwanesaich 5:bb533bf81ee6 593 void oriente_doigt(int palet)
marwanesaich 5:bb533bf81ee6 594 {
marwanesaich 5:bb533bf81ee6 595 if(cote) {
marwanesaich 5:bb533bf81ee6 596 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 597 positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 598 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 599 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 600 positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 601 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 14:bf2810b95f25 602
marwanesaich 5:bb533bf81ee6 603 }
marwanesaich 5:bb533bf81ee6 604 } else {
marwanesaich 5:bb533bf81ee6 605 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 606 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 607 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 5:bb533bf81ee6 608 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 609 positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 610 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 611 }
marwanesaich 5:bb533bf81ee6 612 }
marwanesaich 5:bb533bf81ee6 613 }
marwanesaich 5:bb533bf81ee6 614
Artiom 0:f900f4ea9dfd 615 void presentoir_avant(void)
Artiom 0:f900f4ea9dfd 616 {
kyxstark 2:33583329d6c8 617 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
kyxstark 2:33583329d6c8 618
kyxstark 3:a630a1ccf5f0 619 uint16_t pos_av_centre[2] = {512,512};
kyxstark 2:33583329d6c8 620
kyxstark 2:33583329d6c8 621 int speed=1;
kyxstark 2:33583329d6c8 622
kyxstark 2:33583329d6c8 623 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
kyxstark 2:33583329d6c8 624 verification();
Artiom 0:f900f4ea9dfd 625 }
Artiom 0:f900f4ea9dfd 626
Artiom 0:f900f4ea9dfd 627
Artiom 0:f900f4ea9dfd 628 void automate_ventouse_presentoir_avant(void)
Artiom 0:f900f4ea9dfd 629 {
kyxstark 2:33583329d6c8 630 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 631 static type_etat etat = init;
kyxstark 2:33583329d6c8 632
kyxstark 2:33583329d6c8 633 switch(etat) {
kyxstark 2:33583329d6c8 634 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 635 if(fpresentoir_avant)
kyxstark 2:33583329d6c8 636 etat=envoi_instruction;
kyxstark 2:33583329d6c8 637 break;
kyxstark 2:33583329d6c8 638
kyxstark 2:33583329d6c8 639 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 640 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 641 presentoir_avant();
kyxstark 2:33583329d6c8 642 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
kyxstark 2:33583329d6c8 643 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 644 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 645 break;
kyxstark 2:33583329d6c8 646
kyxstark 2:33583329d6c8 647 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 648 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
kyxstark 2:33583329d6c8 649 fpresentoir_avant=0;
kyxstark 2:33583329d6c8 650 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 651 etat = init;
kyxstark 2:33583329d6c8 652 }
kyxstark 2:33583329d6c8 653 break;
kyxstark 2:33583329d6c8 654
kyxstark 2:33583329d6c8 655 }
Artiom 0:f900f4ea9dfd 656 }
Artiom 0:f900f4ea9dfd 657
Artiom 0:f900f4ea9dfd 658 void automate_ventouse_relache_avant(void)
Artiom 0:f900f4ea9dfd 659 {
Artiom 0:f900f4ea9dfd 660
kyxstark 2:33583329d6c8 661 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 662 static type_etat etat = init;
kyxstark 2:33583329d6c8 663
kyxstark 2:33583329d6c8 664 switch(etat) {
kyxstark 2:33583329d6c8 665 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 666 if(favant_relache)
kyxstark 2:33583329d6c8 667 etat=envoi_instruction;
kyxstark 2:33583329d6c8 668 break;
kyxstark 2:33583329d6c8 669
kyxstark 2:33583329d6c8 670 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 671 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 672 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 673 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 674 break;
kyxstark 2:33583329d6c8 675
kyxstark 2:33583329d6c8 676 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 677 SendRawId(HACHEUR_RELEASE_AV);
kyxstark 2:33583329d6c8 678 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
kyxstark 2:33583329d6c8 679 favant_relache=0;
kyxstark 2:33583329d6c8 680 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 681 etat = init;
kyxstark 2:33583329d6c8 682 }
kyxstark 2:33583329d6c8 683 break;
kyxstark 2:33583329d6c8 684
kyxstark 2:33583329d6c8 685 }
Artiom 0:f900f4ea9dfd 686
Artiom 0:f900f4ea9dfd 687 }
Artiom 0:f900f4ea9dfd 688
Artiom 0:f900f4ea9dfd 689 void goldenium_avant(void)
Artiom 0:f900f4ea9dfd 690 {
Artiom 0:f900f4ea9dfd 691
Artiom 0:f900f4ea9dfd 692 }
Artiom 0:f900f4ea9dfd 693
Artiom 0:f900f4ea9dfd 694 void automate_ventouse_goldenium_avant (void)
Artiom 0:f900f4ea9dfd 695 {
Artiom 0:f900f4ea9dfd 696 }
Artiom 0:f900f4ea9dfd 697
Artiom 0:f900f4ea9dfd 698 void accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 699 {
Artiom 0:f900f4ea9dfd 700 }
Artiom 0:f900f4ea9dfd 701
Artiom 0:f900f4ea9dfd 702 void automate_ventouse_accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 703 {
Artiom 0:f900f4ea9dfd 704
kyxstark 1:af508f84a079 705 }
Artiom 0:f900f4ea9dfd 706 #endif