![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 17:631f978121c2
- Parent:
- 16:08bd2d5d21fb
- Child:
- 18:ede80eb1e6d5
diff -r 08bd2d5d21fb -r 631f978121c2 main.cpp --- a/main.cpp Thu Oct 31 09:00:36 2019 +0000 +++ b/main.cpp Thu Oct 31 09:52:11 2019 +0000 @@ -36,17 +36,19 @@ Timer timer; -typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN} T_state; +typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,FOLLOW} T_state; main () { T_pixyNMBloc NMBloc; int nbNM,nbCC; - double VL=0,VR=0; + double x,y,theta,VL=0,VR=0; T_state state= IDLE; timer.start(); while (1) { + motor.getPosition(&x,&y,&theta); motor.setSpeed(VL,VR); + //float distance= ultraSon.readUSB(); switch (state) { case IDLE: if (pixy.checkPixy()==0)state= WAIT1; @@ -75,8 +77,16 @@ VR=159-NMBloc.x; if(pixy.checkNewImage()) state=HOW; if(timer.read()>0.5f) state= WAIT2; + if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW; + else state = WAIT2; break; + + case FOLLOW : + if(x<-300)state= WAIT2; + VL=-100; + VR=-100; + break; } } }