Alexandre Pirotte
/
0concours_cachan_programme_ok
ok
Fork of _test_suivi_mur by
main.cpp@7:2f4660e9cf92, 2017-06-23 (annotated)
- Committer:
- pirottealex
- Date:
- Fri Jun 23 11:19:58 2017 +0000
- Revision:
- 7:2f4660e9cf92
- Parent:
- 6:5f7df5c74a77
pixy_ok2;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vermaelen | 0:dcb865a03d57 | 1 | #include "mbed.h" |
vermaelen | 2:82b72fa8dbcd | 2 | #include "fct.h" |
pirottealex | 7:2f4660e9cf92 | 3 | #include "Pixy.h" |
pirottealex | 7:2f4660e9cf92 | 4 | #include "PixyLink.h" |
pirottealex | 7:2f4660e9cf92 | 5 | #include "CMPS03.h" |
pirottealex | 7:2f4660e9cf92 | 6 | |
pirottealex | 7:2f4660e9cf92 | 7 | |
pirottealex | 7:2f4660e9cf92 | 8 | Serial pc(USBTX,USBRX); |
pirottealex | 7:2f4660e9cf92 | 9 | CMPS03 compass(p9, p10, CMPS03_DEFAULT_I2C_ADDRESS); |
vermaelen | 0:dcb865a03d57 | 10 | |
vermaelen | 0:dcb865a03d57 | 11 | int main() |
vermaelen | 0:dcb865a03d57 | 12 | { |
pirottealex | 7:2f4660e9cf92 | 13 | |
vermaelen | 2:82b72fa8dbcd | 14 | init(); |
pirottealex | 7:2f4660e9cf92 | 15 | Pixy pixy = Pixy(p28,p27); |
pirottealex | 7:2f4660e9cf92 | 16 | pixy.setSerialOutput(&pc); |
pirottealex | 7:2f4660e9cf92 | 17 | int tempo=0; |
pirottealex | 7:2f4660e9cf92 | 18 | int i=0; |
pirottealex | 7:2f4660e9cf92 | 19 | while(true) { |
pirottealex | 7:2f4660e9cf92 | 20 | //capg=capb1.read(); |
pirottealex | 7:2f4660e9cf92 | 21 | //capd=capb2.read(); |
pirottealex | 7:2f4660e9cf92 | 22 | capg=0; |
pirottealex | 7:2f4660e9cf92 | 23 | capd=0; |
pirottealex | 7:2f4660e9cf92 | 24 | |
pirottealex | 7:2f4660e9cf92 | 25 | //i++;pc.printf("%d\n\r",i); |
pirottealex | 7:2f4660e9cf92 | 26 | jck=jack.read(); |
pirottealex | 7:2f4660e9cf92 | 27 | leds.write(etat); |
pirottealex | 7:2f4660e9cf92 | 28 | //pc.printf("g=%d,d=%d\n\r",capd,capg); |
pirottealex | 7:2f4660e9cf92 | 29 | //pc.printf("etat=%d,taille=%f,capt_eg=%3.0f,capt_g=%3.0f,capt_m=%3.0f,capt_d=%3.0f,capt_ed=%3.0f\n\r",etat,taille,capt_eg,capt_g,capt_m,capt_d,capt_ed); |
pirottealex | 7:2f4660e9cf92 | 30 | //pc.printf("etat=%d, cap_lum=%f, cap=%f, block=%d,taille=%f\n\r",etat,cap_lum,cap,blocks,taille); |
pirottealex | 7:2f4660e9cf92 | 31 | cap=compass.readBearing()/10.0; |
pirottealex | 7:2f4660e9cf92 | 32 | blocks = pixy.getBlocks(); |
pirottealex | 7:2f4660e9cf92 | 33 | if (blocks) { |
pirottealex | 7:2f4660e9cf92 | 34 | taille=pixy.blocks[j].width*pixy.blocks[j].height; |
pirottealex | 7:2f4660e9cf92 | 35 | errorX = (160-pixy.blocks[j].x); |
pirottealex | 7:2f4660e9cf92 | 36 | errorY = (100-pixy.blocks[j].y); |
pirottealex | 7:2f4660e9cf92 | 37 | } /*else if(!blocks) { |
pirottealex | 7:2f4660e9cf92 | 38 | blocks = pixy.getBlocks(); |
pirottealex | 7:2f4660e9cf92 | 39 | taille=pixy.blocks[j].width*pixy.blocks[j].height; |
pirottealex | 7:2f4660e9cf92 | 40 | errorX = (160-pixy.blocks[j].x); |
pirottealex | 7:2f4660e9cf92 | 41 | errorY = (100-pixy.blocks[j].y); |
pirottealex | 7:2f4660e9cf92 | 42 | }*/ |
pirottealex | 7:2f4660e9cf92 | 43 | if(tr.read()>10) { |
pirottealex | 7:2f4660e9cf92 | 44 | tempo=1; |
pirottealex | 7:2f4660e9cf92 | 45 | } |
pirottealex | 7:2f4660e9cf92 | 46 | |
pirottealex | 7:2f4660e9cf92 | 47 | //pc.printf("taille=%f\n\r",taille); |
vermaelen | 2:82b72fa8dbcd | 48 | switch(etat) { |
pirottealex | 7:2f4660e9cf92 | 49 | case(0) : |
pirottealex | 7:2f4660e9cf92 | 50 | if(jck==1) { |
vermaelen | 2:82b72fa8dbcd | 51 | etat=1; |
pirottealex | 7:2f4660e9cf92 | 52 | cap_lum=cap; |
pirottealex | 7:2f4660e9cf92 | 53 | tr.stop(); |
pirottealex | 7:2f4660e9cf92 | 54 | tr.reset(); |
pirottealex | 7:2f4660e9cf92 | 55 | tr.start(); |
pirottealex | 7:2f4660e9cf92 | 56 | } |
pirottealex | 7:2f4660e9cf92 | 57 | break; |
pirottealex | 7:2f4660e9cf92 | 58 | |
pirottealex | 7:2f4660e9cf92 | 59 | case(1) : |
pirottealex | 7:2f4660e9cf92 | 60 | |
pirottealex | 7:2f4660e9cf92 | 61 | cap_lum=cap; |
pirottealex | 7:2f4660e9cf92 | 62 | if(!blocks) { |
pirottealex | 7:2f4660e9cf92 | 63 | etat=3; |
pirottealex | 7:2f4660e9cf92 | 64 | } |
pirottealex | 7:2f4660e9cf92 | 65 | if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { |
pirottealex | 7:2f4660e9cf92 | 66 | etat=2; |
vermaelen | 2:82b72fa8dbcd | 67 | } |
pirottealex | 7:2f4660e9cf92 | 68 | |
pirottealex | 7:2f4660e9cf92 | 69 | |
pirottealex | 7:2f4660e9cf92 | 70 | if((taille>seuil_taille)&&(tempo==1)) { |
pirottealex | 7:2f4660e9cf92 | 71 | tb.reset(); |
pirottealex | 7:2f4660e9cf92 | 72 | tb.start(); |
pirottealex | 7:2f4660e9cf92 | 73 | etat=100; |
pirottealex | 7:2f4660e9cf92 | 74 | } |
pirottealex | 7:2f4660e9cf92 | 75 | break; |
pirottealex | 7:2f4660e9cf92 | 76 | |
pirottealex | 7:2f4660e9cf92 | 77 | case(2) : |
pirottealex | 7:2f4660e9cf92 | 78 | if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(blocks)) { |
pirottealex | 7:2f4660e9cf92 | 79 | etat=1; |
pirottealex | 7:2f4660e9cf92 | 80 | } |
pirottealex | 7:2f4660e9cf92 | 81 | if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(!blocks)) { |
pirottealex | 7:2f4660e9cf92 | 82 | |
pirottealex | 7:2f4660e9cf92 | 83 | etat=3; |
pirottealex | 7:2f4660e9cf92 | 84 | } |
pirottealex | 7:2f4660e9cf92 | 85 | |
pirottealex | 7:2f4660e9cf92 | 86 | if((taille>seuil_taille)&&(tempo==1)) { |
pirottealex | 7:2f4660e9cf92 | 87 | tb.reset(); |
pirottealex | 7:2f4660e9cf92 | 88 | tb.start(); |
pirottealex | 7:2f4660e9cf92 | 89 | etat=100; |
vermaelen | 3:b91371837109 | 90 | } |
vermaelen | 2:82b72fa8dbcd | 91 | break; |
pirottealex | 7:2f4660e9cf92 | 92 | |
pirottealex | 7:2f4660e9cf92 | 93 | |
pirottealex | 7:2f4660e9cf92 | 94 | case(3) : |
pirottealex | 7:2f4660e9cf92 | 95 | if(cap_lum<180) { |
pirottealex | 7:2f4660e9cf92 | 96 | if(cap<=cap_lum+180 && cap>cap_lum) { |
pirottealex | 7:2f4660e9cf92 | 97 | etat=5; |
pirottealex | 7:2f4660e9cf92 | 98 | } else { |
pirottealex | 7:2f4660e9cf92 | 99 | etat=4; |
pirottealex | 7:2f4660e9cf92 | 100 | } |
pirottealex | 7:2f4660e9cf92 | 101 | } else { |
pirottealex | 7:2f4660e9cf92 | 102 | if(cap<=cap_lum && cap>cap_lum-180) { |
pirottealex | 7:2f4660e9cf92 | 103 | etat=4; |
pirottealex | 7:2f4660e9cf92 | 104 | } |
pirottealex | 7:2f4660e9cf92 | 105 | |
pirottealex | 7:2f4660e9cf92 | 106 | else { |
pirottealex | 7:2f4660e9cf92 | 107 | etat=5; |
pirottealex | 7:2f4660e9cf92 | 108 | } |
pirottealex | 7:2f4660e9cf92 | 109 | } |
pirottealex | 7:2f4660e9cf92 | 110 | |
pirottealex | 7:2f4660e9cf92 | 111 | if((taille>seuil_taille)&&(tempo==1)) { |
pirottealex | 7:2f4660e9cf92 | 112 | tb.reset(); |
pirottealex | 7:2f4660e9cf92 | 113 | tb.start(); |
pirottealex | 7:2f4660e9cf92 | 114 | etat=100; |
pirottealex | 7:2f4660e9cf92 | 115 | } |
pirottealex | 7:2f4660e9cf92 | 116 | break; |
pirottealex | 7:2f4660e9cf92 | 117 | |
pirottealex | 7:2f4660e9cf92 | 118 | case(4) : //pc.printf("tourner a gauche"); |
pirottealex | 7:2f4660e9cf92 | 119 | |
pirottealex | 7:2f4660e9cf92 | 120 | if (blocks) { |
pirottealex | 7:2f4660e9cf92 | 121 | etat=1; |
pirottealex | 7:2f4660e9cf92 | 122 | } |
pirottealex | 7:2f4660e9cf92 | 123 | if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { |
vermaelen | 4:78a9354fcee8 | 124 | etat=2; |
pirottealex | 7:2f4660e9cf92 | 125 | } |
pirottealex | 7:2f4660e9cf92 | 126 | |
pirottealex | 7:2f4660e9cf92 | 127 | if((taille>seuil_taille)&&(tempo==1)) { |
pirottealex | 7:2f4660e9cf92 | 128 | tb.reset(); |
pirottealex | 7:2f4660e9cf92 | 129 | tb.start(); |
pirottealex | 7:2f4660e9cf92 | 130 | etat=100; |
vermaelen | 4:78a9354fcee8 | 131 | } |
pirottealex | 7:2f4660e9cf92 | 132 | break; |
pirottealex | 7:2f4660e9cf92 | 133 | |
pirottealex | 7:2f4660e9cf92 | 134 | case(5) : |
pirottealex | 7:2f4660e9cf92 | 135 | |
pirottealex | 7:2f4660e9cf92 | 136 | //pc.printf("touner a droite"); |
pirottealex | 7:2f4660e9cf92 | 137 | if (blocks) { |
pirottealex | 7:2f4660e9cf92 | 138 | etat=1; |
pirottealex | 7:2f4660e9cf92 | 139 | } |
pirottealex | 7:2f4660e9cf92 | 140 | if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) { |
pirottealex | 7:2f4660e9cf92 | 141 | etat=2; |
pirottealex | 7:2f4660e9cf92 | 142 | } |
pirottealex | 7:2f4660e9cf92 | 143 | |
pirottealex | 7:2f4660e9cf92 | 144 | if((taille>seuil_taille)&&(tempo==1)) { |
pirottealex | 7:2f4660e9cf92 | 145 | tb.reset(); |
pirottealex | 7:2f4660e9cf92 | 146 | tb.start(); |
pirottealex | 7:2f4660e9cf92 | 147 | etat=100; |
pirottealex | 7:2f4660e9cf92 | 148 | } |
pirottealex | 7:2f4660e9cf92 | 149 | break; |
pirottealex | 7:2f4660e9cf92 | 150 | |
pirottealex | 7:2f4660e9cf92 | 151 | case(10) : |
pirottealex | 7:2f4660e9cf92 | 152 | |
pirottealex | 7:2f4660e9cf92 | 153 | if(capt_m>=40) { |
pirottealex | 7:2f4660e9cf92 | 154 | etat=2; |
pirottealex | 7:2f4660e9cf92 | 155 | } |
pirottealex | 7:2f4660e9cf92 | 156 | break; |
pirottealex | 7:2f4660e9cf92 | 157 | |
pirottealex | 7:2f4660e9cf92 | 158 | |
pirottealex | 7:2f4660e9cf92 | 159 | case(100) : |
pirottealex | 7:2f4660e9cf92 | 160 | |
pirottealex | 7:2f4660e9cf92 | 161 | if((capt_m<30) || (capt_g<25)) { |
pirottealex | 7:2f4660e9cf92 | 162 | etat=101; |
pirottealex | 7:2f4660e9cf92 | 163 | t.stop(); |
pirottealex | 7:2f4660e9cf92 | 164 | t.reset(); |
pirottealex | 7:2f4660e9cf92 | 165 | t.start(); |
pirottealex | 7:2f4660e9cf92 | 166 | } |
pirottealex | 7:2f4660e9cf92 | 167 | break; |
pirottealex | 7:2f4660e9cf92 | 168 | |
pirottealex | 7:2f4660e9cf92 | 169 | case(101) : |
pirottealex | 7:2f4660e9cf92 | 170 | if(t.read()<10) { |
pirottealex | 7:2f4660e9cf92 | 171 | servo_start(); |
pirottealex | 7:2f4660e9cf92 | 172 | } else if(t.read()>=10) { |
pirottealex | 7:2f4660e9cf92 | 173 | servo_stop(); |
vermaelen | 2:82b72fa8dbcd | 174 | } |
vermaelen | 4:78a9354fcee8 | 175 | |
vermaelen | 2:82b72fa8dbcd | 176 | break; |
pirottealex | 7:2f4660e9cf92 | 177 | |
pirottealex | 7:2f4660e9cf92 | 178 | |
vermaelen | 2:82b72fa8dbcd | 179 | } |
vermaelen | 2:82b72fa8dbcd | 180 | switch(etat) { |
pirottealex | 7:2f4660e9cf92 | 181 | case(0) : |
pirottealex | 7:2f4660e9cf92 | 182 | vitesse(0,0); |
pirottealex | 7:2f4660e9cf92 | 183 | break; |
pirottealex | 7:2f4660e9cf92 | 184 | |
pirottealex | 7:2f4660e9cf92 | 185 | case(1) : |
pirottealex | 7:2f4660e9cf92 | 186 | vitesse(V_max-K*errorX,V_max+K*errorX); |
vermaelen | 2:82b72fa8dbcd | 187 | break; |
pirottealex | 7:2f4660e9cf92 | 188 | case(2) : |
pirottealex | 7:2f4660e9cf92 | 189 | erreur_cap=(cap_lum+(80-(capt_eg+capt_g+capt_m)/3)*Kdist-(80-(capt_ed+capt_d)/2)*Kdist)-cap; |
pirottealex | 7:2f4660e9cf92 | 190 | if(erreur_cap>180) { |
pirottealex | 7:2f4660e9cf92 | 191 | erreur_cap=erreur_cap-360; |
pirottealex | 7:2f4660e9cf92 | 192 | } |
pirottealex | 7:2f4660e9cf92 | 193 | if(erreur_cap<-180) { |
pirottealex | 7:2f4660e9cf92 | 194 | erreur_cap=erreur_cap+360; |
pirottealex | 7:2f4660e9cf92 | 195 | } |
pirottealex | 7:2f4660e9cf92 | 196 | if(erreur_cap>0) { |
pirottealex | 7:2f4660e9cf92 | 197 | cmdD=V_max-Kcap*erreur_cap; |
pirottealex | 7:2f4660e9cf92 | 198 | cmdG=V_max; |
pirottealex | 7:2f4660e9cf92 | 199 | } else { |
pirottealex | 7:2f4660e9cf92 | 200 | cmdD=V_max; |
pirottealex | 7:2f4660e9cf92 | 201 | cmdG=V_max+Kcap*erreur_cap; |
pirottealex | 7:2f4660e9cf92 | 202 | } |
pirottealex | 7:2f4660e9cf92 | 203 | if(cmdD<0) { |
pirottealex | 7:2f4660e9cf92 | 204 | cmdD=0; |
pirottealex | 7:2f4660e9cf92 | 205 | } |
pirottealex | 7:2f4660e9cf92 | 206 | if(cmdG<0) { |
pirottealex | 7:2f4660e9cf92 | 207 | cmdG=0; |
pirottealex | 7:2f4660e9cf92 | 208 | } |
pirottealex | 7:2f4660e9cf92 | 209 | vitesse(cmdG,cmdD); |
pirottealex | 7:2f4660e9cf92 | 210 | break; |
pirottealex | 7:2f4660e9cf92 | 211 | case(3) : |
vermaelen | 2:82b72fa8dbcd | 212 | break; |
pirottealex | 7:2f4660e9cf92 | 213 | |
pirottealex | 7:2f4660e9cf92 | 214 | case(4) : |
pirottealex | 7:2f4660e9cf92 | 215 | vitesse(V_moy,lent); |
pirottealex | 7:2f4660e9cf92 | 216 | break; |
pirottealex | 7:2f4660e9cf92 | 217 | |
pirottealex | 7:2f4660e9cf92 | 218 | case(5) : |
pirottealex | 7:2f4660e9cf92 | 219 | vitesse(lent,V_moy); |
pirottealex | 7:2f4660e9cf92 | 220 | break; |
pirottealex | 7:2f4660e9cf92 | 221 | |
pirottealex | 7:2f4660e9cf92 | 222 | case(10): |
pirottealex | 7:2f4660e9cf92 | 223 | vitesse(lent,lent); |
vermaelen | 3:b91371837109 | 224 | break; |
pirottealex | 7:2f4660e9cf92 | 225 | |
pirottealex | 7:2f4660e9cf92 | 226 | case(100) : |
pirottealex | 7:2f4660e9cf92 | 227 | |
pirottealex | 7:2f4660e9cf92 | 228 | vitesse(V_max-K*errorX,V_max+K*errorX); |
pirottealex | 7:2f4660e9cf92 | 229 | break; |
pirottealex | 7:2f4660e9cf92 | 230 | |
pirottealex | 7:2f4660e9cf92 | 231 | case(101) : |
pirottealex | 7:2f4660e9cf92 | 232 | |
pirottealex | 7:2f4660e9cf92 | 233 | vitesse(0,0); |
pirottealex | 7:2f4660e9cf92 | 234 | break; |
pirottealex | 7:2f4660e9cf92 | 235 | case(111) : |
pirottealex | 7:2f4660e9cf92 | 236 | vitesse(-30,0); |
pirottealex | 7:2f4660e9cf92 | 237 | break; |
pirottealex | 7:2f4660e9cf92 | 238 | |
pirottealex | 7:2f4660e9cf92 | 239 | case(112) : |
pirottealex | 7:2f4660e9cf92 | 240 | vitesse(0,-30); |
pirottealex | 7:2f4660e9cf92 | 241 | break; |
pirottealex | 7:2f4660e9cf92 | 242 | |
vermaelen | 2:82b72fa8dbcd | 243 | } |
vermaelen | 0:dcb865a03d57 | 244 | } |
vermaelen | 0:dcb865a03d57 | 245 | } |