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@19:2281a0ef54e4, 2019-05-31 (annotated)

Committer:
marwanesaich
Date:
Fri May 31 18:55:17 2019 +0000
Revision:
19:2281a0ef54e4
Parent:
18:2082875295df
Child:
20:109dbdd0230b
Child:
22:a0af2f50ff1d
ajout rateau et roulette rentree;

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