![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
pixy info2
Dependencies: CMPS03 SRF05 mbed pixy
Fork of 0000Non_stop_code_v3 by
main.cpp@2:48f7e5f79caa, 2017-06-22 (annotated)
- Committer:
- qj604184
- Date:
- Thu Jun 22 09:32:24 2017 +0000
- Revision:
- 2:48f7e5f79caa
- Parent:
- 1:99f469d63b9a
pixy
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pirottealex | 0:a8cee96c9250 | 1 | #include "mbed.h" |
pirottealex | 0:a8cee96c9250 | 2 | #include "fct.h" |
pirottealex | 0:a8cee96c9250 | 3 | #include "Pixy.h" |
pirottealex | 0:a8cee96c9250 | 4 | #include "PixyLink.h" |
pirottealex | 0:a8cee96c9250 | 5 | #include "SRF05.h" |
pirottealex | 0:a8cee96c9250 | 6 | #include <CMPS03.h> |
pirottealex | 0:a8cee96c9250 | 7 | |
pirottealex | 0:a8cee96c9250 | 8 | Serial pc (USBTX,USBRX); |
pirottealex | 0:a8cee96c9250 | 9 | int main() |
pirottealex | 0:a8cee96c9250 | 10 | { |
qj604184 | 2:48f7e5f79caa | 11 | float infra; |
pirottealex | 0:a8cee96c9250 | 12 | init(); |
pirottealex | 0:a8cee96c9250 | 13 | Pixy pixy= Pixy(p28,p27); |
pirottealex | 0:a8cee96c9250 | 14 | pixy.setSerialOutput(&pc); |
pirottealex | 0:a8cee96c9250 | 15 | pc.printf("alive"); |
pirottealex | 0:a8cee96c9250 | 16 | |
pirottealex | 0:a8cee96c9250 | 17 | while(1) { |
qj604184 | 2:48f7e5f79caa | 18 | infra=1/(((SD_2.read()*3.3)-0.12766)/11.489);//valeur en cm |
qj604184 | 2:48f7e5f79caa | 19 | if(infra<0||infra>25) { |
qj604184 | 2:48f7e5f79caa | 20 | infra=25.0; |
qj604184 | 2:48f7e5f79caa | 21 | } |
pirottealex | 0:a8cee96c9250 | 22 | lecture_blanc(); // recupere valeurs capteurs ligne blanche |
pirottealex | 0:a8cee96c9250 | 23 | lecture_us(); // recupere valeurs capteurs us |
pirottealex | 0:a8cee96c9250 | 24 | lecture_boussole(); //gBoussole |
pirottealex | 0:a8cee96c9250 | 25 | bout=bp.read(); |
pirottealex | 0:a8cee96c9250 | 26 | blocks = pixy.getBlocks(); |
qj604184 | 2:48f7e5f79caa | 27 | //pc.printf("infra : %f\n\r",infra); |
qj604184 | 2:48f7e5f79caa | 28 | //pc.printf("us gauche= %f us droit= %f\n\r",us_gauche,us_droit); |
qj604184 | 2:48f7e5f79caa | 29 | //wait(0.5); |
pirottealex | 0:a8cee96c9250 | 30 | //pc.printf("etat=%d\n\r",etat); |
pirottealex | 0:a8cee96c9250 | 31 | //pc.printf("etat=%d,capt1= %d, capt2= %d, us_ar=%f, bp=%d, boussole=%f\n\r",etat,captL1,captL3,us_arriere,bp.read(),gBoussole); |
pirottealex | 0:a8cee96c9250 | 32 | if (blocks) { |
pirottealex | 0:a8cee96c9250 | 33 | taille=pixy.blocks[j].width*pixy.blocks[j].height; |
pirottealex | 0:a8cee96c9250 | 34 | errorX = (160-pixy.blocks[j].x); |
pirottealex | 1:99f469d63b9a | 35 | //pc.printf("taille=%f,sig: %d x: %d y: %d width: %d height: %d\n\r",taille, pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height); |
pirottealex | 0:a8cee96c9250 | 36 | //pc.printf("OK"); |
pirottealex | 0:a8cee96c9250 | 37 | } |
qj604184 | 2:48f7e5f79caa | 38 | |
qj604184 | 2:48f7e5f79caa | 39 | switch(etat) { |
pirottealex | 0:a8cee96c9250 | 40 | case(0) : |
qj604184 | 2:48f7e5f79caa | 41 | |
qj604184 | 2:48f7e5f79caa | 42 | if(bout==0) { |
pirottealex | 0:a8cee96c9250 | 43 | etat=100; |
pirottealex | 0:a8cee96c9250 | 44 | cap_set=gBoussole; |
pirottealex | 0:a8cee96c9250 | 45 | } |
qj604184 | 2:48f7e5f79caa | 46 | if((captL1>=1)||(captL3>=1)) { |
pirottealex | 1:99f469d63b9a | 47 | etat=3; |
pirottealex | 1:99f469d63b9a | 48 | } |
pirottealex | 0:a8cee96c9250 | 49 | break; |
qj604184 | 2:48f7e5f79caa | 50 | |
pirottealex | 0:a8cee96c9250 | 51 | case(100): |
qj604184 | 2:48f7e5f79caa | 52 | |
qj604184 | 2:48f7e5f79caa | 53 | if((blocks)&&(taille>50)) { |
pirottealex | 0:a8cee96c9250 | 54 | etat=1; |
pirottealex | 0:a8cee96c9250 | 55 | } |
qj604184 | 2:48f7e5f79caa | 56 | |
qj604184 | 2:48f7e5f79caa | 57 | else { |
pirottealex | 0:a8cee96c9250 | 58 | etat=21; |
pirottealex | 0:a8cee96c9250 | 59 | } |
qj604184 | 2:48f7e5f79caa | 60 | if((captL1>=1)||(captL3>=1)) { |
pirottealex | 1:99f469d63b9a | 61 | etat=3; |
pirottealex | 1:99f469d63b9a | 62 | } |
pirottealex | 0:a8cee96c9250 | 63 | break; |
qj604184 | 2:48f7e5f79caa | 64 | case(200): |
qj604184 | 2:48f7e5f79caa | 65 | if((blocks)&&(taille>50)) { |
qj604184 | 2:48f7e5f79caa | 66 | etat=1; |
qj604184 | 2:48f7e5f79caa | 67 | } |
qj604184 | 2:48f7e5f79caa | 68 | |
qj604184 | 2:48f7e5f79caa | 69 | else { |
qj604184 | 2:48f7e5f79caa | 70 | etat=21; |
qj604184 | 2:48f7e5f79caa | 71 | } |
qj604184 | 2:48f7e5f79caa | 72 | if((captL1>=1)||(captL3>=1)) { |
qj604184 | 2:48f7e5f79caa | 73 | etat=3; |
pirottealex | 1:99f469d63b9a | 74 | } |
qj604184 | 2:48f7e5f79caa | 75 | break; |
qj604184 | 2:48f7e5f79caa | 76 | |
qj604184 | 2:48f7e5f79caa | 77 | case(1) : |
qj604184 | 2:48f7e5f79caa | 78 | |
qj604184 | 2:48f7e5f79caa | 79 | if(infra<20) { |
qj604184 | 2:48f7e5f79caa | 80 | |
qj604184 | 2:48f7e5f79caa | 81 | |
pirottealex | 0:a8cee96c9250 | 82 | etat=2; |
pirottealex | 1:99f469d63b9a | 83 | } |
qj604184 | 2:48f7e5f79caa | 84 | |
qj604184 | 2:48f7e5f79caa | 85 | if((captL1>=1)||(captL3>=1)) { |
pirottealex | 1:99f469d63b9a | 86 | etat=3; |
pirottealex | 1:99f469d63b9a | 87 | flag=0; |
pirottealex | 0:a8cee96c9250 | 88 | } |
pirottealex | 0:a8cee96c9250 | 89 | break; |
pirottealex | 0:a8cee96c9250 | 90 | case(2) : |
qj604184 | 2:48f7e5f79caa | 91 | |
qj604184 | 2:48f7e5f79caa | 92 | if(infra<5) { |
pirottealex | 0:a8cee96c9250 | 93 | etat=3; |
pirottealex | 0:a8cee96c9250 | 94 | } |
qj604184 | 2:48f7e5f79caa | 95 | if(us_gauche<15 || us_droit<15){ |
qj604184 | 2:48f7e5f79caa | 96 | etat=200; |
qj604184 | 2:48f7e5f79caa | 97 | } |
qj604184 | 2:48f7e5f79caa | 98 | if((captL1>=1)||(captL3>=1)) { |
pirottealex | 1:99f469d63b9a | 99 | etat=3; |
pirottealex | 1:99f469d63b9a | 100 | } |
pirottealex | 0:a8cee96c9250 | 101 | break; |
pirottealex | 0:a8cee96c9250 | 102 | case(3) : |
qj604184 | 2:48f7e5f79caa | 103 | |
qj604184 | 2:48f7e5f79caa | 104 | if(us_arriere<25) { |
pirottealex | 0:a8cee96c9250 | 105 | etat=100; |
pirottealex | 0:a8cee96c9250 | 106 | } |
pirottealex | 0:a8cee96c9250 | 107 | break; |
pirottealex | 0:a8cee96c9250 | 108 | case(21) : |
pirottealex | 0:a8cee96c9250 | 109 | cap_rot=cap_set+30; |
qj604184 | 2:48f7e5f79caa | 110 | if(cap_rot>360) { |
pirottealex | 0:a8cee96c9250 | 111 | cap_rot=cap_rot-360; |
pirottealex | 0:a8cee96c9250 | 112 | } |
qj604184 | 2:48f7e5f79caa | 113 | |
qj604184 | 2:48f7e5f79caa | 114 | if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180)) { |
pirottealex | 0:a8cee96c9250 | 115 | etat=22; |
pirottealex | 0:a8cee96c9250 | 116 | } |
qj604184 | 2:48f7e5f79caa | 117 | if(blocks) { |
pirottealex | 0:a8cee96c9250 | 118 | etat=1; |
pirottealex | 0:a8cee96c9250 | 119 | } |
pirottealex | 0:a8cee96c9250 | 120 | break; |
pirottealex | 0:a8cee96c9250 | 121 | case(22) : |
pirottealex | 0:a8cee96c9250 | 122 | cap_rot=cap_set-30; |
qj604184 | 2:48f7e5f79caa | 123 | if(cap_rot<0) { |
pirottealex | 0:a8cee96c9250 | 124 | cap_rot=cap_rot+360; |
pirottealex | 0:a8cee96c9250 | 125 | } |
qj604184 | 2:48f7e5f79caa | 126 | |
qj604184 | 2:48f7e5f79caa | 127 | if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180)) { |
pirottealex | 0:a8cee96c9250 | 128 | etat=21; |
pirottealex | 0:a8cee96c9250 | 129 | } |
qj604184 | 2:48f7e5f79caa | 130 | if(blocks) { |
pirottealex | 0:a8cee96c9250 | 131 | etat=1; |
pirottealex | 0:a8cee96c9250 | 132 | } |
pirottealex | 0:a8cee96c9250 | 133 | break; |
qj604184 | 2:48f7e5f79caa | 134 | |
qj604184 | 2:48f7e5f79caa | 135 | } |
qj604184 | 2:48f7e5f79caa | 136 | |
qj604184 | 2:48f7e5f79caa | 137 | switch(etat) { |
qj604184 | 2:48f7e5f79caa | 138 | case(0) : |
qj604184 | 2:48f7e5f79caa | 139 | vitmoteur(0,0); |
qj604184 | 2:48f7e5f79caa | 140 | break; |
qj604184 | 2:48f7e5f79caa | 141 | |
qj604184 | 2:48f7e5f79caa | 142 | case(1) : |
qj604184 | 2:48f7e5f79caa | 143 | vitmoteur(V_max-K*errorX,V_max+K*errorX); |
qj604184 | 2:48f7e5f79caa | 144 | break; |
qj604184 | 2:48f7e5f79caa | 145 | |
qj604184 | 2:48f7e5f79caa | 146 | case(2) : |
qj604184 | 2:48f7e5f79caa | 147 | cap_avant=cap_set; |
qj604184 | 2:48f7e5f79caa | 148 | erreur_av=cap_avant-gBoussole; |
qj604184 | 2:48f7e5f79caa | 149 | |
qj604184 | 2:48f7e5f79caa | 150 | if(erreur_av>20) { |
qj604184 | 2:48f7e5f79caa | 151 | erreur_av=20; |
qj604184 | 2:48f7e5f79caa | 152 | } |
qj604184 | 2:48f7e5f79caa | 153 | if(erreur_av<-20) { |
qj604184 | 2:48f7e5f79caa | 154 | erreur_av=-20; |
qj604184 | 2:48f7e5f79caa | 155 | } |
qj604184 | 2:48f7e5f79caa | 156 | vitmoteur(V_max*1.5+erreur_av*0.2,V_max*1.5-erreur_av*0.2); |
qj604184 | 2:48f7e5f79caa | 157 | //vitmoteur(V_max*1.5,V_max*1.5); |
qj604184 | 2:48f7e5f79caa | 158 | break; |
qj604184 | 2:48f7e5f79caa | 159 | |
qj604184 | 2:48f7e5f79caa | 160 | case(3) : |
qj604184 | 2:48f7e5f79caa | 161 | |
qj604184 | 2:48f7e5f79caa | 162 | cap_arriere=cap_set; |
qj604184 | 2:48f7e5f79caa | 163 | erreur_ar=cap_arriere-gBoussole; |
qj604184 | 2:48f7e5f79caa | 164 | |
qj604184 | 2:48f7e5f79caa | 165 | if(erreur_ar>20) { |
qj604184 | 2:48f7e5f79caa | 166 | erreur_ar=20; |
qj604184 | 2:48f7e5f79caa | 167 | } |
qj604184 | 2:48f7e5f79caa | 168 | if(erreur_ar<-20) { |
qj604184 | 2:48f7e5f79caa | 169 | erreur_ar=-20; |
qj604184 | 2:48f7e5f79caa | 170 | } |
qj604184 | 2:48f7e5f79caa | 171 | vitmoteur(-V_moy+erreur_ar*0.2,-V_moy-erreur_ar*0.2); |
qj604184 | 2:48f7e5f79caa | 172 | |
qj604184 | 2:48f7e5f79caa | 173 | break; |
qj604184 | 2:48f7e5f79caa | 174 | |
qj604184 | 2:48f7e5f79caa | 175 | case(21) : |
qj604184 | 2:48f7e5f79caa | 176 | vitmoteur(V_recherche,-V_recherche); |
qj604184 | 2:48f7e5f79caa | 177 | break; |
qj604184 | 2:48f7e5f79caa | 178 | |
qj604184 | 2:48f7e5f79caa | 179 | case(22) : |
qj604184 | 2:48f7e5f79caa | 180 | vitmoteur(-V_recherche,V_recherche); |
qj604184 | 2:48f7e5f79caa | 181 | break; |
qj604184 | 2:48f7e5f79caa | 182 | |
qj604184 | 2:48f7e5f79caa | 183 | case(100) : |
qj604184 | 2:48f7e5f79caa | 184 | break; |
qj604184 | 2:48f7e5f79caa | 185 | |
qj604184 | 2:48f7e5f79caa | 186 | case(200): |
qj604184 | 2:48f7e5f79caa | 187 | vitmoteur(0,20); |
qj604184 | 2:48f7e5f79caa | 188 | wait(2); |
qj604184 | 2:48f7e5f79caa | 189 | vitmoteur(20,20); |
qj604184 | 2:48f7e5f79caa | 190 | wait(2); |
qj604184 | 2:48f7e5f79caa | 191 | break; |
pirottealex | 0:a8cee96c9250 | 192 | |
pirottealex | 0:a8cee96c9250 | 193 | } |
qj604184 | 2:48f7e5f79caa | 194 | |
pirottealex | 0:a8cee96c9250 | 195 | } |
pirottealex | 0:a8cee96c9250 | 196 | } |
qj604184 | 2:48f7e5f79caa | 197 | |
pirottealex | 0:a8cee96c9250 | 198 | |
qj604184 | 2:48f7e5f79caa | 199 |