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@15:1fefa6b1569b, 2019-05-27 (annotated)

Committer:
marwanesaich
Date:
Mon May 27 22:59:28 2019 +0000
Revision:
15:1fefa6b1569b
Parent:
14:bf2810b95f25
Child:
16:06463f24811d
ajout roulettes MOS accelerateur;

Who changed what in which revision?

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