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@16:06463f24811d, 2019-05-28 (annotated)

Committer:
marwanesaich
Date:
Tue May 28 18:02:59 2019 +0000
Revision:
16:06463f24811d
Parent:
15:1fefa6b1569b
Child:
17:5973f13f44e2
convoyeurs bon, vide bleu et vert/rouge

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 15:1fefa6b1569b 26 #define POS_DOIGT_GAUCHE 605
marwanesaich 15:1fefa6b1569b 27 #define POS_DOIGT_DROIT 369
marwanesaich 15:1fefa6b1569b 28 #define POS_DOIGT_GAUCHE_SUIV 535
marwanesaich 15:1fefa6b1569b 29 #define POS_DOIGT_DROIT_SUIV 439
Artiom 0:f900f4ea9dfd 30
Artiom 0:f900f4ea9dfd 31 char status_pompe=0;
marwanesaich 15:1fefa6b1569b 32 bool flag_ascenseur = 0, flag_ascenseur_commande = 0;
Artiom 0:f900f4ea9dfd 33
Artiom 0:f900f4ea9dfd 34 char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom 0:f900f4ea9dfd 35 char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom 0:f900f4ea9dfd 36 char fsol_avant=0,fsol_arriere=0;
Artiom 0:f900f4ea9dfd 37 char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom 0:f900f4ea9dfd 38 char fbalance_avant=0,fbalance_arriere=0;
Artiom 0:f900f4ea9dfd 39 char favant_relache=0,farriere_relache=0;
Artiom 0:f900f4ea9dfd 40 char faccelerateur_avant=0,faccelerateur_arriere=0;
Artiom 0:f900f4ea9dfd 41
marwanesaich 14:bf2810b95f25 42 int flag_vide_vert_rouge=0,flag_vide_bleu=0;
Artiom 0:f900f4ea9dfd 43
Artiom 0:f900f4ea9dfd 44 DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
Artiom 0:f900f4ea9dfd 45 DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
Artiom 0:f900f4ea9dfd 46
marwanesaich 15:1fefa6b1569b 47 PwmOut PWM_roue_Gauche(PA_15);
marwanesaich 15:1fefa6b1569b 48 PwmOut PWM_roue_Droite(PC_9);
marwanesaich 15:1fefa6b1569b 49
Artiom 0:f900f4ea9dfd 50 char buffer_couleur_bas[SIZE_FIFO];
Artiom 0:f900f4ea9dfd 51 unsigned char FIFO_couleur_ecriture=0;
Artiom 0:f900f4ea9dfd 52 signed char FIFO_couleur_lecture=0;
marwanesaich 5:bb533bf81ee6 53 char status_contact=0;
marwanesaich 16:06463f24811d 54 int nbPaletsVerts = 0, nbPaletsBleus = 0;
marwanesaich 16:06463f24811d 55
Artiom 0:f900f4ea9dfd 56
Artiom 0:f900f4ea9dfd 57
marwanesaich 15:1fefa6b1569b 58 void roue(int allume)
marwanesaich 15:1fefa6b1569b 59 {
marwanesaich 15:1fefa6b1569b 60 if(cote)
marwanesaich 15:1fefa6b1569b 61 {
marwanesaich 15:1fefa6b1569b 62 PWM_roue_Gauche.write(float(allume));// = 1.0;//VIT_ROUE*allume;
marwanesaich 15:1fefa6b1569b 63 }
marwanesaich 15:1fefa6b1569b 64 else
marwanesaich 15:1fefa6b1569b 65 {
marwanesaich 15:1fefa6b1569b 66 PWM_roue_Droite.write(float(allume));// = 1.0;//VIT_ROUE*allume;
marwanesaich 15:1fefa6b1569b 67 }
marwanesaich 15:1fefa6b1569b 68 }
marwanesaich 15:1fefa6b1569b 69
marwanesaich 14:bf2810b95f25 70 void convoyeur_gauche_jaune(void)
marwanesaich 14:bf2810b95f25 71 {
marwanesaich 14:bf2810b95f25 72
marwanesaich 15:1fefa6b1569b 73 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, tmp_vide, fin_vide} type_etat ;
marwanesaich 5:bb533bf81ee6 74 static type_etat etat = init;
marwanesaich 15:1fefa6b1569b 75 static int16_t pos = 0, consigne_pos, consigne_roue;
marwanesaich 5:bb533bf81ee6 76 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 15:1fefa6b1569b 77 static int cpt_vider = 0;
marwanesaich 16:06463f24811d 78 int flag_debut = 0;
marwanesaich 16:06463f24811d 79 static int pos_vide[5] = {800, 200, 600, 1000, 400};
marwanesaich 14:bf2810b95f25 80 /*if(flag_vide_vert_rouge)
marwanesaich 14:bf2810b95f25 81 {
marwanesaich 14:bf2810b95f25 82 etat = init;
marwanesaich 14:bf2810b95f25 83 flag_vide_vert_rouge = 0;
marwanesaich 14:bf2810b95f25 84 }*/
marwanesaich 14:bf2810b95f25 85
marwanesaich 14:bf2810b95f25 86 switch(etat) {
marwanesaich 14:bf2810b95f25 87
marwanesaich 14:bf2810b95f25 88 case init :
marwanesaich 5:bb533bf81ee6 89 velocityControl(stockage_G,-512,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 90 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 91 if(previous_pos != pos) {
marwanesaich 5:bb533bf81ee6 92 etat = etalonnage;
marwanesaich 14:bf2810b95f25 93 //pc.printf("ETALONNAGE\n");
marwanesaich 14:bf2810b95f25 94
marwanesaich 5:bb533bf81ee6 95 }
marwanesaich 5:bb533bf81ee6 96 break;
Artiom 0:f900f4ea9dfd 97
marwanesaich 5:bb533bf81ee6 98 case etalonnage :
marwanesaich 5:bb533bf81ee6 99 pos = getPos(stockage_G,1);
marwanesaich 11:dfff2e200d0c 100 wait_us(500);
marwanesaich 14:bf2810b95f25 101 if( ( (21<pos && pos<260) /*|| (404<pos && pos<812)*/ ) and (status_contact & MASK_FC_GAUCHE)) {
marwanesaich 5:bb533bf81ee6 102 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 11:dfff2e200d0c 103 wait_ms(300);
marwanesaich 5:bb533bf81ee6 104 pos = getPos(stockage_G,1) ;
marwanesaich 11:dfff2e200d0c 105 consigne_pos = pos + 180;
marwanesaich 11:dfff2e200d0c 106 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 107 etat = tmp_pret;
marwanesaich 14:bf2810b95f25 108 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 109 }
marwanesaich 5:bb533bf81ee6 110 break;
marwanesaich 14:bf2810b95f25 111
marwanesaich 5:bb533bf81ee6 112 case tmp_pret :
marwanesaich 11:dfff2e200d0c 113 pos = getPos(stockage_G,1);
marwanesaich 11:dfff2e200d0c 114 wait_us(500);
marwanesaich 14:bf2810b95f25 115 if(pos> (consigne_pos-5)) {
marwanesaich 13:0b5310c6dc44 116 //pc.printf("pos pret %d\n", pos);
marwanesaich 14:bf2810b95f25 117 etat = pret;
marwanesaich 14:bf2810b95f25 118 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 119
marwanesaich 14:bf2810b95f25 120 }
marwanesaich 5:bb533bf81ee6 121 break;
marwanesaich 14:bf2810b95f25 122
marwanesaich 14:bf2810b95f25 123 case pret :
marwanesaich 14:bf2810b95f25 124 if(flag_vide_vert_rouge) {
marwanesaich 14:bf2810b95f25 125 flag_vide_vert_rouge =0;
marwanesaich 16:06463f24811d 126 flag_debut = 0;
marwanesaich 14:bf2810b95f25 127 etat = vide;
marwanesaich 14:bf2810b95f25 128 } else if((status_contact & MASK_CT_GAUCHE)) {
marwanesaich 13:0b5310c6dc44 129 consigne_pos = (getPos(stockage_G,1)+550);
marwanesaich 14:bf2810b95f25 130
marwanesaich 13:0b5310c6dc44 131 //if(consigne_pos>1105) consigne_pos -= 1105;
marwanesaich 13:0b5310c6dc44 132 //pc.printf("consigne pousse %d\n", consigne_pos);
marwanesaich 6:aa800daf5ff9 133 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 134 wait_us(500);
marwanesaich 11:dfff2e200d0c 135 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 13:0b5310c6dc44 136 //compteTour(stockage_G,+1023,0,consigne_pos,BLED_ON,1);
marwanesaich 14:bf2810b95f25 137
marwanesaich 5:bb533bf81ee6 138 etat = pousse;
marwanesaich 14:bf2810b95f25 139 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 140 }
marwanesaich 5:bb533bf81ee6 141 break;
marwanesaich 14:bf2810b95f25 142
marwanesaich 5:bb533bf81ee6 143 case pousse :
marwanesaich 5:bb533bf81ee6 144 pos = getPos(stockage_G,1);
marwanesaich 13:0b5310c6dc44 145 //pc.printf("%d\n", pos);
marwanesaich 11:dfff2e200d0c 146 wait_us(500);
marwanesaich 14:bf2810b95f25 147 if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0) {
marwanesaich 14:bf2810b95f25 148 consigne_pos = pos - 550;
marwanesaich 6:aa800daf5ff9 149 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 150 wait_us(500);
marwanesaich 11:dfff2e200d0c 151 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 152 etat = retour;
marwanesaich 14:bf2810b95f25 153 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 154 }
marwanesaich 14:bf2810b95f25 155
marwanesaich 5:bb533bf81ee6 156 break;
marwanesaich 5:bb533bf81ee6 157 case retour :
marwanesaich 5:bb533bf81ee6 158 pos = getPos(stockage_G,1) ;
marwanesaich 11:dfff2e200d0c 159 wait_us(500);
marwanesaich 14:bf2810b95f25 160 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 161 etat = pret;
marwanesaich 14:bf2810b95f25 162 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 163 }
marwanesaich 14:bf2810b95f25 164 break;
marwanesaich 16:06463f24811d 165 /*
marwanesaich 14:bf2810b95f25 166 case vide:
marwanesaich 15:1fefa6b1569b 167 pos = getPos(stockage_G,1);
marwanesaich 16:06463f24811d 168 if(!flag_debut){
marwanesaich 16:06463f24811d 169 flag_debut =1;
marwanesaich 16:06463f24811d 170 consigne_pos = (pos+420)%1105;
marwanesaich 16:06463f24811d 171 }
marwanesaich 16:06463f24811d 172 else{
marwanesaich 16:06463f24811d 173
marwanesaich 16:06463f24811d 174 consigne_pos = (pos+400)%1105;
marwanesaich 16:06463f24811d 175 }
marwanesaich 16:06463f24811d 176
marwanesaich 15:1fefa6b1569b 177 pc.printf("\n1 pos : %d consigne %d\n",pos, consigne_pos);
marwanesaich 15:1fefa6b1569b 178 //ACTIVERMOTEUR
marwanesaich 15:1fefa6b1569b 179 roue(1);
marwanesaich 15:1fefa6b1569b 180 //compteTour(stockage_G,+1023,3,consigne_pos,BLED_ON,1);
marwanesaich 15:1fefa6b1569b 181
marwanesaich 16:06463f24811d 182 if(cpt_vider<7){
marwanesaich 16:06463f24811d 183 compteTour(stockage_G,+200,0,consigne_pos,BLED_ON,1);
marwanesaich 15:1fefa6b1569b 184 //positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 15:1fefa6b1569b 185 pc.printf("TMP_VIDE\n");
marwanesaich 15:1fefa6b1569b 186 etat = tmp_vide;
marwanesaich 15:1fefa6b1569b 187
marwanesaich 15:1fefa6b1569b 188 }else{
marwanesaich 15:1fefa6b1569b 189 //arreter moteurs
marwanesaich 15:1fefa6b1569b 190 etat = init;
marwanesaich 15:1fefa6b1569b 191 cpt_vider = 0;
marwanesaich 15:1fefa6b1569b 192 roue(0);
marwanesaich 15:1fefa6b1569b 193 pc.printf("INIT\n");
marwanesaich 15:1fefa6b1569b 194 }
marwanesaich 5:bb533bf81ee6 195 break;
marwanesaich 15:1fefa6b1569b 196
marwanesaich 15:1fefa6b1569b 197 case tmp_vide:
marwanesaich 15:1fefa6b1569b 198
marwanesaich 14:bf2810b95f25 199 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 200 wait_us(500);
marwanesaich 16:06463f24811d 201 if(pos< (consigne_pos+5)) {
marwanesaich 15:1fefa6b1569b 202 cpt_vider +=1;
marwanesaich 16:06463f24811d 203 consigne_roue = 815;
marwanesaich 15:1fefa6b1569b 204 positionControl(roue_G,consigne_roue,40,BLED_ON,1);
marwanesaich 15:1fefa6b1569b 205 etat = fin_vide;
marwanesaich 15:1fefa6b1569b 206 pc.printf("FIN_VIDE\n");
marwanesaich 15:1fefa6b1569b 207 }
marwanesaich 15:1fefa6b1569b 208
marwanesaich 15:1fefa6b1569b 209 break;
marwanesaich 15:1fefa6b1569b 210
marwanesaich 15:1fefa6b1569b 211 case fin_vide:
marwanesaich 15:1fefa6b1569b 212 pos = getPos(roue_G,1) ;
marwanesaich 15:1fefa6b1569b 213 wait_us(500);
marwanesaich 15:1fefa6b1569b 214 if(pos> (consigne_roue-5)) {
marwanesaich 15:1fefa6b1569b 215 wait_ms(300.0);
marwanesaich 15:1fefa6b1569b 216 positionControl(roue_G,780,1,BLED_ON,1);
marwanesaich 15:1fefa6b1569b 217 pc.printf("VIDE\n");
marwanesaich 15:1fefa6b1569b 218 wait_ms(300.0);
marwanesaich 15:1fefa6b1569b 219 etat = vide;
marwanesaich 14:bf2810b95f25 220 }
marwanesaich 5:bb533bf81ee6 221
marwanesaich 14:bf2810b95f25 222 break;
marwanesaich 16:06463f24811d 223 }*/
marwanesaich 16:06463f24811d 224 case vide:
marwanesaich 16:06463f24811d 225
marwanesaich 16:06463f24811d 226 velocityControl(stockage_G, 400, BLED_ON,1);
marwanesaich 16:06463f24811d 227 wait_ms(200);
marwanesaich 16:06463f24811d 228 roue(1);
marwanesaich 16:06463f24811d 229
marwanesaich 16:06463f24811d 230 if(cpt_vider<9){
marwanesaich 16:06463f24811d 231 etat = tmp_vide;
marwanesaich 16:06463f24811d 232 //pc.printf("TMP_VIDE\n");
marwanesaich 16:06463f24811d 233
marwanesaich 16:06463f24811d 234 }else{
marwanesaich 16:06463f24811d 235 etat = init;
marwanesaich 16:06463f24811d 236 cpt_vider = 0;
marwanesaich 16:06463f24811d 237 roue(0);
marwanesaich 16:06463f24811d 238 //pc.printf("INIT\n");
marwanesaich 16:06463f24811d 239 }
marwanesaich 16:06463f24811d 240 break;
marwanesaich 16:06463f24811d 241
marwanesaich 16:06463f24811d 242 case tmp_vide:
marwanesaich 16:06463f24811d 243 pos = getPos(stockage_G,1);
marwanesaich 16:06463f24811d 244 wait_us(100);
marwanesaich 16:06463f24811d 245 if(pos >= pos_vide[cpt_vider%5] && pos <= (pos_vide[cpt_vider%5]+20)){ //if(pos == pos_vide[cpt_vider%5])
marwanesaich 16:06463f24811d 246 velocityControl(stockage_G, 0, BLED_ON,1);
marwanesaich 16:06463f24811d 247 positionControl(stockage_G,pos-30,1,BLED_ON,1);
marwanesaich 16:06463f24811d 248 //pc.printf("pos :%d\n", pos);
marwanesaich 16:06463f24811d 249 consigne_roue = 840;
marwanesaich 16:06463f24811d 250 positionControl(roue_G,consigne_roue,1,BLED_ON,1);
marwanesaich 16:06463f24811d 251 etat = fin_vide;
marwanesaich 16:06463f24811d 252 }
marwanesaich 16:06463f24811d 253
marwanesaich 16:06463f24811d 254 break;
marwanesaich 16:06463f24811d 255
marwanesaich 16:06463f24811d 256 case fin_vide:
marwanesaich 16:06463f24811d 257 pos = getPos(roue_G,1) ;
marwanesaich 16:06463f24811d 258 wait_us(500);
marwanesaich 16:06463f24811d 259 if(pos> (consigne_roue-5)) {
marwanesaich 16:06463f24811d 260 positionControl(roue_G,750,50,BLED_ON,1);
marwanesaich 16:06463f24811d 261 verification();
marwanesaich 16:06463f24811d 262 cpt_vider ++;
marwanesaich 16:06463f24811d 263 etat = vide;
marwanesaich 16:06463f24811d 264 }
marwanesaich 16:06463f24811d 265
marwanesaich 16:06463f24811d 266 break;
marwanesaich 14:bf2810b95f25 267 }
marwanesaich 14:bf2810b95f25 268 }
marwanesaich 14:bf2810b95f25 269
marwanesaich 14:bf2810b95f25 270 void convoyeur_droit_jaune(void)
marwanesaich 14:bf2810b95f25 271 {
marwanesaich 14:bf2810b95f25 272
marwanesaich 14:bf2810b95f25 273 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour, vide, fin_vide} type_etat ;
marwanesaich 5:bb533bf81ee6 274 static type_etat etat = init;
marwanesaich 11:dfff2e200d0c 275 static int16_t pos = 0, consigne_pos;
marwanesaich 5:bb533bf81ee6 276 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 16:06463f24811d 277 int vInit = 512;
marwanesaich 14:bf2810b95f25 278 /*
marwanesaich 14:bf2810b95f25 279 if(flag_vide_bleu){
marwanesaich 14:bf2810b95f25 280 etat = init;
marwanesaich 14:bf2810b95f25 281 flag_vide_bleu = 0;
marwanesaich 14:bf2810b95f25 282 }*/
marwanesaich 14:bf2810b95f25 283
marwanesaich 14:bf2810b95f25 284 switch(etat) {
marwanesaich 14:bf2810b95f25 285
marwanesaich 14:bf2810b95f25 286 case init :
marwanesaich 16:06463f24811d 287 velocityControl(stockage_D,vInit,BLED_ON,3);
marwanesaich 11:dfff2e200d0c 288 wait_us(500);
marwanesaich 5:bb533bf81ee6 289 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 290 if(previous_pos != pos) {
marwanesaich 5:bb533bf81ee6 291 etat = etalonnage;
marwanesaich 14:bf2810b95f25 292 //pc.printf("ETALONNAGE\n");
marwanesaich 5:bb533bf81ee6 293 }
marwanesaich 5:bb533bf81ee6 294 break;
marwanesaich 5:bb533bf81ee6 295
marwanesaich 5:bb533bf81ee6 296 case etalonnage :
marwanesaich 5:bb533bf81ee6 297 pos = getPos(stockage_D,3);
marwanesaich 11:dfff2e200d0c 298 wait_us(500);
marwanesaich 16:06463f24811d 299 if(( /*(21<pos && pos<428) || */(572<pos && pos<700) ) and (status_contact & MASK_FC_DROIT)) {
marwanesaich 5:bb533bf81ee6 300 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 11:dfff2e200d0c 301 wait_ms(300);
marwanesaich 5:bb533bf81ee6 302 pos = getPos(stockage_D,3) ;
marwanesaich 16:06463f24811d 303 //pc.printf("pos %d\n", pos);
marwanesaich 5:bb533bf81ee6 304 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
marwanesaich 13:0b5310c6dc44 305 consigne_pos =pos-550;
marwanesaich 15:1fefa6b1569b 306 if(consigne_pos<0) consigne_pos += 1105;
marwanesaich 16:06463f24811d 307 //pc.printf("consigne : %d \n",consigne_pos);
marwanesaich 13:0b5310c6dc44 308 compteTour(stockage_D,-1023,1,consigne_pos,BLED_ON,3);
marwanesaich 13:0b5310c6dc44 309 //pc.printf("pos fin tour : %d \n",getPos(stockage_D,3));
marwanesaich 10:fba0699b7a74 310 wait_us(500);
marwanesaich 5:bb533bf81ee6 311 etat = tmp_pret;
marwanesaich 13:0b5310c6dc44 312 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 313 }
marwanesaich 5:bb533bf81ee6 314 break;
marwanesaich 14:bf2810b95f25 315
marwanesaich 5:bb533bf81ee6 316 case tmp_pret :
marwanesaich 5:bb533bf81ee6 317 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 318 wait_us(500);
marwanesaich 14:bf2810b95f25 319 if(pos < (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 320
marwanesaich 13:0b5310c6dc44 321 //pc.printf("pos fin %d\n", pos);
marwanesaich 14:bf2810b95f25 322 etat = pret;
marwanesaich 14:bf2810b95f25 323 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 324 }
marwanesaich 5:bb533bf81ee6 325 break;
marwanesaich 14:bf2810b95f25 326
marwanesaich 14:bf2810b95f25 327 case pret :
marwanesaich 14:bf2810b95f25 328
marwanesaich 14:bf2810b95f25 329 if(flag_vide_bleu) {
marwanesaich 14:bf2810b95f25 330 flag_vide_bleu =0;
marwanesaich 16:06463f24811d 331 vInit = 1023;
marwanesaich 16:06463f24811d 332 etat = init;
marwanesaich 16:06463f24811d 333 //pc.printf("vide\n");
marwanesaich 14:bf2810b95f25 334 } else if((status_contact & MASK_CT_DROIT)) {
marwanesaich 16:06463f24811d 335 ///pc.printf("%d\n",pos);
marwanesaich 11:dfff2e200d0c 336 consigne_pos = getPos(stockage_D,3)+ 550;
marwanesaich 6:aa800daf5ff9 337 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 338 wait_us(500);
marwanesaich 11:dfff2e200d0c 339 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 340 wait_us(500);
marwanesaich 5:bb533bf81ee6 341 etat = pousse;
marwanesaich 13:0b5310c6dc44 342 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 343 }
marwanesaich 5:bb533bf81ee6 344 break;
marwanesaich 14:bf2810b95f25 345
marwanesaich 5:bb533bf81ee6 346 case pousse :
marwanesaich 5:bb533bf81ee6 347 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 348 wait_us(500);
marwanesaich 14:bf2810b95f25 349 if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0) {
marwanesaich 5:bb533bf81ee6 350 //pc.printf("%d\n",pos);
marwanesaich 14:bf2810b95f25 351 consigne_pos = pos - 550;
marwanesaich 6:aa800daf5ff9 352 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 353 wait_us(500);
marwanesaich 11:dfff2e200d0c 354 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 355 wait_us(500);
marwanesaich 5:bb533bf81ee6 356 etat = retour;
marwanesaich 13:0b5310c6dc44 357 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 358 }
marwanesaich 14:bf2810b95f25 359
marwanesaich 5:bb533bf81ee6 360 break;
marwanesaich 14:bf2810b95f25 361
marwanesaich 5:bb533bf81ee6 362 case retour :
marwanesaich 5:bb533bf81ee6 363 pos = getPos(stockage_D,3) ;
marwanesaich 11:dfff2e200d0c 364 wait_us(500);
marwanesaich 14:bf2810b95f25 365 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 366 etat = pret;
marwanesaich 14:bf2810b95f25 367 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 368 }
marwanesaich 14:bf2810b95f25 369 break;
marwanesaich 14:bf2810b95f25 370
marwanesaich 14:bf2810b95f25 371 case vide:
marwanesaich 14:bf2810b95f25 372 consigne_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 373 compteTour(stockage_D,+1023,2,consigne_pos,BLED_ON,3);
marwanesaich 14:bf2810b95f25 374 etat = fin_vide;
marwanesaich 5:bb533bf81ee6 375 break;
marwanesaich 14:bf2810b95f25 376 case fin_vide:
marwanesaich 14:bf2810b95f25 377 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 378 wait_us(500);
marwanesaich 14:bf2810b95f25 379 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 380 etat = init;
marwanesaich 16:06463f24811d 381 //pc.printf("INIT\n");
marwanesaich 14:bf2810b95f25 382 }
marwanesaich 14:bf2810b95f25 383
marwanesaich 14:bf2810b95f25 384 break;
marwanesaich 14:bf2810b95f25 385 }
marwanesaich 5:bb533bf81ee6 386 }
Artiom 0:f900f4ea9dfd 387
marwanesaich 14:bf2810b95f25 388 void convoyeur_gauche_violet(void)
marwanesaich 14:bf2810b95f25 389 {
marwanesaich 14:bf2810b95f25 390
marwanesaich 14:bf2810b95f25 391 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour, vide, fin_vide} type_etat ;
marwanesaich 8:7bd34e838ca3 392 static type_etat etat = init;
marwanesaich 16:06463f24811d 393 static int16_t pos = 0, consigne_pos, pos_pret;
marwanesaich 8:7bd34e838ca3 394 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 16:06463f24811d 395 static int vInit = -512;
marwanesaich 14:bf2810b95f25 396 /*
marwanesaich 14:bf2810b95f25 397 if(flag_vide_bleu){
marwanesaich 14:bf2810b95f25 398 etat = init;
marwanesaich 14:bf2810b95f25 399 flag_vide_bleu = 0;
marwanesaich 14:bf2810b95f25 400 }*/
marwanesaich 14:bf2810b95f25 401 switch(etat) {
marwanesaich 14:bf2810b95f25 402
marwanesaich 14:bf2810b95f25 403 case init :
marwanesaich 16:06463f24811d 404 velocityControl(stockage_G,vInit,BLED_ON,1);
marwanesaich 14:bf2810b95f25 405 wait_us(500);
marwanesaich 8:7bd34e838ca3 406 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 407 wait_us(500);
marwanesaich 14:bf2810b95f25 408 if(previous_pos != pos) {
marwanesaich 8:7bd34e838ca3 409 etat = etalonnage;
marwanesaich 14:bf2810b95f25 410 // pc.printf("ETALONNAGE\n");
marwanesaich 8:7bd34e838ca3 411 }
marwanesaich 8:7bd34e838ca3 412 break;
marwanesaich 8:7bd34e838ca3 413
marwanesaich 8:7bd34e838ca3 414 case etalonnage :
marwanesaich 8:7bd34e838ca3 415 pos = getPos(stockage_G,1);
marwanesaich 16:06463f24811d 416 if(((21<pos && pos<228)/* || (572<pos && pos<980)*/) and (status_contact & MASK_FC_GAUCHE)) { //!!!!!!!!!!!!!changer les bornes
marwanesaich 8:7bd34e838ca3 417 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 8:7bd34e838ca3 418 wait_ms(500);
marwanesaich 8:7bd34e838ca3 419 pos = getPos(stockage_G,1) ;
marwanesaich 16:06463f24811d 420 //pc.printf("pos depart: %d\n",pos);
marwanesaich 10:fba0699b7a74 421 wait_us(500);
marwanesaich 16:06463f24811d 422 pos_pret = pos + 500;
marwanesaich 16:06463f24811d 423 if(pos_pret >1105) pos_pret -= 1105;
marwanesaich 15:1fefa6b1569b 424
marwanesaich 16:06463f24811d 425 //pc.printf("pos : %d consigne : %d\n",pos,pos_pret);
marwanesaich 16:06463f24811d 426 compteTour(stockage_G,1023,1,pos_pret,BLED_ON,1);
marwanesaich 10:fba0699b7a74 427 wait_us(500);
marwanesaich 8:7bd34e838ca3 428 etat = tmp_pret;
marwanesaich 10:fba0699b7a74 429 ////pc.printf("TMP_PRET\n");
marwanesaich 8:7bd34e838ca3 430 }
marwanesaich 8:7bd34e838ca3 431 break;
marwanesaich 14:bf2810b95f25 432
marwanesaich 8:7bd34e838ca3 433 case tmp_pret :
marwanesaich 8:7bd34e838ca3 434 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 435 wait_us(500);
marwanesaich 16:06463f24811d 436 if(pos> (pos_pret-5)) {
marwanesaich 14:bf2810b95f25 437 etat = pret;
marwanesaich 14:bf2810b95f25 438 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 439 }
marwanesaich 8:7bd34e838ca3 440 break;
marwanesaich 14:bf2810b95f25 441
marwanesaich 14:bf2810b95f25 442 case pret :
marwanesaich 14:bf2810b95f25 443 if(flag_vide_bleu) {
marwanesaich 14:bf2810b95f25 444 flag_vide_bleu =0;
marwanesaich 16:06463f24811d 445 vInit = -1023;
marwanesaich 16:06463f24811d 446 etat = init;
marwanesaich 14:bf2810b95f25 447 } else if((status_contact & MASK_CT_GAUCHE)) {
marwanesaich 16:06463f24811d 448 consigne_pos = (pos_pret- 550);
marwanesaich 16:06463f24811d 449 if(consigne_pos<0)consigne_pos+=1105;
marwanesaich 10:fba0699b7a74 450 wait_us(500);
marwanesaich 8:7bd34e838ca3 451 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 452 wait_us(500);
marwanesaich 16:06463f24811d 453 //pc.printf("pos:%d consigne:%d\n",pos, consigne_pos);
marwanesaich 16:06463f24811d 454
marwanesaich 16:06463f24811d 455 //compteTour(stockage_G,-1023,0,consigne_pos,BLED_ON,1);
marwanesaich 11:dfff2e200d0c 456 positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
marwanesaich 10:fba0699b7a74 457 wait_us(500);
marwanesaich 8:7bd34e838ca3 458 etat = pousse;
marwanesaich 10:fba0699b7a74 459 //pc.printf("POUSSE\n");
marwanesaich 8:7bd34e838ca3 460 }
marwanesaich 8:7bd34e838ca3 461 break;
marwanesaich 14:bf2810b95f25 462
marwanesaich 8:7bd34e838ca3 463 case pousse :
marwanesaich 8:7bd34e838ca3 464 pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 465 wait_us(500);
marwanesaich 14:bf2810b95f25 466 if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0) {
marwanesaich 13:0b5310c6dc44 467 //pc.printf("%d\n",pos);
marwanesaich 8:7bd34e838ca3 468 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 469 wait_us(500);
marwanesaich 16:06463f24811d 470
marwanesaich 16:06463f24811d 471 //compteTour(stockage_G,1023,0,pos_pret,BLED_ON,1);
marwanesaich 16:06463f24811d 472 positionControl(stockage_G,pos_pret,1,BLED_ON,1);
marwanesaich 10:fba0699b7a74 473 wait_us(500);
marwanesaich 8:7bd34e838ca3 474 etat = retour;
marwanesaich 10:fba0699b7a74 475 //pc.printf("RETOUR\n");
marwanesaich 8:7bd34e838ca3 476 }
marwanesaich 14:bf2810b95f25 477
marwanesaich 8:7bd34e838ca3 478 break;
marwanesaich 14:bf2810b95f25 479
marwanesaich 8:7bd34e838ca3 480 case retour :
marwanesaich 8:7bd34e838ca3 481 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 482 wait_us(500);
marwanesaich 16:06463f24811d 483 if(pos> (pos_pret-5)) {
marwanesaich 14:bf2810b95f25 484 etat = pret;
marwanesaich 14:bf2810b95f25 485 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 486 }
marwanesaich 8:7bd34e838ca3 487 break;
marwanesaich 14:bf2810b95f25 488
marwanesaich 14:bf2810b95f25 489 case vide:
marwanesaich 14:bf2810b95f25 490 consigne_pos = getPos(stockage_G,1);
marwanesaich 14:bf2810b95f25 491 compteTour(stockage_G,-1023,2,consigne_pos,BLED_ON,1);
marwanesaich 14:bf2810b95f25 492 etat = fin_vide;
marwanesaich 14:bf2810b95f25 493 break;
marwanesaich 14:bf2810b95f25 494 case fin_vide:
marwanesaich 14:bf2810b95f25 495 pos = getPos(stockage_G,1) ;
marwanesaich 14:bf2810b95f25 496 wait_us(500);
marwanesaich 14:bf2810b95f25 497 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 498 etat = init;
marwanesaich 14:bf2810b95f25 499 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 500 }
marwanesaich 14:bf2810b95f25 501
marwanesaich 14:bf2810b95f25 502 break;
marwanesaich 14:bf2810b95f25 503 }
marwanesaich 8:7bd34e838ca3 504 }
marwanesaich 8:7bd34e838ca3 505
marwanesaich 14:bf2810b95f25 506 void convoyeur_droit_violet(void)
marwanesaich 14:bf2810b95f25 507 {
marwanesaich 14:bf2810b95f25 508
marwanesaich 16:06463f24811d 509 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour, vide, fin_vide,tmp_vide} type_etat ;
marwanesaich 9:05da4300730a 510 static type_etat etat = init;
marwanesaich 16:06463f24811d 511 static int16_t pos = 0, consigne_pos,consigne_roue;
marwanesaich 9:05da4300730a 512 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 16:06463f24811d 513 static int cpt_vider=0;
marwanesaich 16:06463f24811d 514 static int pos_vide[5] = {400, 1000, 600, 200, 800};
marwanesaich 14:bf2810b95f25 515 /*
marwanesaich 14:bf2810b95f25 516 if(flag_vide_vert_rouge){
marwanesaich 14:bf2810b95f25 517 etat = init;
marwanesaich 14:bf2810b95f25 518 flag_vide_vert_rouge = 0;
marwanesaich 14:bf2810b95f25 519 } */
marwanesaich 14:bf2810b95f25 520
marwanesaich 14:bf2810b95f25 521 switch(etat) {
marwanesaich 14:bf2810b95f25 522
marwanesaich 14:bf2810b95f25 523 case init :
marwanesaich 9:05da4300730a 524 velocityControl(stockage_D,+512,BLED_ON,3);
marwanesaich 14:bf2810b95f25 525 wait_us(500);
marwanesaich 9:05da4300730a 526 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 527 wait_us(500);
marwanesaich 14:bf2810b95f25 528 if(previous_pos != pos) {
marwanesaich 10:fba0699b7a74 529 //pc.printf("ETALONNAGE\n");
marwanesaich 9:05da4300730a 530 etat = etalonnage;
marwanesaich 9:05da4300730a 531 }
marwanesaich 9:05da4300730a 532 break;
Artiom 0:f900f4ea9dfd 533
marwanesaich 9:05da4300730a 534 case etalonnage :
marwanesaich 9:05da4300730a 535 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 536 wait_us(500);
marwanesaich 10:fba0699b7a74 537 //pc.printf("%d\n", pos);
marwanesaich 14:bf2810b95f25 538 if( /*(21<pos && pos<260) ||*/ (754<pos && pos<812) and (status_contact & MASK_FC_DROIT)) { //!!!!!!!!!!!!!changer les bornes
marwanesaich 9:05da4300730a 539 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 9:05da4300730a 540 wait_ms(500);
marwanesaich 11:dfff2e200d0c 541 consigne_pos = getPos(stockage_D,3) - 180 ;
marwanesaich 10:fba0699b7a74 542 wait_us(500);
marwanesaich 11:dfff2e200d0c 543 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 544 wait_us(500);
marwanesaich 9:05da4300730a 545 etat = tmp_pret;
marwanesaich 14:bf2810b95f25 546 // pc.printf("TMP_PRET\n");
marwanesaich 9:05da4300730a 547 }
marwanesaich 9:05da4300730a 548 break;
marwanesaich 14:bf2810b95f25 549
marwanesaich 14:bf2810b95f25 550 case tmp_pret :
marwanesaich 9:05da4300730a 551 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 552 wait_us(500);
marwanesaich 14:bf2810b95f25 553 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 554 etat = pret;
marwanesaich 14:bf2810b95f25 555 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 556 }
marwanesaich 9:05da4300730a 557 break;
marwanesaich 14:bf2810b95f25 558
marwanesaich 14:bf2810b95f25 559 case pret :
marwanesaich 14:bf2810b95f25 560 if(flag_vide_vert_rouge) {
marwanesaich 14:bf2810b95f25 561 flag_vide_vert_rouge =0;
marwanesaich 14:bf2810b95f25 562 etat = vide;
marwanesaich 14:bf2810b95f25 563 } else if((status_contact & MASK_CT_DROIT)) {
marwanesaich 11:dfff2e200d0c 564 consigne_pos = getPos(stockage_D,3) - 550;
marwanesaich 10:fba0699b7a74 565 wait_us(500);
marwanesaich 9:05da4300730a 566 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 567 wait_us(500);
marwanesaich 14:bf2810b95f25 568 //pc.printf("consigne %d", consigne_pos);
marwanesaich 11:dfff2e200d0c 569 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 570 wait_us(500);
marwanesaich 9:05da4300730a 571 etat = pousse;
marwanesaich 14:bf2810b95f25 572 //pc.printf("POUSSE\n");
marwanesaich 9:05da4300730a 573 }
marwanesaich 9:05da4300730a 574 break;
marwanesaich 14:bf2810b95f25 575
marwanesaich 9:05da4300730a 576 case pousse :
marwanesaich 9:05da4300730a 577 pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 578 wait_us(500);
marwanesaich 14:bf2810b95f25 579 if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0) {
marwanesaich 14:bf2810b95f25 580 consigne_pos = pos + 550;
marwanesaich 14:bf2810b95f25 581 //pc.printf("consigne %d", consigne_pos);
marwanesaich 9:05da4300730a 582 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 583 wait_us(500);
marwanesaich 11:dfff2e200d0c 584 positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
marwanesaich 10:fba0699b7a74 585 wait_us(500);
marwanesaich 9:05da4300730a 586 etat = retour;
marwanesaich 14:bf2810b95f25 587 //pc.printf("RETOUR\n");
marwanesaich 9:05da4300730a 588 }
marwanesaich 14:bf2810b95f25 589
marwanesaich 9:05da4300730a 590 break;
marwanesaich 9:05da4300730a 591 case retour :
marwanesaich 9:05da4300730a 592 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 593 wait_us(500);
marwanesaich 14:bf2810b95f25 594 if(pos> (consigne_pos-5)) {
marwanesaich 14:bf2810b95f25 595 etat = pret;
marwanesaich 14:bf2810b95f25 596 // pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 597 }
marwanesaich 14:bf2810b95f25 598 break;
marwanesaich 16:06463f24811d 599 /*
marwanesaich 14:bf2810b95f25 600 case vide:
marwanesaich 14:bf2810b95f25 601 consigne_pos = getPos(stockage_D,3);
marwanesaich 14:bf2810b95f25 602 //pc.printf("consigne %d",consigne_pos);
marwanesaich 14:bf2810b95f25 603 compteTour(stockage_D,-1023,3,consigne_pos,BLED_ON,3);
marwanesaich 14:bf2810b95f25 604 etat = fin_vide;
marwanesaich 9:05da4300730a 605 break;
marwanesaich 14:bf2810b95f25 606
marwanesaich 14:bf2810b95f25 607 case fin_vide:
marwanesaich 14:bf2810b95f25 608 pos = getPos(stockage_D,3) ;
marwanesaich 14:bf2810b95f25 609 //pc.printf("pos %d", pos);
marwanesaich 14:bf2810b95f25 610 wait_us(500);
marwanesaich 14:bf2810b95f25 611 if(pos< (consigne_pos+5)) {
marwanesaich 14:bf2810b95f25 612 etat = init;
marwanesaich 14:bf2810b95f25 613 //pc.printf("PRET\n");
marwanesaich 14:bf2810b95f25 614 }
marwanesaich 14:bf2810b95f25 615
marwanesaich 14:bf2810b95f25 616 break;
marwanesaich 16:06463f24811d 617 */
marwanesaich 16:06463f24811d 618 case vide:
marwanesaich 16:06463f24811d 619
marwanesaich 16:06463f24811d 620 velocityControl(stockage_D, -400, BLED_ON,3);
marwanesaich 16:06463f24811d 621 wait_ms(200);
marwanesaich 16:06463f24811d 622 roue(1);
marwanesaich 16:06463f24811d 623
marwanesaich 16:06463f24811d 624 if(cpt_vider<9){
marwanesaich 16:06463f24811d 625 etat = tmp_vide;
marwanesaich 16:06463f24811d 626 //pc.printf("TMP_VIDE\n");
marwanesaich 16:06463f24811d 627
marwanesaich 16:06463f24811d 628 }else{
marwanesaich 16:06463f24811d 629 etat = init;
marwanesaich 16:06463f24811d 630 cpt_vider = 0;
marwanesaich 16:06463f24811d 631 roue(0);
marwanesaich 16:06463f24811d 632 //pc.printf("INIT\n");
marwanesaich 16:06463f24811d 633 }
marwanesaich 16:06463f24811d 634 break;
marwanesaich 16:06463f24811d 635
marwanesaich 16:06463f24811d 636 case tmp_vide:
marwanesaich 16:06463f24811d 637 pos = getPos(stockage_D,3);
marwanesaich 16:06463f24811d 638 wait_us(100);
marwanesaich 16:06463f24811d 639 if(pos <= pos_vide[cpt_vider%5] && pos >= (pos_vide[cpt_vider%5]-20)){ //if(pos == pos_vide[cpt_vider%5])
marwanesaich 16:06463f24811d 640 velocityControl(stockage_D, 0, BLED_ON,3);
marwanesaich 16:06463f24811d 641 positionControl(stockage_D,pos+30,1,BLED_ON,3);
marwanesaich 16:06463f24811d 642 //pc.printf("pos :%d\n", pos);
marwanesaich 16:06463f24811d 643 consigne_roue = 225;
marwanesaich 16:06463f24811d 644 positionControl(roue_D,consigne_roue,1,BLED_ON,3);
marwanesaich 16:06463f24811d 645 etat = fin_vide;
marwanesaich 16:06463f24811d 646 }
marwanesaich 16:06463f24811d 647
marwanesaich 16:06463f24811d 648 break;
marwanesaich 16:06463f24811d 649
marwanesaich 16:06463f24811d 650 case fin_vide:
marwanesaich 16:06463f24811d 651 pos = getPos(roue_D,3) ;
marwanesaich 16:06463f24811d 652 wait_us(500);
marwanesaich 16:06463f24811d 653 if(pos> (consigne_roue+5)) {
marwanesaich 16:06463f24811d 654 positionControl(roue_D,374,50,BLED_ON,3);
marwanesaich 16:06463f24811d 655 verification();
marwanesaich 16:06463f24811d 656 cpt_vider ++;
marwanesaich 16:06463f24811d 657 etat = vide;
marwanesaich 16:06463f24811d 658 }
marwanesaich 16:06463f24811d 659
marwanesaich 16:06463f24811d 660 break;
marwanesaich 14:bf2810b95f25 661 }
marwanesaich 14:bf2810b95f25 662 }
kyxstark 3:a630a1ccf5f0 663 void gabarit_robot(void)
Artiom 0:f900f4ea9dfd 664 {
Artiom 0:f900f4ea9dfd 665 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:f900f4ea9dfd 666
kyxstark 3:a630a1ccf5f0 667 uint16_t pos_av_centre[2] = {550,550};//470,350
Artiom 0:f900f4ea9dfd 668
marwanesaich 15:1fefa6b1569b 669 uint8_t servos_roulette_gauche[2] = {GLED_ON, roue_G};
marwanesaich 15:1fefa6b1569b 670 uint16_t pos_roulette_gauche[1] = {512};
marwanesaich 15:1fefa6b1569b 671
marwanesaich 15:1fefa6b1569b 672 uint8_t servos_roulette_droit[2] = {GLED_ON, roue_D};
marwanesaich 15:1fefa6b1569b 673 uint16_t pos_roulette_droit[1] = {512};
marwanesaich 15:1fefa6b1569b 674
marwanesaich 15:1fefa6b1569b 675 PWM_roue_Gauche.period(0.00005);
marwanesaich 15:1fefa6b1569b 676 PWM_roue_Droite.period(0.00005);
marwanesaich 15:1fefa6b1569b 677
Artiom 0:f900f4ea9dfd 678 int speed=100;
marwanesaich 15:1fefa6b1569b 679 roue(0);
Artiom 0:f900f4ea9dfd 680 deverouillage_torque();
kyxstark 3:a630a1ccf5f0 681 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 15:1fefa6b1569b 682 positionControl_Mul_ensemble_complex(1,speed,servos_roulette_droit,pos_roulette_droit,3);
marwanesaich 15:1fefa6b1569b 683 positionControl_Mul_ensemble_complex(1,speed,servos_roulette_gauche,pos_roulette_gauche,1);
marwanesaich 15:1fefa6b1569b 684
marwanesaich 15:1fefa6b1569b 685 SendRawId(HACHEUR_ETAT_CONTACTS);
Artiom 0:f900f4ea9dfd 686
Artiom 0:f900f4ea9dfd 687 }
Artiom 0:f900f4ea9dfd 688
Artiom 0:f900f4ea9dfd 689
Artiom 0:f900f4ea9dfd 690 void fifo_couleur(void)
Artiom 0:f900f4ea9dfd 691 {
Artiom 0:f900f4ea9dfd 692
Artiom 0:f900f4ea9dfd 693 typedef enum {n_atome, atome, tmp} type_etat ;
Artiom 0:f900f4ea9dfd 694 static type_etat etat = n_atome;
Artiom 0:f900f4ea9dfd 695
Artiom 0:f900f4ea9dfd 696 int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
Artiom 0:f900f4ea9dfd 697 switch(etat) {
Artiom 0:f900f4ea9dfd 698 case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom 0:f900f4ea9dfd 699 if(etat_cap) {
Artiom 0:f900f4ea9dfd 700 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 701 flag_ascenseur = 1;
marwanesaich 15:1fefa6b1569b 702 SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 703 if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
marwanesaich 5:bb533bf81ee6 704 oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom 0:f900f4ea9dfd 705 }
marwanesaich 14:bf2810b95f25 706
Artiom 0:f900f4ea9dfd 707 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 708 etat = tmp;
Artiom 0:f900f4ea9dfd 709 }
Artiom 0:f900f4ea9dfd 710 break;
Artiom 0:f900f4ea9dfd 711
Artiom 0:f900f4ea9dfd 712 case tmp : //on attend que l'atome traité soit totalement passé
Artiom 0:f900f4ea9dfd 713 if(!etat_cap) {
Artiom 0:f900f4ea9dfd 714 etat = n_atome;
Artiom 0:f900f4ea9dfd 715 }
Artiom 0:f900f4ea9dfd 716 break;
Artiom 0:f900f4ea9dfd 717
Artiom 0:f900f4ea9dfd 718 }
Artiom 0:f900f4ea9dfd 719
Artiom 0:f900f4ea9dfd 720 }
Artiom 0:f900f4ea9dfd 721
Artiom 0:f900f4ea9dfd 722
Artiom 0:f900f4ea9dfd 723 void ascenseur(void)
Artiom 0:f900f4ea9dfd 724 {
Artiom 0:f900f4ea9dfd 725 typedef enum {init, atome, tmp} type_etat;
Artiom 0:f900f4ea9dfd 726 static type_etat etat = init;
Artiom 0:f900f4ea9dfd 727
Artiom 0:f900f4ea9dfd 728 int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
marwanesaich 15:1fefa6b1569b 729 static int flag_dernier = 0, flag_suivant = 0;
marwanesaich 15:1fefa6b1569b 730 static long cpt;
marwanesaich 15:1fefa6b1569b 731 static char memo_FIFO_couleur_lecture;
marwanesaich 15:1fefa6b1569b 732
Artiom 0:f900f4ea9dfd 733 switch(etat) {
Artiom 0:f900f4ea9dfd 734 case init :
Artiom 0:f900f4ea9dfd 735 //on attend le premier atome et place le herkulex en fonction
marwanesaich 15:1fefa6b1569b 736 if(flag_ascenseur || flag_ascenseur_commande) {
marwanesaich 6:aa800daf5ff9 737 //SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 738 etat = atome;
Artiom 0:f900f4ea9dfd 739 }
Artiom 0:f900f4ea9dfd 740 break;
Artiom 0:f900f4ea9dfd 741
Artiom 0:f900f4ea9dfd 742 case atome :
Artiom 0:f900f4ea9dfd 743 //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
marwanesaich 15:1fefa6b1569b 744 cpt++;
marwanesaich 15:1fefa6b1569b 745 if( ((cpt > 30000)&& flag_dernier) )
marwanesaich 15:1fefa6b1569b 746 {
marwanesaich 15:1fefa6b1569b 747 flag_dernier = 0;
marwanesaich 15:1fefa6b1569b 748 oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
marwanesaich 15:1fefa6b1569b 749
marwanesaich 15:1fefa6b1569b 750 flag_ascenseur = 0;
marwanesaich 15:1fefa6b1569b 751
marwanesaich 15:1fefa6b1569b 752 /*
marwanesaich 15:1fefa6b1569b 753 if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
marwanesaich 15:1fefa6b1569b 754 oriente_doigt(2);
marwanesaich 15:1fefa6b1569b 755 else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
marwanesaich 15:1fefa6b1569b 756 oriente_doigt(1);*/
marwanesaich 15:1fefa6b1569b 757
marwanesaich 15:1fefa6b1569b 758 }else if((cpt > 5000)&& flag_suivant){
marwanesaich 15:1fefa6b1569b 759 flag_suivant =0;
marwanesaich 15:1fefa6b1569b 760 oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
marwanesaich 15:1fefa6b1569b 761 }else if( (cpt>60000) && !flag_ascenseur && !flag_ascenseur_commande){
marwanesaich 15:1fefa6b1569b 762 SendCharCan(HACHEUR_ID_COUROIES,0);
marwanesaich 15:1fefa6b1569b 763 etat = init;
marwanesaich 15:1fefa6b1569b 764 }
marwanesaich 16:06463f24811d 765
marwanesaich 14:bf2810b95f25 766 if(etat_cap != 0) {
marwanesaich 15:1fefa6b1569b 767 cpt = 0;
marwanesaich 16:06463f24811d 768 if(etat_cap == 1)nbPaletsBleus ++;
marwanesaich 16:06463f24811d 769 else if(etat_cap == 2) nbPaletsVerts ++;
marwanesaich 7:283d66d5c3b9 770 oriente_doigt(etat_cap);
marwanesaich 6:aa800daf5ff9 771 etat = tmp;
marwanesaich 6:aa800daf5ff9 772 }
marwanesaich 10:fba0699b7a74 773 /*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
marwanesaich 10:fba0699b7a74 774 {
marwanesaich 10:fba0699b7a74 775 oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO);
marwanesaich 6:aa800daf5ff9 776 }*/
Artiom 0:f900f4ea9dfd 777 break;
Artiom 0:f900f4ea9dfd 778
Artiom 0:f900f4ea9dfd 779 case tmp :
Artiom 0:f900f4ea9dfd 780 //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom 0:f900f4ea9dfd 781 if(etat_cap == 0) {
marwanesaich 15:1fefa6b1569b 782 memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
marwanesaich 16:06463f24811d 783
Artiom 0:f900f4ea9dfd 784 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
marwanesaich 14:bf2810b95f25 785 if(FIFO_couleur_lecture == FIFO_couleur_ecriture) {
marwanesaich 14:bf2810b95f25 786 //pc.printf("Fifo vide\n");
marwanesaich 15:1fefa6b1569b 787 flag_dernier = 1;/*
marwanesaich 10:fba0699b7a74 788 if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
marwanesaich 10:fba0699b7a74 789 oriente_doigt(2);
marwanesaich 10:fba0699b7a74 790 else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
marwanesaich 15:1fefa6b1569b 791 oriente_doigt(1);*/
marwanesaich 15:1fefa6b1569b 792 }else{
marwanesaich 15:1fefa6b1569b 793 flag_suivant = 1;
marwanesaich 15:1fefa6b1569b 794 /*if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
marwanesaich 15:1fefa6b1569b 795 oriente_doigt(2);
marwanesaich 15:1fefa6b1569b 796 else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
marwanesaich 15:1fefa6b1569b 797 oriente_doigt(1);*/
marwanesaich 10:fba0699b7a74 798 }
Artiom 0:f900f4ea9dfd 799 etat = atome;
Artiom 0:f900f4ea9dfd 800 }
Artiom 0:f900f4ea9dfd 801 break;
Artiom 0:f900f4ea9dfd 802 }
Artiom 0:f900f4ea9dfd 803 }
kyxstark 1:af508f84a079 804
marwanesaich 5:bb533bf81ee6 805 void oriente_doigt(int palet)
marwanesaich 5:bb533bf81ee6 806 {
marwanesaich 5:bb533bf81ee6 807 if(cote) {
marwanesaich 5:bb533bf81ee6 808 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 809 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 15:1fefa6b1569b 810 positionControl(doigt,POS_DOIGT_GAUCHE,20,BLED_ON,2);
marwanesaich 5:bb533bf81ee6 811 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 812 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 15:1fefa6b1569b 813 positionControl(doigt,POS_DOIGT_DROIT,20,GLED_ON,2);
marwanesaich 14:bf2810b95f25 814
marwanesaich 15:1fefa6b1569b 815 }else
marwanesaich 15:1fefa6b1569b 816 positionControl(doigt,512,20,GLED_ON,2);
marwanesaich 5:bb533bf81ee6 817 } else {
marwanesaich 5:bb533bf81ee6 818 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 15:1fefa6b1569b 819 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 6:aa800daf5ff9 820 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
marwanesaich 5:bb533bf81ee6 821 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 15:1fefa6b1569b 822 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 6:aa800daf5ff9 823 positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
marwanesaich 15:1fefa6b1569b 824 }else
marwanesaich 15:1fefa6b1569b 825 positionControl(doigt,512,20,GLED_ON,2);
marwanesaich 5:bb533bf81ee6 826 }
marwanesaich 5:bb533bf81ee6 827 }
marwanesaich 5:bb533bf81ee6 828
marwanesaich 15:1fefa6b1569b 829 void oriente_doigt_suiv(int palet)
marwanesaich 15:1fefa6b1569b 830 {
marwanesaich 15:1fefa6b1569b 831 if(cote) {
marwanesaich 15:1fefa6b1569b 832 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 15:1fefa6b1569b 833 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 15:1fefa6b1569b 834 positionControl(doigt,POS_DOIGT_GAUCHE_SUIV,20,BLED_ON,2);
marwanesaich 15:1fefa6b1569b 835 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 15:1fefa6b1569b 836 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 15:1fefa6b1569b 837 positionControl(doigt,POS_DOIGT_DROIT_SUIV,20,GLED_ON,2);
marwanesaich 15:1fefa6b1569b 838
marwanesaich 15:1fefa6b1569b 839 }else
marwanesaich 15:1fefa6b1569b 840 positionControl(doigt,512,20,GLED_ON,2);
marwanesaich 15:1fefa6b1569b 841 } else {
marwanesaich 15:1fefa6b1569b 842 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 15:1fefa6b1569b 843 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 15:1fefa6b1569b 844 positionControl(doigt,POS_DOIGT_DROIT_SUIV,10,BLED_ON,2);
marwanesaich 15:1fefa6b1569b 845 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 15:1fefa6b1569b 846 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 15:1fefa6b1569b 847 positionControl(doigt,POS_DOIGT_GAUCHE_SUIV,10,GLED_ON,2);
marwanesaich 15:1fefa6b1569b 848 }else
marwanesaich 15:1fefa6b1569b 849 positionControl(doigt,512,20,GLED_ON,2);
marwanesaich 15:1fefa6b1569b 850 }
marwanesaich 15:1fefa6b1569b 851 }
Artiom 0:f900f4ea9dfd 852 void presentoir_avant(void)
Artiom 0:f900f4ea9dfd 853 {
kyxstark 2:33583329d6c8 854 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
kyxstark 2:33583329d6c8 855
kyxstark 3:a630a1ccf5f0 856 uint16_t pos_av_centre[2] = {512,512};
kyxstark 2:33583329d6c8 857
kyxstark 2:33583329d6c8 858 int speed=1;
kyxstark 2:33583329d6c8 859
kyxstark 2:33583329d6c8 860 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
kyxstark 2:33583329d6c8 861 verification();
Artiom 0:f900f4ea9dfd 862 }
Artiom 0:f900f4ea9dfd 863
Artiom 0:f900f4ea9dfd 864
Artiom 0:f900f4ea9dfd 865 void automate_ventouse_presentoir_avant(void)
Artiom 0:f900f4ea9dfd 866 {
kyxstark 2:33583329d6c8 867 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 868 static type_etat etat = init;
kyxstark 2:33583329d6c8 869
kyxstark 2:33583329d6c8 870 switch(etat) {
kyxstark 2:33583329d6c8 871 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 872 if(fpresentoir_avant)
kyxstark 2:33583329d6c8 873 etat=envoi_instruction;
kyxstark 2:33583329d6c8 874 break;
kyxstark 2:33583329d6c8 875
kyxstark 2:33583329d6c8 876 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 877 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 878 presentoir_avant();
kyxstark 2:33583329d6c8 879 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
kyxstark 2:33583329d6c8 880 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 881 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 882 break;
kyxstark 2:33583329d6c8 883
kyxstark 2:33583329d6c8 884 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 885 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
kyxstark 2:33583329d6c8 886 fpresentoir_avant=0;
kyxstark 2:33583329d6c8 887 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 888 etat = init;
kyxstark 2:33583329d6c8 889 }
kyxstark 2:33583329d6c8 890 break;
kyxstark 2:33583329d6c8 891
kyxstark 2:33583329d6c8 892 }
Artiom 0:f900f4ea9dfd 893 }
Artiom 0:f900f4ea9dfd 894
Artiom 0:f900f4ea9dfd 895 void automate_ventouse_relache_avant(void)
Artiom 0:f900f4ea9dfd 896 {
Artiom 0:f900f4ea9dfd 897
kyxstark 2:33583329d6c8 898 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 899 static type_etat etat = init;
kyxstark 2:33583329d6c8 900
kyxstark 2:33583329d6c8 901 switch(etat) {
kyxstark 2:33583329d6c8 902 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 903 if(favant_relache)
kyxstark 2:33583329d6c8 904 etat=envoi_instruction;
kyxstark 2:33583329d6c8 905 break;
kyxstark 2:33583329d6c8 906
kyxstark 2:33583329d6c8 907 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 908 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 909 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 910 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 911 break;
kyxstark 2:33583329d6c8 912
kyxstark 2:33583329d6c8 913 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 914 SendRawId(HACHEUR_RELEASE_AV);
kyxstark 2:33583329d6c8 915 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
kyxstark 2:33583329d6c8 916 favant_relache=0;
kyxstark 2:33583329d6c8 917 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 918 etat = init;
kyxstark 2:33583329d6c8 919 }
kyxstark 2:33583329d6c8 920 break;
kyxstark 2:33583329d6c8 921
kyxstark 2:33583329d6c8 922 }
Artiom 0:f900f4ea9dfd 923
Artiom 0:f900f4ea9dfd 924 }
Artiom 0:f900f4ea9dfd 925
Artiom 0:f900f4ea9dfd 926 void goldenium_avant(void)
Artiom 0:f900f4ea9dfd 927 {
marwanesaich 15:1fefa6b1569b 928 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
marwanesaich 15:1fefa6b1569b 929 uint16_t pos_av_centre[2] = {165,165};
marwanesaich 15:1fefa6b1569b 930 int speed=25;
Artiom 0:f900f4ea9dfd 931
marwanesaich 15:1fefa6b1569b 932 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 15:1fefa6b1569b 933 verification();
Artiom 0:f900f4ea9dfd 934 }
Artiom 0:f900f4ea9dfd 935
Artiom 0:f900f4ea9dfd 936 void automate_ventouse_goldenium_avant (void)
Artiom 0:f900f4ea9dfd 937 {
marwanesaich 15:1fefa6b1569b 938 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
marwanesaich 15:1fefa6b1569b 939 static type_etat etat = init;
marwanesaich 15:1fefa6b1569b 940
marwanesaich 15:1fefa6b1569b 941 switch(etat) {
marwanesaich 15:1fefa6b1569b 942 case init: //attente d'initialisation
marwanesaich 15:1fefa6b1569b 943 if(fgoldenium_avant)
marwanesaich 15:1fefa6b1569b 944 etat=envoi_instruction;
marwanesaich 15:1fefa6b1569b 945 break;
marwanesaich 15:1fefa6b1569b 946
marwanesaich 15:1fefa6b1569b 947 case envoi_instruction://envoi instruction
marwanesaich 15:1fefa6b1569b 948 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
marwanesaich 15:1fefa6b1569b 949 goldenium_avant();
marwanesaich 15:1fefa6b1569b 950 char pompe=AV_CENTRE;
marwanesaich 15:1fefa6b1569b 951 can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
marwanesaich 15:1fefa6b1569b 952 SendRawId(HACHEUR_STATUT_VENTOUSES);
marwanesaich 15:1fefa6b1569b 953 GoStraight(distance_goldenium,0,0,0);
marwanesaich 15:1fefa6b1569b 954 etat = attente_ack_ventouse;
marwanesaich 15:1fefa6b1569b 955 break;
marwanesaich 15:1fefa6b1569b 956
marwanesaich 15:1fefa6b1569b 957 case attente_ack_ventouse:
marwanesaich 15:1fefa6b1569b 958 if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
marwanesaich 15:1fefa6b1569b 959 wait(1);
marwanesaich 15:1fefa6b1569b 960 GoStraight(-distance_goldenium,0,0,0);
marwanesaich 15:1fefa6b1569b 961 positionControl(AV_poigne_C,470,100,BLED_ON,2);//forklift
marwanesaich 15:1fefa6b1569b 962 verification();
marwanesaich 15:1fefa6b1569b 963 fgoldenium_avant=0;
marwanesaich 15:1fefa6b1569b 964 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
marwanesaich 15:1fefa6b1569b 965 etat = init;
marwanesaich 15:1fefa6b1569b 966 }
marwanesaich 15:1fefa6b1569b 967 break;
marwanesaich 15:1fefa6b1569b 968
marwanesaich 15:1fefa6b1569b 969 }
Artiom 0:f900f4ea9dfd 970 }
Artiom 0:f900f4ea9dfd 971
Artiom 0:f900f4ea9dfd 972 void accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 973 {
Artiom 0:f900f4ea9dfd 974 }
Artiom 0:f900f4ea9dfd 975
Artiom 0:f900f4ea9dfd 976 void automate_ventouse_accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 977 {
Artiom 0:f900f4ea9dfd 978
kyxstark 1:af508f84a079 979 }
Artiom 0:f900f4ea9dfd 980 #endif