![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
,,,,,,,,,,,,
Dependencies: CMPS03 SRF05 mbed pixy
Fork of 0000Non_stop_code_v1 by
Diff: main.cpp
- Revision:
- 0:a8cee96c9250
- Child:
- 1:99f469d63b9a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Jun 10 04:58:21 2017 +0000 @@ -0,0 +1,135 @@ +#include "mbed.h" +#include "fct.h" +#include "Pixy.h" +#include "PixyLink.h" +#include "SRF05.h" +#include <CMPS03.h> + +Serial pc (USBTX,USBRX); +int main() +{ + init(); + Pixy pixy= Pixy(p28,p27); + pixy.setSerialOutput(&pc); + pc.printf("alive"); + + while(1) { + lecture_blanc(); // recupere valeurs capteurs ligne blanche + lecture_us(); // recupere valeurs capteurs us + lecture_boussole(); //gBoussole + bout=bp.read(); + blocks = pixy.getBlocks(); + //pc.printf("etat=%d\n\r",etat); + //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); + if (blocks) { + taille=pixy.blocks[j].width*pixy.blocks[j].height; + errorX = (160-pixy.blocks[j].x); + //pc.printf("sig: %d x: %d y: %d width: %d height: %d\n\r", pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y, pixy.blocks[j].width, pixy.blocks[j].height); + //pc.printf("OK"); + } + + switch(etat) + { + case(0) : + + if(bout==0) + { + etat=100; + cap_set=gBoussole; + } + break; + + case(100): + + if(blocks) + { + etat=1; + } + + else + { + etat=21; + } + break; + + case(1) : + + if(!blocks) + { + etat=2; + tempo.stop(); + tempo.reset(); + tempo.start(); + } + break; + case(2) : + + if(tempo.read()>0.5) + { + etat=3; + } + break; + case(3) : + + if(us_arriere<25) + { + etat=100; + } + break; + case(21) : + cap_rot=cap_set+30; + if(cap_rot>360) + { + cap_rot=cap_rot-360; + } + + if((gBoussole>cap_rot)&&(gBoussole<cap_rot+180)) + { + etat=22; + } + if(blocks) + { + etat=1; + } + break; + case(22) : + cap_rot=cap_set-30; + if(cap_rot<0) + { + cap_rot=cap_rot+360; + } + + if((gBoussole<cap_rot)&&(gBoussole>cap_rot-180)) + { + etat=21; + } + if(blocks) + { + etat=1; + } + break; + + } + + switch(etat) + { + case(0) : vitmoteur(0,0);break; + + case(1) : vitmoteur(V_max-K*errorX,V_max+K*errorX);break; + + case(2) : vitmoteur(V_max,V_max);break; + + case(3) : vitmoteur(-V_moy,-V_moy);break; + + case(21) : vitmoteur(V_recherche,-V_recherche);break; + + case(22) : vitmoteur(-V_recherche,V_recherche);break; + + case(100) : break; + } + + } +} + + +