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@17:5973f13f44e2, 2019-05-29 (annotated)

Committer:
marwanesaich
Date:
Wed May 29 17:18:16 2019 +0000
Revision:
17:5973f13f44e2
Parent:
16:06463f24811d
Child:
18:2082875295df
ascenseur force_on/off et auto;

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