Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 13:f81c8451d601
- Parent:
- 12:548cdc49cdba
- Child:
- 14:899c6d29c0a8
diff -r 548cdc49cdba -r f81c8451d601 main.cpp --- a/main.cpp Fri Oct 25 13:18:15 2019 +0000 +++ b/main.cpp Fri Oct 25 13:24:47 2019 +0000 @@ -58,16 +58,18 @@ { case IDLE : motor.setSpeed(0,0); - if (bp == 0) current_state = LOOK_FOR_FILE; + if (bp == 0) current_state = TURN_RIGHT; break; case TURN_RIGHT : - motor.setSpeed(speed_L,-speed_R); + motor.setSpeed(-speed_L,speed_R);// turn right - if (theta <= -PI/3) { - motor.resetPosition(); - current_state = FORWARD; + if (pixy.checkNewImage()) { + pixy.detectedObject(&numberOfObjects,&dummy); + if (numberOfObjects==1) { + current_state = LOOK_FOR_FILE; } + } break; case SAFEMODE : @@ -105,6 +107,7 @@ } else { motor.setSpeed(0,0); + motor.resetPosition(); current_state = FORWARD; } }