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@13:0b5310c6dc44, 2019-05-26 (annotated)

Committer:
marwanesaich
Date:
Sun May 26 11:26:06 2019 +0000
Revision:
13:0b5310c6dc44
Parent:
11:dfff2e200d0c
Child:
14:bf2810b95f25
plages ok ++;

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