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

Committer:
marwanesaich
Date:
Sat May 25 17:47:38 2019 +0000
Revision:
9:05da4300730a
Parent:
8:7bd34e838ca3
Child:
10:fba0699b7a74
convoyeurs deux cotes deux ;

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 9:05da4300730a 225 pc.printf("%d\n",pos);
marwanesaich 9:05da4300730a 226 if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){
marwanesaich 8:7bd34e838ca3 227 velocityControl(stockage_G,0,GLED_ON,1);
marwanesaich 8:7bd34e838ca3 228 wait_ms(500);
marwanesaich 8:7bd34e838ca3 229 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 230 compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1);
marwanesaich 8:7bd34e838ca3 231 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 232 etat = tmp_pret;
marwanesaich 8:7bd34e838ca3 233 pc.printf("TMP_PRET\n");
marwanesaich 8:7bd34e838ca3 234 }
marwanesaich 8:7bd34e838ca3 235 break;
marwanesaich 8:7bd34e838ca3 236
marwanesaich 8:7bd34e838ca3 237 case tmp_pret :
marwanesaich 8:7bd34e838ca3 238 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 239 if(pos> (previous_pos-5)){
marwanesaich 8:7bd34e838ca3 240 etat = pret;
marwanesaich 8:7bd34e838ca3 241 pc.printf("PRET\n");
marwanesaich 8:7bd34e838ca3 242 }
marwanesaich 8:7bd34e838ca3 243 break;
marwanesaich 8:7bd34e838ca3 244
marwanesaich 8:7bd34e838ca3 245 case pret :
marwanesaich 8:7bd34e838ca3 246 if((status_contact & MASK_CT_GAUCHE)){
marwanesaich 8:7bd34e838ca3 247 //pc.printf("%d\n",pos);
marwanesaich 8:7bd34e838ca3 248 pos = getPos(stockage_G,1)- 550;
marwanesaich 8:7bd34e838ca3 249 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 250 wait_us(500);
marwanesaich 8:7bd34e838ca3 251 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 8:7bd34e838ca3 252 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 253 etat = pousse;
marwanesaich 8:7bd34e838ca3 254 pc.printf("POUSSE\n");
marwanesaich 8:7bd34e838ca3 255 }
marwanesaich 8:7bd34e838ca3 256 break;
marwanesaich 8:7bd34e838ca3 257
marwanesaich 8:7bd34e838ca3 258 case pousse :
marwanesaich 8:7bd34e838ca3 259 pos = getPos(stockage_G,1);
marwanesaich 8:7bd34e838ca3 260 if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){
marwanesaich 8:7bd34e838ca3 261 //pc.printf("%d\n",pos);
marwanesaich 8:7bd34e838ca3 262 pos = pos + 550;
marwanesaich 8:7bd34e838ca3 263 deverouillage_torque_convoyeurs_gauche();
marwanesaich 8:7bd34e838ca3 264 wait_us(500);
marwanesaich 8:7bd34e838ca3 265 positionControl(stockage_G,pos,1,BLED_ON,1);
marwanesaich 8:7bd34e838ca3 266 previous_pos = pos;
marwanesaich 8:7bd34e838ca3 267 etat = retour;
marwanesaich 8:7bd34e838ca3 268 pc.printf("RETOUR\n");
marwanesaich 8:7bd34e838ca3 269 }
marwanesaich 8:7bd34e838ca3 270
marwanesaich 8:7bd34e838ca3 271 break;
marwanesaich 8:7bd34e838ca3 272
marwanesaich 8:7bd34e838ca3 273 case retour :
marwanesaich 8:7bd34e838ca3 274 pos = getPos(stockage_G,1) ;
marwanesaich 8:7bd34e838ca3 275 if(pos> (previous_pos-5)){
marwanesaich 8:7bd34e838ca3 276 etat = pret;
marwanesaich 8:7bd34e838ca3 277 pc.printf("PRET\n");
marwanesaich 8:7bd34e838ca3 278 }
marwanesaich 8:7bd34e838ca3 279 break;
marwanesaich 8:7bd34e838ca3 280 }
marwanesaich 8:7bd34e838ca3 281 }
marwanesaich 8:7bd34e838ca3 282
marwanesaich 9:05da4300730a 283 void convoyeur_droit_violet(void){
marwanesaich 9:05da4300730a 284
marwanesaich 9:05da4300730a 285 typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
marwanesaich 9:05da4300730a 286 static type_etat etat = init;
marwanesaich 9:05da4300730a 287 static int16_t pos = 0;
marwanesaich 9:05da4300730a 288 static int16_t previous_pos = getPos(stockage_D,3);
marwanesaich 9:05da4300730a 289
marwanesaich 9:05da4300730a 290
marwanesaich 9:05da4300730a 291 switch(etat){
marwanesaich 9:05da4300730a 292
marwanesaich 9:05da4300730a 293 case init :
marwanesaich 9:05da4300730a 294 velocityControl(stockage_D,+512,BLED_ON,3);
marwanesaich 9:05da4300730a 295 pos = getPos(stockage_D,3);
marwanesaich 9:05da4300730a 296 if(previous_pos != pos){
marwanesaich 9:05da4300730a 297 pc.printf("ETALONNAGE\n");
marwanesaich 9:05da4300730a 298 etat = etalonnage;
marwanesaich 9:05da4300730a 299 }
marwanesaich 9:05da4300730a 300 break;
Artiom 0:f900f4ea9dfd 301
marwanesaich 9:05da4300730a 302 case etalonnage :
marwanesaich 9:05da4300730a 303 pos = getPos(stockage_D,3);
marwanesaich 9:05da4300730a 304 pc.printf("%d\n", pos);
marwanesaich 9:05da4300730a 305 if(pos>900 and (status_contact & MASK_FC_DROIT)){
marwanesaich 9:05da4300730a 306 velocityControl(stockage_D,0,GLED_ON,3);
marwanesaich 9:05da4300730a 307 wait_ms(500);
marwanesaich 9:05da4300730a 308 pos = getPos(stockage_D,3) ;
marwanesaich 9:05da4300730a 309 positionControl(stockage_D,pos-180,1,BLED_ON,3);
marwanesaich 9:05da4300730a 310 previous_pos = pos;
marwanesaich 9:05da4300730a 311 etat = tmp_pret;
marwanesaich 9:05da4300730a 312 pc.printf("TMP_PRET\n");
marwanesaich 9:05da4300730a 313 }
marwanesaich 9:05da4300730a 314 break;
marwanesaich 9:05da4300730a 315
marwanesaich 9:05da4300730a 316 case tmp_pret :
marwanesaich 9:05da4300730a 317 pos = getPos(stockage_D,3) ;
marwanesaich 9:05da4300730a 318 if(pos< (previous_pos+5)){
marwanesaich 9:05da4300730a 319 etat = pret;
marwanesaich 9:05da4300730a 320 pc.printf("PRET\n");
marwanesaich 9:05da4300730a 321 }
marwanesaich 9:05da4300730a 322 break;
marwanesaich 9:05da4300730a 323
marwanesaich 9:05da4300730a 324 case pret :
marwanesaich 9:05da4300730a 325 if((status_contact & MASK_CT_DROIT)){
marwanesaich 9:05da4300730a 326 pos = getPos(stockage_D,3)- 550;
marwanesaich 9:05da4300730a 327 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 328 wait_us(500);
marwanesaich 9:05da4300730a 329 positionControl(stockage_D,pos,1,BLED_ON,3);
marwanesaich 9:05da4300730a 330 previous_pos = pos;
marwanesaich 9:05da4300730a 331 etat = pousse;
marwanesaich 9:05da4300730a 332 pc.printf("POUSSE\n");
marwanesaich 9:05da4300730a 333 }
marwanesaich 9:05da4300730a 334 break;
marwanesaich 9:05da4300730a 335
marwanesaich 9:05da4300730a 336 case pousse :
marwanesaich 9:05da4300730a 337 pos = getPos(stockage_D,3);
marwanesaich 9:05da4300730a 338 if(pos<(previous_pos+5) and (status_contact & MASK_CT_DROIT)==0){
marwanesaich 9:05da4300730a 339 pos = pos + 550;
marwanesaich 9:05da4300730a 340 deverouillage_torque_convoyeurs_droit();
marwanesaich 9:05da4300730a 341 wait_us(500);
marwanesaich 9:05da4300730a 342 positionControl(stockage_D,pos,1,BLED_ON,3);
marwanesaich 9:05da4300730a 343 previous_pos = pos;
marwanesaich 9:05da4300730a 344 etat = retour;
marwanesaich 9:05da4300730a 345 pc.printf("RETOUR\n");
marwanesaich 9:05da4300730a 346 }
marwanesaich 9:05da4300730a 347
marwanesaich 9:05da4300730a 348 break;
marwanesaich 9:05da4300730a 349 case retour :
marwanesaich 9:05da4300730a 350 pos = getPos(stockage_D,3) ;
marwanesaich 9:05da4300730a 351 if(pos> (previous_pos-5)){
marwanesaich 9:05da4300730a 352 etat = pret;
marwanesaich 9:05da4300730a 353 pc.printf("PRET\n");
marwanesaich 9:05da4300730a 354 }
marwanesaich 9:05da4300730a 355 break;
marwanesaich 9:05da4300730a 356 }
marwanesaich 9:05da4300730a 357 }
kyxstark 3:a630a1ccf5f0 358 void gabarit_robot(void)
Artiom 0:f900f4ea9dfd 359 {
Artiom 0:f900f4ea9dfd 360 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:f900f4ea9dfd 361
kyxstark 3:a630a1ccf5f0 362 uint16_t pos_av_centre[2] = {550,550};//470,350
Artiom 0:f900f4ea9dfd 363
Artiom 0:f900f4ea9dfd 364
Artiom 0:f900f4ea9dfd 365 int speed=100;
Artiom 0:f900f4ea9dfd 366
Artiom 0:f900f4ea9dfd 367 deverouillage_torque();
kyxstark 3:a630a1ccf5f0 368 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 6:aa800daf5ff9 369 wait(0.5);
marwanesaich 6:aa800daf5ff9 370 positionControl(doigt,POS_DOIGT_DROIT,100,BLED_ON,2);
Artiom 0:f900f4ea9dfd 371
Artiom 0:f900f4ea9dfd 372 }
Artiom 0:f900f4ea9dfd 373
Artiom 0:f900f4ea9dfd 374
Artiom 0:f900f4ea9dfd 375 void fifo_couleur(void)
Artiom 0:f900f4ea9dfd 376 {
Artiom 0:f900f4ea9dfd 377
Artiom 0:f900f4ea9dfd 378 typedef enum {n_atome, atome, tmp} type_etat ;
Artiom 0:f900f4ea9dfd 379 static type_etat etat = n_atome;
Artiom 0:f900f4ea9dfd 380
Artiom 0:f900f4ea9dfd 381 int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
Artiom 0:f900f4ea9dfd 382 switch(etat) {
Artiom 0:f900f4ea9dfd 383 case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom 0:f900f4ea9dfd 384 if(etat_cap) {
Artiom 0:f900f4ea9dfd 385 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 386 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 387 if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
marwanesaich 5:bb533bf81ee6 388 /* if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
Artiom 0:f900f4ea9dfd 389 positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu
Artiom 0:f900f4ea9dfd 390 } else {
Artiom 0:f900f4ea9dfd 391 positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert
marwanesaich 5:bb533bf81ee6 392 }*/
marwanesaich 5:bb533bf81ee6 393 oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom 0:f900f4ea9dfd 394 }
marwanesaich 5:bb533bf81ee6 395
Artiom 0:f900f4ea9dfd 396 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 397 etat = tmp;
Artiom 0:f900f4ea9dfd 398 }
Artiom 0:f900f4ea9dfd 399 break;
marwanesaich 6:aa800daf5ff9 400 /*
Artiom 0:f900f4ea9dfd 401 case atome : //on stocke dans la fifo la couleur de l'atome
Artiom 0:f900f4ea9dfd 402 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 403 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 404 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 405 etat = tmp;
marwanesaich 6:aa800daf5ff9 406 break;*/
Artiom 0:f900f4ea9dfd 407
Artiom 0:f900f4ea9dfd 408 case tmp : //on attend que l'atome traité soit totalement passé
Artiom 0:f900f4ea9dfd 409 if(!etat_cap) {
Artiom 0:f900f4ea9dfd 410 etat = n_atome;
Artiom 0:f900f4ea9dfd 411 }
Artiom 0:f900f4ea9dfd 412 break;
Artiom 0:f900f4ea9dfd 413
Artiom 0:f900f4ea9dfd 414 }
Artiom 0:f900f4ea9dfd 415
Artiom 0:f900f4ea9dfd 416 }
Artiom 0:f900f4ea9dfd 417
Artiom 0:f900f4ea9dfd 418
Artiom 0:f900f4ea9dfd 419 void ascenseur(void)
Artiom 0:f900f4ea9dfd 420 {
Artiom 0:f900f4ea9dfd 421 typedef enum {init, atome, tmp} type_etat;
Artiom 0:f900f4ea9dfd 422 static type_etat etat = init;
Artiom 0:f900f4ea9dfd 423
Artiom 0:f900f4ea9dfd 424 int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
Artiom 0:f900f4ea9dfd 425 static int flag_sortie = 0;
marwanesaich 5:bb533bf81ee6 426
Artiom 0:f900f4ea9dfd 427 switch(etat) {
Artiom 0:f900f4ea9dfd 428 case init :
Artiom 0:f900f4ea9dfd 429 //on attend le premier atome et place le herkulex en fonction
Artiom 0:f900f4ea9dfd 430 if(flag_ascenseur) {
marwanesaich 6:aa800daf5ff9 431 //SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 432 etat = atome;
Artiom 0:f900f4ea9dfd 433 }
Artiom 0:f900f4ea9dfd 434 break;
Artiom 0:f900f4ea9dfd 435
Artiom 0:f900f4ea9dfd 436 case atome :
Artiom 0:f900f4ea9dfd 437 //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
marwanesaich 6:aa800daf5ff9 438 if(etat_cap != 0)
marwanesaich 6:aa800daf5ff9 439 {
marwanesaich 7:283d66d5c3b9 440 oriente_doigt(etat_cap);
marwanesaich 6:aa800daf5ff9 441 etat = tmp;
marwanesaich 6:aa800daf5ff9 442 }
marwanesaich 6:aa800daf5ff9 443
marwanesaich 6:aa800daf5ff9 444 /*if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
marwanesaich 6:aa800daf5ff9 445 oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
marwanesaich 6:aa800daf5ff9 446 etat = tmp;
marwanesaich 5:bb533bf81ee6 447 }else if(etat_cap != 0) {
marwanesaich 5:bb533bf81ee6 448 etat = tmp;
marwanesaich 6:aa800daf5ff9 449 }*/
Artiom 0:f900f4ea9dfd 450 break;
Artiom 0:f900f4ea9dfd 451
Artiom 0:f900f4ea9dfd 452 case tmp :
Artiom 0:f900f4ea9dfd 453 //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom 0:f900f4ea9dfd 454 if(etat_cap == 0) {
Artiom 0:f900f4ea9dfd 455 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 456 etat = atome;
Artiom 0:f900f4ea9dfd 457 }
Artiom 0:f900f4ea9dfd 458 break;
Artiom 0:f900f4ea9dfd 459 }
Artiom 0:f900f4ea9dfd 460 }
kyxstark 1:af508f84a079 461
marwanesaich 5:bb533bf81ee6 462 void oriente_doigt(int palet)
marwanesaich 5:bb533bf81ee6 463 {
marwanesaich 5:bb533bf81ee6 464 if(cote) {
marwanesaich 5:bb533bf81ee6 465 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 466 positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 467 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 468 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 469 positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 470 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 6:aa800daf5ff9 471
marwanesaich 5:bb533bf81ee6 472 }
marwanesaich 5:bb533bf81ee6 473 } else {
marwanesaich 5:bb533bf81ee6 474 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 475 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 476 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 5:bb533bf81ee6 477 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 478 positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 479 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 480 }
marwanesaich 5:bb533bf81ee6 481 }
marwanesaich 5:bb533bf81ee6 482 }
marwanesaich 5:bb533bf81ee6 483
Artiom 0:f900f4ea9dfd 484 void presentoir_avant(void)
Artiom 0:f900f4ea9dfd 485 {
kyxstark 2:33583329d6c8 486 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
kyxstark 2:33583329d6c8 487
kyxstark 3:a630a1ccf5f0 488 uint16_t pos_av_centre[2] = {512,512};
kyxstark 2:33583329d6c8 489
kyxstark 2:33583329d6c8 490 int speed=1;
kyxstark 2:33583329d6c8 491
kyxstark 2:33583329d6c8 492 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
kyxstark 2:33583329d6c8 493 verification();
Artiom 0:f900f4ea9dfd 494 }
Artiom 0:f900f4ea9dfd 495
Artiom 0:f900f4ea9dfd 496
Artiom 0:f900f4ea9dfd 497 void automate_ventouse_presentoir_avant(void)
Artiom 0:f900f4ea9dfd 498 {
kyxstark 2:33583329d6c8 499 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 500 static type_etat etat = init;
kyxstark 2:33583329d6c8 501
kyxstark 2:33583329d6c8 502 switch(etat) {
kyxstark 2:33583329d6c8 503 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 504 if(fpresentoir_avant)
kyxstark 2:33583329d6c8 505 etat=envoi_instruction;
kyxstark 2:33583329d6c8 506 break;
kyxstark 2:33583329d6c8 507
kyxstark 2:33583329d6c8 508 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 509 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 510 presentoir_avant();
kyxstark 2:33583329d6c8 511 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
kyxstark 2:33583329d6c8 512 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 513 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 514 break;
kyxstark 2:33583329d6c8 515
kyxstark 2:33583329d6c8 516 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 517 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
kyxstark 2:33583329d6c8 518 fpresentoir_avant=0;
kyxstark 2:33583329d6c8 519 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 520 etat = init;
kyxstark 2:33583329d6c8 521 }
kyxstark 2:33583329d6c8 522 break;
kyxstark 2:33583329d6c8 523
kyxstark 2:33583329d6c8 524 }
Artiom 0:f900f4ea9dfd 525 }
Artiom 0:f900f4ea9dfd 526
Artiom 0:f900f4ea9dfd 527 void automate_ventouse_relache_avant(void)
Artiom 0:f900f4ea9dfd 528 {
Artiom 0:f900f4ea9dfd 529
kyxstark 2:33583329d6c8 530 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 531 static type_etat etat = init;
kyxstark 2:33583329d6c8 532
kyxstark 2:33583329d6c8 533 switch(etat) {
kyxstark 2:33583329d6c8 534 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 535 if(favant_relache)
kyxstark 2:33583329d6c8 536 etat=envoi_instruction;
kyxstark 2:33583329d6c8 537 break;
kyxstark 2:33583329d6c8 538
kyxstark 2:33583329d6c8 539 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 540 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 541 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 542 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 543 break;
kyxstark 2:33583329d6c8 544
kyxstark 2:33583329d6c8 545 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 546 SendRawId(HACHEUR_RELEASE_AV);
kyxstark 2:33583329d6c8 547 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
kyxstark 2:33583329d6c8 548 favant_relache=0;
kyxstark 2:33583329d6c8 549 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 550 etat = init;
kyxstark 2:33583329d6c8 551 }
kyxstark 2:33583329d6c8 552 break;
kyxstark 2:33583329d6c8 553
kyxstark 2:33583329d6c8 554 }
Artiom 0:f900f4ea9dfd 555
Artiom 0:f900f4ea9dfd 556 }
Artiom 0:f900f4ea9dfd 557
Artiom 0:f900f4ea9dfd 558 void goldenium_avant(void)
Artiom 0:f900f4ea9dfd 559 {
Artiom 0:f900f4ea9dfd 560
Artiom 0:f900f4ea9dfd 561 }
Artiom 0:f900f4ea9dfd 562
Artiom 0:f900f4ea9dfd 563 void automate_ventouse_goldenium_avant (void)
Artiom 0:f900f4ea9dfd 564 {
Artiom 0:f900f4ea9dfd 565 }
Artiom 0:f900f4ea9dfd 566
Artiom 0:f900f4ea9dfd 567 void accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 568 {
Artiom 0:f900f4ea9dfd 569 }
Artiom 0:f900f4ea9dfd 570
Artiom 0:f900f4ea9dfd 571 void automate_ventouse_accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 572 {
Artiom 0:f900f4ea9dfd 573
kyxstark 1:af508f84a079 574 }
Artiom 0:f900f4ea9dfd 575 #endif