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@11:dfff2e200d0c, 2019-05-26 (annotated)

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