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@18:2082875295df, 2019-05-31 (annotated)

Committer:
marwanesaich
Date:
Fri May 31 05:39:55 2019 +0000
Revision:
18:2082875295df
Parent:
17:5973f13f44e2
Child:
19:2281a0ef54e4
points automatiques, goldenium, debloquage roulette

Who changed what in which revision?

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