![](/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:
- 12:548cdc49cdba
- Parent:
- 11:31564089b41c
- Child:
- 13:f81c8451d601
diff -r 31564089b41c -r 548cdc49cdba main.cpp --- a/main.cpp Fri Oct 25 12:40:27 2019 +0000 +++ b/main.cpp Fri Oct 25 13:18:15 2019 +0000 @@ -51,7 +51,46 @@ motor.setDerivativeValue(1.0); double speed_L = -50, speed_R = -50; while (1) { - if (pixy.checkNewImage()) { + + motor.getPosition(&x, &y, &theta); + pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta); + switch (current_state) + { + case IDLE : + motor.setSpeed(0,0); + if (bp == 0) current_state = LOOK_FOR_FILE; + break; + + case TURN_RIGHT : + motor.setSpeed(speed_L,-speed_R); + + if (theta <= -PI/3) { + motor.resetPosition(); + current_state = FORWARD; + } + break; + + case SAFEMODE : + motor.setSpeed(0,0); + if (ultraSon.readUSB() >= 20) current_state = FORWARD; + if (ultraSon.readUSB() <= 15) current_state = BACKWARD; + break; + + case FORWARD : + motor.setSpeed(speed_L,speed_R); + if (ultraSon.readUSB() <= 20) { + current_state = SAFEMODE; + } + if (x<=-2000) current_state = IDLE; + break; + + case BACKWARD : + motor.setSpeed(-speed_L,-speed_R); + if (ultraSon.readUSB() >= 15) current_state = SAFEMODE; + break; + + case LOOK_FOR_FILE : + if (pixy.checkNewImage()) { pixy.detectedObject(&numberOfObjects,&dummy); if (numberOfObjects==1) { file=pixy.getNMBloc(); @@ -66,58 +105,22 @@ } else { motor.setSpeed(0,0); + current_state = FORWARD; } } else { - motor.setSpeed(0,0); + motor.setSpeed(-speed_L,speed_R);// turn right pc.printf(" - No object"); } - } - else - { - numberOfObjects = 0; - // motor.setSpeed(0,0); } - //continue; - - motor.getPosition(&x, &y, &theta); - pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta); - switch (current_state) - { - case IDLE : - motor.setSpeed(0,0); - if (bp == 0) current_state = FORWARD; + else + { + numberOfObjects = 0; + //motor.setSpeed(-speed_L,speed_R);// turn right + // motor.setSpeed(0,0); + } // end of pixy.checkNewImage + break; - - case TURN_RIGHT : - motor.setSpeed(speed_L,-speed_R); - - if (theta <= -PI/3) { - motor.resetPosition(); - current_state = FORWARD; - } - break; - - case SAFEMODE : - motor.setSpeed(0,0); - if (ultraSon.readUSB() >= 20) current_state = FORWARD; - break; - - case FORWARD : - motor.setSpeed(speed_L,speed_R); - if (ultraSon.readUSB() <= 20) { - current_state = SAFEMODE; - } - if (x<=-1000) current_state = IDLE; - break; - - case BACKWARD : - motor.setSpeed(-speed_L,-speed_R); - if (x <= 0) { - current_state = IDLE; - } - break; - default : //do something