Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 11:31564089b41c
- Parent:
- 10:6c3653c53eca
- Child:
- 12:548cdc49cdba
diff -r 6c3653c53eca -r 31564089b41c main.cpp --- a/main.cpp Fri Oct 25 11:19:43 2019 +0000 +++ b/main.cpp Fri Oct 25 12:40:27 2019 +0000 @@ -10,7 +10,7 @@ #define PI 3.1415926535898 Serial pc (PA_2, PA_3, 115200); -TIM + PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); @@ -21,7 +21,7 @@ VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); -PIXY pixy (PA_0, PA_1, 115200); +PIXY pixy (PA_0, PA_1, 50000); DigitalIn bp (PC_13); @@ -49,18 +49,37 @@ motor.setProportionnalValue(2.0); motor.setintegralValue(0.4); motor.setDerivativeValue(1.0); - + double speed_L = -50, speed_R = -50; while (1) { if (pixy.checkNewImage()) { pixy.detectedObject(&numberOfObjects,&dummy); if (numberOfObjects==1) { file=pixy.getNMBloc(); - if (file.x >160){ + pc.printf("\r x = %d", file.x); + if (file.x >165){ + motor.setSpeed(-speed_L,speed_R);// turn right + pc.printf(" - Turning right"); + } + else if (file.x <155){ + motor.setSpeed(speed_L,-speed_R);// turn left + pc.printf(" - Turning left"); + } + else { + motor.setSpeed(0,0); } } - }else numberOfObjects = 0; - - double speed_L = -100, speed_R = -100; + else { + motor.setSpeed(0,0); + 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)