Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 16:08bd2d5d21fb
- Parent:
- 15:a0a88aeb4fe1
- Child:
- 17:631f978121c2
diff -r a0a88aeb4fe1 -r 08bd2d5d21fb main.cpp --- a/main.cpp Thu Oct 31 07:45:20 2019 +0000 +++ b/main.cpp Thu Oct 31 09:00:36 2019 +0000 @@ -34,43 +34,49 @@ DigitalIn unused6 (PC_1, PullUp); DigitalIn unused7 (PC_0, PullUp); -Timer temps; -typedef enum {IDLE,FORWARD,REVERSE} T_state; +Timer timer; + +typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN} T_state; main () { - double x,y,theta, VL=0, VR=0; + T_pixyNMBloc NMBloc; + int nbNM,nbCC; + double VL=0,VR=0; T_state state= IDLE; - motor.resetPosition(); - + timer.start(); while (1) { - pc.printf("\r x is %lf ",x); - motor.getPosition(&x,&y,&theta); - motor.setSpeed (VL,VR); + motor.setSpeed(VL,VR); switch (state) { case IDLE: - if (button==0)state= FORWARD; + if (pixy.checkPixy()==0)state= WAIT1; + break; + case WAIT1: + if (button==0) state= WAIT2; + break; + case WAIT2: VL=0; VR=0; + if (pixy.checkNewImage()) state= HOW; + break; - case FORWARD: - if (x<-300) state= REVERSE; - VL=-100; - VR=-100; - break; - case REVERSE: - if (x>0) state= IDLE; - VL=100; - VR=100; + case HOW: + pixy.detectedObject(&nbNM,&nbCC); + if(nbNM==1)state= GET; + else state= WAIT2; break; - - - - }// switch - }// while 1 + case GET: + NMBloc=pixy.getNMBloc(); + state=TURN; + timer.reset(); + break; + case TURN: + VL=NMBloc.x-159; + VR=159-NMBloc.x; + if(pixy.checkNewImage()) state=HOW; + if(timer.read()>0.5f) state= WAIT2; - -}// main - - - + break; + } + } +}