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@5:bb533bf81ee6, 2019-05-24 (annotated)

Committer:
marwanesaich
Date:
Fri May 24 19:36:22 2019 +0000
Revision:
5:bb533bf81ee6
Parent:
4:8ac0f7c17ac7
Child:
6:aa800daf5ff9
ajout convoyeurs

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