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

Committer:
marwanesaich
Date:
Sat May 25 00:58:36 2019 +0000
Revision:
6:aa800daf5ff9
Parent:
5:bb533bf81ee6
Child:
7:283d66d5c3b9
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
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 5:bb533bf81ee6 50 void convoyeur_gauche(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 5:bb533bf81ee6 125 void convoyeur_droit(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
Artiom 0:f900f4ea9dfd 204
kyxstark 3:a630a1ccf5f0 205 void gabarit_robot(void)
Artiom 0:f900f4ea9dfd 206 {
Artiom 0:f900f4ea9dfd 207 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
Artiom 0:f900f4ea9dfd 208
kyxstark 3:a630a1ccf5f0 209 uint16_t pos_av_centre[2] = {550,550};//470,350
Artiom 0:f900f4ea9dfd 210
Artiom 0:f900f4ea9dfd 211
Artiom 0:f900f4ea9dfd 212 int speed=100;
Artiom 0:f900f4ea9dfd 213
Artiom 0:f900f4ea9dfd 214 deverouillage_torque();
kyxstark 3:a630a1ccf5f0 215 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
marwanesaich 6:aa800daf5ff9 216 wait(0.5);
marwanesaich 6:aa800daf5ff9 217 positionControl(doigt,POS_DOIGT_DROIT,100,BLED_ON,2);
Artiom 0:f900f4ea9dfd 218
Artiom 0:f900f4ea9dfd 219 }
Artiom 0:f900f4ea9dfd 220
Artiom 0:f900f4ea9dfd 221
Artiom 0:f900f4ea9dfd 222 void fifo_couleur(void)
Artiom 0:f900f4ea9dfd 223 {
Artiom 0:f900f4ea9dfd 224
Artiom 0:f900f4ea9dfd 225 typedef enum {n_atome, atome, tmp} type_etat ;
Artiom 0:f900f4ea9dfd 226 static type_etat etat = n_atome;
Artiom 0:f900f4ea9dfd 227
Artiom 0:f900f4ea9dfd 228 int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
Artiom 0:f900f4ea9dfd 229 switch(etat) {
Artiom 0:f900f4ea9dfd 230 case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
Artiom 0:f900f4ea9dfd 231 if(etat_cap) {
Artiom 0:f900f4ea9dfd 232 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 233 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 234 if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
marwanesaich 5:bb533bf81ee6 235 /* if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
Artiom 0:f900f4ea9dfd 236 positionControl(doigt,384,10,BLED_ON,2); //position herkulex stockage bleu
Artiom 0:f900f4ea9dfd 237 } else {
Artiom 0:f900f4ea9dfd 238 positionControl(doigt,640,10,GLED_ON,2); //position herkulex stockage rouge/vert
marwanesaich 5:bb533bf81ee6 239 }*/
marwanesaich 5:bb533bf81ee6 240 oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
Artiom 0:f900f4ea9dfd 241 }
marwanesaich 5:bb533bf81ee6 242
Artiom 0:f900f4ea9dfd 243 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 244 etat = tmp;
Artiom 0:f900f4ea9dfd 245 }
Artiom 0:f900f4ea9dfd 246 break;
marwanesaich 6:aa800daf5ff9 247 /*
Artiom 0:f900f4ea9dfd 248 case atome : //on stocke dans la fifo la couleur de l'atome
Artiom 0:f900f4ea9dfd 249 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
Artiom 0:f900f4ea9dfd 250 flag_ascenseur = 1;
Artiom 0:f900f4ea9dfd 251 FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 252 etat = tmp;
marwanesaich 6:aa800daf5ff9 253 break;*/
Artiom 0:f900f4ea9dfd 254
Artiom 0:f900f4ea9dfd 255 case tmp : //on attend que l'atome traité soit totalement passé
Artiom 0:f900f4ea9dfd 256 if(!etat_cap) {
Artiom 0:f900f4ea9dfd 257 etat = n_atome;
Artiom 0:f900f4ea9dfd 258 }
Artiom 0:f900f4ea9dfd 259 break;
Artiom 0:f900f4ea9dfd 260
Artiom 0:f900f4ea9dfd 261 }
Artiom 0:f900f4ea9dfd 262
Artiom 0:f900f4ea9dfd 263 }
Artiom 0:f900f4ea9dfd 264
Artiom 0:f900f4ea9dfd 265
Artiom 0:f900f4ea9dfd 266 void ascenseur(void)
Artiom 0:f900f4ea9dfd 267 {
Artiom 0:f900f4ea9dfd 268 typedef enum {init, atome, tmp} type_etat;
Artiom 0:f900f4ea9dfd 269 static type_etat etat = init;
Artiom 0:f900f4ea9dfd 270
Artiom 0:f900f4ea9dfd 271 int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
Artiom 0:f900f4ea9dfd 272 static int flag_sortie = 0;
marwanesaich 5:bb533bf81ee6 273
Artiom 0:f900f4ea9dfd 274 switch(etat) {
Artiom 0:f900f4ea9dfd 275 case init :
Artiom 0:f900f4ea9dfd 276 //on attend le premier atome et place le herkulex en fonction
Artiom 0:f900f4ea9dfd 277 if(flag_ascenseur) {
marwanesaich 6:aa800daf5ff9 278 //SendCharCan(HACHEUR_ID_COUROIES,1);
Artiom 0:f900f4ea9dfd 279 etat = atome;
Artiom 0:f900f4ea9dfd 280 }
Artiom 0:f900f4ea9dfd 281 break;
Artiom 0:f900f4ea9dfd 282
Artiom 0:f900f4ea9dfd 283 case atome :
Artiom 0:f900f4ea9dfd 284 //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
marwanesaich 6:aa800daf5ff9 285 if(etat_cap != 0)
marwanesaich 6:aa800daf5ff9 286 {
marwanesaich 5:bb533bf81ee6 287 oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
marwanesaich 6:aa800daf5ff9 288 etat = tmp;
marwanesaich 6:aa800daf5ff9 289 }
marwanesaich 6:aa800daf5ff9 290
marwanesaich 6:aa800daf5ff9 291 /*if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
marwanesaich 6:aa800daf5ff9 292 oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
marwanesaich 6:aa800daf5ff9 293 etat = tmp;
marwanesaich 5:bb533bf81ee6 294 }else if(etat_cap != 0) {
marwanesaich 5:bb533bf81ee6 295 etat = tmp;
marwanesaich 6:aa800daf5ff9 296 }*/
Artiom 0:f900f4ea9dfd 297 break;
Artiom 0:f900f4ea9dfd 298
Artiom 0:f900f4ea9dfd 299 case tmp :
Artiom 0:f900f4ea9dfd 300 //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
Artiom 0:f900f4ea9dfd 301 if(etat_cap == 0) {
Artiom 0:f900f4ea9dfd 302 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
Artiom 0:f900f4ea9dfd 303 etat = atome;
Artiom 0:f900f4ea9dfd 304 }
Artiom 0:f900f4ea9dfd 305 break;
Artiom 0:f900f4ea9dfd 306 }
Artiom 0:f900f4ea9dfd 307 }
kyxstark 1:af508f84a079 308
marwanesaich 5:bb533bf81ee6 309 void oriente_doigt(int palet)
marwanesaich 5:bb533bf81ee6 310 {
marwanesaich 5:bb533bf81ee6 311 if(cote) {
marwanesaich 5:bb533bf81ee6 312 if(palet == 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 313 positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 314 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 315 } else if (palet == 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 316 positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 317 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 6:aa800daf5ff9 318
marwanesaich 5:bb533bf81ee6 319 }
marwanesaich 5:bb533bf81ee6 320 } else {
marwanesaich 5:bb533bf81ee6 321 if(palet== 1) {//position herkulex stockage bleu
marwanesaich 6:aa800daf5ff9 322 positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
marwanesaich 6:aa800daf5ff9 323 SendCharCan(HACHEUR_ID_COUROIES,1);
marwanesaich 5:bb533bf81ee6 324 } else if (palet== 2) {//position herkulex stockage rouge/vert
marwanesaich 6:aa800daf5ff9 325 positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
marwanesaich 6:aa800daf5ff9 326 SendCharCan(HACHEUR_ID_COUROIES,2);
marwanesaich 5:bb533bf81ee6 327 }
marwanesaich 5:bb533bf81ee6 328 }
marwanesaich 5:bb533bf81ee6 329 }
marwanesaich 5:bb533bf81ee6 330
Artiom 0:f900f4ea9dfd 331 void presentoir_avant(void)
Artiom 0:f900f4ea9dfd 332 {
kyxstark 2:33583329d6c8 333 uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
kyxstark 2:33583329d6c8 334
kyxstark 3:a630a1ccf5f0 335 uint16_t pos_av_centre[2] = {512,512};
kyxstark 2:33583329d6c8 336
kyxstark 2:33583329d6c8 337 int speed=1;
kyxstark 2:33583329d6c8 338
kyxstark 2:33583329d6c8 339 positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
kyxstark 2:33583329d6c8 340 verification();
Artiom 0:f900f4ea9dfd 341 }
Artiom 0:f900f4ea9dfd 342
Artiom 0:f900f4ea9dfd 343
Artiom 0:f900f4ea9dfd 344 void automate_ventouse_presentoir_avant(void)
Artiom 0:f900f4ea9dfd 345 {
kyxstark 2:33583329d6c8 346 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 347 static type_etat etat = init;
kyxstark 2:33583329d6c8 348
kyxstark 2:33583329d6c8 349 switch(etat) {
kyxstark 2:33583329d6c8 350 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 351 if(fpresentoir_avant)
kyxstark 2:33583329d6c8 352 etat=envoi_instruction;
kyxstark 2:33583329d6c8 353 break;
kyxstark 2:33583329d6c8 354
kyxstark 2:33583329d6c8 355 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 356 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 357 presentoir_avant();
kyxstark 2:33583329d6c8 358 SendRawId(HACHEUR_GET_PRESENTOIR_AV);
kyxstark 2:33583329d6c8 359 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 360 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 361 break;
kyxstark 2:33583329d6c8 362
kyxstark 2:33583329d6c8 363 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 364 if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
kyxstark 2:33583329d6c8 365 fpresentoir_avant=0;
kyxstark 2:33583329d6c8 366 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 367 etat = init;
kyxstark 2:33583329d6c8 368 }
kyxstark 2:33583329d6c8 369 break;
kyxstark 2:33583329d6c8 370
kyxstark 2:33583329d6c8 371 }
Artiom 0:f900f4ea9dfd 372 }
Artiom 0:f900f4ea9dfd 373
Artiom 0:f900f4ea9dfd 374 void automate_ventouse_relache_avant(void)
Artiom 0:f900f4ea9dfd 375 {
Artiom 0:f900f4ea9dfd 376
kyxstark 2:33583329d6c8 377 typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
kyxstark 2:33583329d6c8 378 static type_etat etat = init;
kyxstark 2:33583329d6c8 379
kyxstark 2:33583329d6c8 380 switch(etat) {
kyxstark 2:33583329d6c8 381 case init: //attente d'initialisation
kyxstark 2:33583329d6c8 382 if(favant_relache)
kyxstark 2:33583329d6c8 383 etat=envoi_instruction;
kyxstark 2:33583329d6c8 384 break;
kyxstark 2:33583329d6c8 385
kyxstark 2:33583329d6c8 386 case envoi_instruction://envoi instruction
kyxstark 2:33583329d6c8 387 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
kyxstark 2:33583329d6c8 388 SendRawId(HACHEUR_STATUT_VENTOUSES);
kyxstark 2:33583329d6c8 389 etat = attente_ack_ventouse;
kyxstark 2:33583329d6c8 390 break;
kyxstark 2:33583329d6c8 391
kyxstark 2:33583329d6c8 392 case attente_ack_ventouse:
kyxstark 2:33583329d6c8 393 SendRawId(HACHEUR_RELEASE_AV);
kyxstark 2:33583329d6c8 394 if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
kyxstark 2:33583329d6c8 395 favant_relache=0;
kyxstark 2:33583329d6c8 396 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
kyxstark 2:33583329d6c8 397 etat = init;
kyxstark 2:33583329d6c8 398 }
kyxstark 2:33583329d6c8 399 break;
kyxstark 2:33583329d6c8 400
kyxstark 2:33583329d6c8 401 }
Artiom 0:f900f4ea9dfd 402
Artiom 0:f900f4ea9dfd 403 }
Artiom 0:f900f4ea9dfd 404
Artiom 0:f900f4ea9dfd 405 void goldenium_avant(void)
Artiom 0:f900f4ea9dfd 406 {
Artiom 0:f900f4ea9dfd 407
Artiom 0:f900f4ea9dfd 408 }
Artiom 0:f900f4ea9dfd 409
Artiom 0:f900f4ea9dfd 410 void automate_ventouse_goldenium_avant (void)
Artiom 0:f900f4ea9dfd 411 {
Artiom 0:f900f4ea9dfd 412 }
Artiom 0:f900f4ea9dfd 413
Artiom 0:f900f4ea9dfd 414 void accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 415 {
Artiom 0:f900f4ea9dfd 416 }
Artiom 0:f900f4ea9dfd 417
Artiom 0:f900f4ea9dfd 418 void automate_ventouse_accelerateur_avant(void)
Artiom 0:f900f4ea9dfd 419 {
Artiom 0:f900f4ea9dfd 420
kyxstark 1:af508f84a079 421 }
Artiom 0:f900f4ea9dfd 422 #endif