![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Revision 18:ede80eb1e6d5, committed 2019-10-31
- Comitter:
- ephymugo
- Date:
- Thu Oct 31 12:27:29 2019 +0000
- Parent:
- 17:631f978121c2
- Commit message:
- ALIGNING ROBOT
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 31 09:52:11 2019 +0000 +++ b/main.cpp Thu Oct 31 12:27:29 2019 +0000 @@ -36,7 +36,7 @@ Timer timer; -typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,FOLLOW} T_state; +typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,ALIGN,FOLLOW} T_state; main () { @@ -48,7 +48,7 @@ while (1) { motor.getPosition(&x,&y,&theta); motor.setSpeed(VL,VR); - //float distance= ultraSon.readUSB(); + float distance= ultraSon.readUSB(); switch (state) { case IDLE: if (pixy.checkPixy()==0)state= WAIT1; @@ -81,12 +81,24 @@ else state = WAIT2; break; + case ALIGN: + VL=NMBloc.x-159; + 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; + // if(distance>20)state=WAIT2; VL=-100; VR=-100; break; + + } } }