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@8:7bd34e838ca3, 2019-05-25 (annotated)

Committer:
marwanesaich
Date:
Sat May 25 16:35:36 2019 +0000
Revision:
8:7bd34e838ca3
Parent:
7:283d66d5c3b9
Child:
9:05da4300730a
ajout convoyeur gauche violet ;

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 6:aa800daf5ff9 26 #define POS_DOIGT_GAUCHE 585
marwanesaich 6:aa800daf5ff9 27 #define POS_DOIGT_DROIT 389
Artiom 0:f900f4ea9dfd 28
Artiom 0:f900f4ea9dfd 29 char status_pompe=0;
Artiom 0:f900f4ea9dfd 30 bool flag_ascenseur = 0;
Artiom 0:f900f4ea9dfd 31
Artiom 0:f900f4ea9dfd 32 char fpresentoir_avant=0, fpresentoir_arriere=0;
Artiom 0:f900f4ea9dfd 33 char fgoldenium_avant=0, fgoldenium_arriere=0;
Artiom 0:f900f4ea9dfd 34 char fsol_avant=0,fsol_arriere=0;
Artiom 0:f900f4ea9dfd 35 char fsol_avant_relache=0,fsol_arriere_relache=0;
Artiom 0:f900f4ea9dfd 36 char fbalance_avant=0,fbalance_arriere=0;
Artiom 0:f900f4ea9dfd 37 char favant_relache=0,farriere_relache=0;
Artiom 0:f900f4ea9dfd 38 char faccelerateur_avant=0,faccelerateur_arriere=0;
Artiom 0:f900f4ea9dfd 39
Artiom 0:f900f4ea9dfd 40
Artiom 0:f900f4ea9dfd 41 DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
Artiom 0:f900f4ea9dfd 42 DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
Artiom 0:f900f4ea9dfd 43
Artiom 0:f900f4ea9dfd 44 char buffer_couleur_bas[SIZE_FIFO];
Artiom 0:f900f4ea9dfd 45 unsigned char FIFO_couleur_ecriture=0;
Artiom 0:f900f4ea9dfd 46 signed char FIFO_couleur_lecture=0;
marwanesaich 5:bb533bf81ee6 47 char status_contact=0;
Artiom 0:f900f4ea9dfd 48
Artiom 0:f900f4ea9dfd 49
marwanesaich 7:283d66d5c3b9 50 void convoyeur_gauche_jaune(void){
marwanesaich 5:bb533bf81ee6 51
marwanesaich 5:bb533bf81ee6 52 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
marwanesaich 5:bb533bf81ee6 53 static type_etat etat = init;
marwanesaich 5:bb533bf81ee6 54 static int16_t pos = 0;
marwanesaich 5:bb533bf81ee6 55 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 5:bb533bf81ee6 56
marwanesaich 5:bb533bf81ee6 57
marwanesaich 5:bb533bf81ee6 58 switch(etat){
marwanesaich 5:bb533bf81ee6 59
marwanesaich 5:bb533bf81ee6 60 case init :
marwanesaich 5:bb533bf81ee6 61 velocityControl(stockage_G,-512,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 62 pos = getPos(stockage_G,1);
marwanesaich 5:bb533bf81ee6 63 if(previous_pos != pos){
marwanesaich 5:bb533bf81ee6 64 etat = etalonnage;
marwanesaich 6:aa800daf5ff9 65 //pc.printf("ETALONNAGE\n");
marwanesaich 5:bb533bf81ee6 66 }
marwanesaich 5:bb533bf81ee6 67 break;
Artiom 0:f900f4ea9dfd 68
marwanesaich 5:bb533bf81ee6 69 case etalonnage :
marwanesaich 5:bb533bf81ee6 70 pos = getPos(stockage_G,1);
marwanesaich 5:bb533bf81ee6 71 if(pos<100 and (status_contact & MASK_FC_GAUCHE)){
marwanesaich 5:bb533bf81ee6 72 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 5:bb533bf81ee6 73 wait_ms(500);
marwanesaich 5:bb533bf81ee6 74 pos = getPos(stockage_G,1) ;
marwanesaich 5:bb533bf81ee6 75 positionControl(stockage_G,pos+180,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 76 etat = tmp_pret;
marwanesaich 6:aa800daf5ff9 77 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 78 }
marwanesaich 5:bb533bf81ee6 79 break;
marwanesaich 5:bb533bf81ee6 80
marwanesaich 5:bb533bf81ee6 81 case tmp_pret :
marwanesaich 5:bb533bf81ee6 82 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 83 pos = getPos(stockage_G,1) ;
marwanesaich 5:bb533bf81ee6 84 if(pos> (previous_pos-5)){
marwanesaich 5:bb533bf81ee6 85 etat = pret;
marwanesaich 6:aa800daf5ff9 86 //pc.printf("PRET\n");
marwanesaich 5:bb533bf81ee6 87 }
marwanesaich 5:bb533bf81ee6 88 break;
marwanesaich 5:bb533bf81ee6 89
marwanesaich 5:bb533bf81ee6 90 case pret :
marwanesaich 5:bb533bf81ee6 91 if((status_contact & MASK_CT_GAUCHE)){
marwanesaich 5:bb533bf81ee6 92 pos = getPos(stockage_G,1)+550;
marwanesaich 6:aa800daf5ff9 93 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 94 wait_us(500);
marwanesaich 5:bb533bf81ee6 95 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 96 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 97 etat = pousse;
marwanesaich 6:aa800daf5ff9 98 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 99 }
marwanesaich 5:bb533bf81ee6 100 break;
marwanesaich 5:bb533bf81ee6 101
marwanesaich 5:bb533bf81ee6 102 case pousse :
marwanesaich 5:bb533bf81ee6 103 pos = getPos(stockage_G,1);
marwanesaich 5:bb533bf81ee6 104 if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){
marwanesaich 5:bb533bf81ee6 105 pos = pos - 550;
marwanesaich 6:aa800daf5ff9 106 deverouillage_torque_convoyeurs_gauche();
marwanesaich 6:aa800daf5ff9 107 wait_us(500);
marwanesaich 5:bb533bf81ee6 108 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 5:bb533bf81ee6 109 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 110 etat = retour;
marwanesaich 6:aa800daf5ff9 111 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 112 }
marwanesaich 5:bb533bf81ee6 113
marwanesaich 5:bb533bf81ee6 114 break;
marwanesaich 5:bb533bf81ee6 115 case retour :
marwanesaich 5:bb533bf81ee6 116 pos = getPos(stockage_G,1) ;
marwanesaich 5:bb533bf81ee6 117 if(pos> (previous_pos-5)){
marwanesaich 5:bb533bf81ee6 118 etat = pret;
marwanesaich 6:aa800daf5ff9 119 //pc.printf("PRET\n");
marwanesaich 5:bb533bf81ee6 120 }
marwanesaich 5:bb533bf81ee6 121 break;
marwanesaich 5:bb533bf81ee6 122 }
marwanesaich 5:bb533bf81ee6 123 }
marwanesaich 5:bb533bf81ee6 124
marwanesaich 7:283d66d5c3b9 125 void convoyeur_droit_jaune(void){
marwanesaich 5:bb533bf81ee6 126
marwanesaich 5:bb533bf81ee6 127 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
marwanesaich 5:bb533bf81ee6 128 static type_etat etat = init;
marwanesaich 5:bb533bf81ee6 129 static int16_t pos = 0;
marwanesaich 5:bb533bf81ee6 130 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 5:bb533bf81ee6 131
marwanesaich 5:bb533bf81ee6 132
marwanesaich 5:bb533bf81ee6 133 switch(etat){
marwanesaich 5:bb533bf81ee6 134
marwanesaich 5:bb533bf81ee6 135 case init :
marwanesaich 5:bb533bf81ee6 136 velocityControl(stockage_D,512,BLED_ON,3);
marwanesaich 5:bb533bf81ee6 137 pos = getPos(stockage_D,3);
marwanesaich 5:bb533bf81ee6 138 if(previous_pos != pos){
marwanesaich 5:bb533bf81ee6 139 etat = etalonnage;
marwanesaich 6:aa800daf5ff9 140 //pc.printf("ETALONNAGE\n");
marwanesaich 5:bb533bf81ee6 141 }
marwanesaich 5:bb533bf81ee6 142 break;
marwanesaich 5:bb533bf81ee6 143
marwanesaich 5:bb533bf81ee6 144 case etalonnage :
marwanesaich 5:bb533bf81ee6 145 pos = getPos(stockage_D,3);
marwanesaich 6:aa800daf5ff9 146 if(pos>600 and (status_contact & MASK_FC_DROIT)){
marwanesaich 5:bb533bf81ee6 147 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 5:bb533bf81ee6 148 wait_ms(500);
marwanesaich 5:bb533bf81ee6 149 pos = getPos(stockage_D,3) ;
marwanesaich 5:bb533bf81ee6 150 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
marwanesaich 5:bb533bf81ee6 151 compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
marwanesaich 5:bb533bf81ee6 152 etat = tmp_pret;
marwanesaich 6:aa800daf5ff9 153 //pc.printf("TMP_PRET\n");
marwanesaich 5:bb533bf81ee6 154 }
marwanesaich 5:bb533bf81ee6 155 break;
marwanesaich 5:bb533bf81ee6 156
marwanesaich 5:bb533bf81ee6 157 case tmp_pret :
marwanesaich 5:bb533bf81ee6 158 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 159 pos = getPos(stockage_D,3) ;
marwanesaich 5:bb533bf81ee6 160 if(pos> (previous_pos+5)){
marwanesaich 5:bb533bf81ee6 161 etat = pret;
marwanesaich 6:aa800daf5ff9 162 //pc.printf("PRET\n");
marwanesaich 5:bb533bf81ee6 163 }
marwanesaich 5:bb533bf81ee6 164 break;
marwanesaich 5:bb533bf81ee6 165
marwanesaich 5:bb533bf81ee6 166 case pret :
marwanesaich 5:bb533bf81ee6 167 if((status_contact & MASK_CT_DROIT)){
marwanesaich 5:bb533bf81ee6 168 //pc.printf("%d\n",pos);
marwanesaich 5:bb533bf81ee6 169 pos = getPos(stockage_D,3)+ 550;
marwanesaich 6:aa800daf5ff9 170 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 171 wait_us(500);
marwanesaich 5:bb533bf81ee6 172 positionControl(stockage_D,pos,1,BLED_ON,3);
marwanesaich 5:bb533bf81ee6 173 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 174 etat = pousse;
marwanesaich 6:aa800daf5ff9 175 //pc.printf("POUSSE\n");
marwanesaich 5:bb533bf81ee6 176 }
marwanesaich 5:bb533bf81ee6 177 break;
marwanesaich 5:bb533bf81ee6 178
marwanesaich 5:bb533bf81ee6 179 case pousse :
marwanesaich 5:bb533bf81ee6 180 pos = getPos(stockage_D,3);
marwanesaich 5:bb533bf81ee6 181 if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){
marwanesaich 5:bb533bf81ee6 182 //pc.printf("%d\n",pos);
marwanesaich 5:bb533bf81ee6 183 pos = pos - 550;
marwanesaich 6:aa800daf5ff9 184 deverouillage_torque_convoyeurs_droit();
marwanesaich 6:aa800daf5ff9 185 wait_us(500);
marwanesaich 5:bb533bf81ee6 186 positionControl(stockage_D,pos,1,BLED_ON,3);
marwanesaich 5:bb533bf81ee6 187 previous_pos = pos;
marwanesaich 5:bb533bf81ee6 188 etat = retour;
marwanesaich 6:aa800daf5ff9 189 //pc.printf("RETOUR\n");
marwanesaich 5:bb533bf81ee6 190 }
marwanesaich 5:bb533bf81ee6 191
marwanesaich 5:bb533bf81ee6 192 break;
marwanesaich 5:bb533bf81ee6 193
marwanesaich 5:bb533bf81ee6 194 case retour :
marwanesaich 5:bb533bf81ee6 195 pos = getPos(stockage_D,3) ;
marwanesaich 5:bb533bf81ee6 196 if(pos< (previous_pos+5)){
marwanesaich 5:bb533bf81ee6 197 etat = pret;
marwanesaich 6:aa800daf5ff9 198 //pc.printf("PRET\n");
marwanesaich 5:bb533bf81ee6 199 }
marwanesaich 5:bb533bf81ee6 200 break;
marwanesaich 5:bb533bf81ee6 201 }
marwanesaich 5:bb533bf81ee6 202 }
Artiom 0:f900f4ea9dfd 203
marwanesaich 8:7bd34e838ca3 204 void convoyeur_gauche_violet(void){
marwanesaich 8:7bd34e838ca3 205
marwanesaich 8:7bd34e838ca3 206 typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
marwanesaich 8:7bd34e838ca3 207 static type_etat etat = init;
marwanesaich 8:7bd34e838ca3 208 static int16_t pos = 0;
marwanesaich 8:7bd34e838ca3 209 static int16_t previous_pos = getPos(stockage_G,1);
marwanesaich 8:7bd34e838ca3 210
marwanesaich 8:7bd34e838ca3 211
marwanesaich 8:7bd34e838ca3 212 switch(etat){
marwanesaich 8:7bd34e838ca3 213
marwanesaich 8:7bd34e838ca3 214 case init :
marwanesaich 8:7bd34e838ca3 215 velocityControl(stockage_G,-512,BLED_ON,1);
marwanesaich 8:7bd34e838ca3 216 pos = getPos(stockage_G,1);
marwanesaich 8:7bd34e838ca3 217 if(previous_pos != pos){
marwanesaich 8:7bd34e838ca3 218 etat = etalonnage;
marwanesaich 8:7bd34e838ca3 219 pc.printf("ETALONNAGE\n");
marwanesaich 8:7bd34e838ca3 220 }
marwanesaich 8:7bd34e838ca3 221 break;
marwanesaich 8:7bd34e838ca3 222
marwanesaich 8:7bd34e838ca3 223 case etalonnage :
marwanesaich 8:7bd34e838ca3 224 pos = getPos(stockage_G,1);
marwanesaich 8:7bd34e838ca3 225 if(pos<100 and (status_contact & MASK_FC_GAUCHE)){
marwanesaich 8:7bd34e838ca3 226 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 8:7bd34e838ca3 227 wait_ms(500);
marwanesaich 8:7bd34e838ca3 228 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 229 compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1);
marwanesaich 8:7bd34e838ca3 230 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 231 etat = tmp_pret;
marwanesaich 8:7bd34e838ca3 232 pc.printf("TMP_PRET\n");
marwanesaich 8:7bd34e838ca3 233 }
marwanesaich 8:7bd34e838ca3 234 break;
marwanesaich 8:7bd34e838ca3 235
marwanesaich 8:7bd34e838ca3 236 case tmp_pret :
marwanesaich 8:7bd34e838ca3 237 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 238 if(pos> (previous_pos-5)){
marwanesaich 8:7bd34e838ca3 239 etat = pret;
marwanesaich 8:7bd34e838ca3 240 pc.printf("PRET\n");
marwanesaich 8:7bd34e838ca3 241 }
marwanesaich 8:7bd34e838ca3 242 break;
marwanesaich 8:7bd34e838ca3 243
marwanesaich 8:7bd34e838ca3 244 case pret :
marwanesaich 8:7bd34e838ca3 245 if((status_contact & MASK_CT_GAUCHE)){
marwanesaich 8:7bd34e838ca3 246 //pc.printf("%d\n",pos);
marwanesaich 8:7bd34e838ca3 247 pos = getPos(stockage_G,1)- 550;
marwanesaich 8:7bd34e838ca3 248 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 249 wait_us(500);
marwanesaich 8:7bd34e838ca3 250 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 8:7bd34e838ca3 251 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 252 etat = pousse;
marwanesaich 8:7bd34e838ca3 253 pc.printf("POUSSE\n");
marwanesaich 8:7bd34e838ca3 254 }
marwanesaich 8:7bd34e838ca3 255 break;
marwanesaich 8:7bd34e838ca3 256
marwanesaich 8:7bd34e838ca3 257 case pousse :
marwanesaich 8:7bd34e838ca3 258 pos = getPos(stockage_G,1);
marwanesaich 8:7bd34e838ca3 259 if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){
marwanesaich 8:7bd34e838ca3 260 //pc.printf("%d\n",pos);
marwanesaich 8:7bd34e838ca3 261 pos = pos + 550;
marwanesaich 8:7bd34e838ca3 262 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 263 wait_us(500);
marwanesaich 8:7bd34e838ca3 264 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 8:7bd34e838ca3 265 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 266 etat = retour;
marwanesaich 8:7bd34e838ca3 267 pc.printf("RETOUR\n");
marwanesaich 8:7bd34e838ca3 268 }
marwanesaich 8:7bd34e838ca3 269
marwanesaich 8:7bd34e838ca3 270 break;
marwanesaich 8:7bd34e838ca3 271
marwanesaich 8:7bd34e838ca3 272 case retour :
marwanesaich 8:7bd34e838ca3 273 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 274 if(pos> (previous_pos-5)){
marwanesaich 8:7bd34e838ca3 275 etat = pret;
marwanesaich 8:7bd34e838ca3 276 pc.printf("PRET\n");
marwanesaich 8:7bd34e838ca3 277 }
marwanesaich 8:7bd34e838ca3 278 break;
marwanesaich 8:7bd34e838ca3 279 }
marwanesaich 8:7bd34e838ca3 280 }
marwanesaich 8:7bd34e838ca3 281
Artiom 0:f900f4ea9dfd 282
kyxstark 3:a630a1ccf5f0 283 void gabarit_robot(void)
Artiom 0:f900f4ea9dfd 284 {
Artiom 0:f900f4ea9dfd 285 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:f900f4ea9dfd 286
kyxstark 3:a630a1ccf5f0 287 uint16_t pos_av_centre[2] = {550,550};//470,350
Artiom 0:f900f4ea9dfd 288
Artiom 0:f900f4ea9dfd 289
Artiom 0:f900f4ea9dfd 290 int speed=100;
Artiom 0:f900f4ea9dfd 291
Artiom 0:f900f4ea9dfd 292 deverouillage_torque();
kyxstark 3:a630a1ccf5f0 293 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 6:aa800daf5ff9 294 wait(0.5);
marwanesaich 6:aa800daf5ff9 295 positionControl(doigt,POS_DOIGT_DROIT,100,BLED_ON,2);
Artiom 0:f900f4ea9dfd 296
Artiom 0:f900f4ea9dfd 297 }
Artiom 0:f900f4ea9dfd 298
Artiom 0:f900f4ea9dfd 299
Artiom 0:f900f4ea9dfd 300 void fifo_couleur(void)
Artiom 0:f900f4ea9dfd 301 {
Artiom 0:f900f4ea9dfd 302
Artiom 0:f900f4ea9dfd 303 typedef enum {n_atome, atome, tmp} type_etat ;
Artiom 0:f900f4ea9dfd 304 static type_etat etat = n_atome;
Artiom 0:f900f4ea9dfd 305
Artiom 0:f900f4ea9dfd 306 int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
Artiom 0:f900f4ea9dfd 307 switch(etat) {
Artiom 0:f900f4ea9dfd 308 case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom 0:f900f4ea9dfd 309 if(etat_cap) {
Artiom 0:f900f4ea9dfd 310 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 311 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 312 if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
marwanesaich 5:bb533bf81ee6 313 /* if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
Artiom 0:f900f4ea9dfd 314 positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu
Artiom 0:f900f4ea9dfd 315 } else {
Artiom 0:f900f4ea9dfd 316 positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert
marwanesaich 5:bb533bf81ee6 317 }*/
marwanesaich 5:bb533bf81ee6 318 oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom 0:f900f4ea9dfd 319 }
marwanesaich 5:bb533bf81ee6 320
Artiom 0:f900f4ea9dfd 321 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 322 etat = tmp;
Artiom 0:f900f4ea9dfd 323 }
Artiom 0:f900f4ea9dfd 324 break;
marwanesaich 6:aa800daf5ff9 325 /*
Artiom 0:f900f4ea9dfd 326 case atome : //on stocke dans la fifo la couleur de l'atome
Artiom 0:f900f4ea9dfd 327 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 328 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 329 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 330 etat = tmp;
marwanesaich 6:aa800daf5ff9 331 break;*/
Artiom 0:f900f4ea9dfd 332
Artiom 0:f900f4ea9dfd 333 case tmp : //on attend que l'atome traité soit totalement passé
Artiom 0:f900f4ea9dfd 334 if(!etat_cap) {
Artiom 0:f900f4ea9dfd 335 etat = n_atome;
Artiom 0:f900f4ea9dfd 336 }
Artiom 0:f900f4ea9dfd 337 break;
Artiom 0:f900f4ea9dfd 338
Artiom 0:f900f4ea9dfd 339 }
Artiom 0:f900f4ea9dfd 340
Artiom 0:f900f4ea9dfd 341 }
Artiom 0:f900f4ea9dfd 342
Artiom 0:f900f4ea9dfd 343
Artiom 0:f900f4ea9dfd 344 void ascenseur(void)
Artiom 0:f900f4ea9dfd 345 {
Artiom 0:f900f4ea9dfd 346 typedef enum {init, atome, tmp} type_etat;
Artiom 0:f900f4ea9dfd 347 static type_etat etat = init;
Artiom 0:f900f4ea9dfd 348
Artiom 0:f900f4ea9dfd 349 int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
Artiom 0:f900f4ea9dfd 350 static int flag_sortie = 0;
marwanesaich 5:bb533bf81ee6 351
Artiom 0:f900f4ea9dfd 352 switch(etat) {
Artiom 0:f900f4ea9dfd 353 case init :
Artiom 0:f900f4ea9dfd 354 //on attend le premier atome et place le herkulex en fonction
Artiom 0:f900f4ea9dfd 355 if(flag_ascenseur) {
marwanesaich 6:aa800daf5ff9 356 //SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 357 etat = atome;
Artiom 0:f900f4ea9dfd 358 }
Artiom 0:f900f4ea9dfd 359 break;
Artiom 0:f900f4ea9dfd 360
Artiom 0:f900f4ea9dfd 361 case atome :
Artiom 0:f900f4ea9dfd 362 //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
marwanesaich 6:aa800daf5ff9 363 if(etat_cap != 0)
marwanesaich 6:aa800daf5ff9 364 {
marwanesaich 7:283d66d5c3b9 365 oriente_doigt(etat_cap);
marwanesaich 6:aa800daf5ff9 366 etat = tmp;
marwanesaich 6:aa800daf5ff9 367 }
marwanesaich 6:aa800daf5ff9 368
marwanesaich 6:aa800daf5ff9 369 /*if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
marwanesaich 6:aa800daf5ff9 370 oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
marwanesaich 6:aa800daf5ff9 371 etat = tmp;
marwanesaich 5:bb533bf81ee6 372 }else if(etat_cap != 0) {
marwanesaich 5:bb533bf81ee6 373 etat = tmp;
marwanesaich 6:aa800daf5ff9 374 }*/
Artiom 0:f900f4ea9dfd 375 break;
Artiom 0:f900f4ea9dfd 376
Artiom 0:f900f4ea9dfd 377 case tmp :
Artiom 0:f900f4ea9dfd 378 //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom 0:f900f4ea9dfd 379 if(etat_cap == 0) {
Artiom 0:f900f4ea9dfd 380 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 381 etat = atome;
Artiom 0:f900f4ea9dfd 382 }
Artiom 0:f900f4ea9dfd 383 break;
Artiom 0:f900f4ea9dfd 384 }
Artiom 0:f900f4ea9dfd 385 }
kyxstark 1:af508f84a079 386
marwanesaich 5:bb533bf81ee6 387 void oriente_doigt(int palet)
marwanesaich 5:bb533bf81ee6 388 {
marwanesaich 5:bb533bf81ee6 389 if(cote) {
marwanesaich 5:bb533bf81ee6 390 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 391 positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 392 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 393 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 394 positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 395 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 6:aa800daf5ff9 396
marwanesaich 5:bb533bf81ee6 397 }
marwanesaich 5:bb533bf81ee6 398 } else {
marwanesaich 5:bb533bf81ee6 399 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 400 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 401 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 5:bb533bf81ee6 402 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 403 positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 404 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 405 }
marwanesaich 5:bb533bf81ee6 406 }
marwanesaich 5:bb533bf81ee6 407 }
marwanesaich 5:bb533bf81ee6 408
Artiom 0:f900f4ea9dfd 409 void presentoir_avant(void)
Artiom 0:f900f4ea9dfd 410 {
kyxstark 2:33583329d6c8 411 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
kyxstark 2:33583329d6c8 412
kyxstark 3:a630a1ccf5f0 413 uint16_t pos_av_centre[2] = {512,512};
kyxstark 2:33583329d6c8 414
kyxstark 2:33583329d6c8 415 int speed=1;
kyxstark 2:33583329d6c8 416
kyxstark 2:33583329d6c8 417 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
kyxstark 2:33583329d6c8 418 verification();
Artiom 0:f900f4ea9dfd 419 }
Artiom 0:f900f4ea9dfd 420
Artiom 0:f900f4ea9dfd 421
Artiom 0:f900f4ea9dfd 422 void automate_ventouse_presentoir_avant(void)
Artiom 0:f900f4ea9dfd 423 {
kyxstark 2:33583329d6c8 424 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 425 static type_etat etat = init;
kyxstark 2:33583329d6c8 426
kyxstark 2:33583329d6c8 427 switch(etat) {
kyxstark 2:33583329d6c8 428 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 429 if(fpresentoir_avant)
kyxstark 2:33583329d6c8 430 etat=envoi_instruction;
kyxstark 2:33583329d6c8 431 break;
kyxstark 2:33583329d6c8 432
kyxstark 2:33583329d6c8 433 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 434 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 435 presentoir_avant();
kyxstark 2:33583329d6c8 436 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
kyxstark 2:33583329d6c8 437 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 438 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 439 break;
kyxstark 2:33583329d6c8 440
kyxstark 2:33583329d6c8 441 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 442 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
kyxstark 2:33583329d6c8 443 fpresentoir_avant=0;
kyxstark 2:33583329d6c8 444 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 445 etat = init;
kyxstark 2:33583329d6c8 446 }
kyxstark 2:33583329d6c8 447 break;
kyxstark 2:33583329d6c8 448
kyxstark 2:33583329d6c8 449 }
Artiom 0:f900f4ea9dfd 450 }
Artiom 0:f900f4ea9dfd 451
Artiom 0:f900f4ea9dfd 452 void automate_ventouse_relache_avant(void)
Artiom 0:f900f4ea9dfd 453 {
Artiom 0:f900f4ea9dfd 454
kyxstark 2:33583329d6c8 455 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 456 static type_etat etat = init;
kyxstark 2:33583329d6c8 457
kyxstark 2:33583329d6c8 458 switch(etat) {
kyxstark 2:33583329d6c8 459 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 460 if(favant_relache)
kyxstark 2:33583329d6c8 461 etat=envoi_instruction;
kyxstark 2:33583329d6c8 462 break;
kyxstark 2:33583329d6c8 463
kyxstark 2:33583329d6c8 464 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 465 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 466 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 467 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 468 break;
kyxstark 2:33583329d6c8 469
kyxstark 2:33583329d6c8 470 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 471 SendRawId(HACHEUR_RELEASE_AV);
kyxstark 2:33583329d6c8 472 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
kyxstark 2:33583329d6c8 473 favant_relache=0;
kyxstark 2:33583329d6c8 474 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 475 etat = init;
kyxstark 2:33583329d6c8 476 }
kyxstark 2:33583329d6c8 477 break;
kyxstark 2:33583329d6c8 478
kyxstark 2:33583329d6c8 479 }
Artiom 0:f900f4ea9dfd 480
Artiom 0:f900f4ea9dfd 481 }
Artiom 0:f900f4ea9dfd 482
Artiom 0:f900f4ea9dfd 483 void goldenium_avant(void)
Artiom 0:f900f4ea9dfd 484 {
Artiom 0:f900f4ea9dfd 485
Artiom 0:f900f4ea9dfd 486 }
Artiom 0:f900f4ea9dfd 487
Artiom 0:f900f4ea9dfd 488 void automate_ventouse_goldenium_avant (void)
Artiom 0:f900f4ea9dfd 489 {
Artiom 0:f900f4ea9dfd 490 }
Artiom 0:f900f4ea9dfd 491
Artiom 0:f900f4ea9dfd 492 void accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 493 {
Artiom 0:f900f4ea9dfd 494 }
Artiom 0:f900f4ea9dfd 495
Artiom 0:f900f4ea9dfd 496 void automate_ventouse_accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 497 {
Artiom 0:f900f4ea9dfd 498
kyxstark 1:af508f84a079 499 }
Artiom 0:f900f4ea9dfd 500 #endif