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

Committer:
marwanesaich
Date:
Sat May 25 18:39:49 2019 +0000
Revision:
10:fba0699b7a74
Parent:
9:05da4300730a
Child:
11:dfff2e200d0c
doigt fifo vide

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