Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 10:6c3653c53eca
- Parent:
- 9:4053b5217339
- Child:
- 11:31564089b41c
diff -r 4053b5217339 -r 6c3653c53eca main.cpp --- a/main.cpp Thu Oct 24 14:21:22 2019 +0000 +++ b/main.cpp Fri Oct 25 11:19:43 2019 +0000 @@ -5,10 +5,12 @@ #include "VMA306.h" #include "Pixy.h" #include "PID.h" +#include "motor_state.h" #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 +23,7 @@ PIXY pixy (PA_0, PA_1, 115200); -InterruptIn bp (PC_13); +DigitalIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); @@ -36,20 +38,72 @@ Timer temps; +motor_state current_state = IDLE; + int main () { + int numberOfObjects=0, dummy=0; + T_pixyNMBloc file; motor.resetPosition(); - double x=0, y, theta = 0; - + double x=0, y=0, theta = 0; + motor.setProportionnalValue(2.0); + motor.setintegralValue(0.4); + motor.setDerivativeValue(1.0); + while (1) { - double speed_L = 50, speed_R = 50; - while (x <= 200) + if (pixy.checkNewImage()) { + pixy.detectedObject(&numberOfObjects,&dummy); + if (numberOfObjects==1) { + file=pixy.getNMBloc(); + if (file.x >160){ + } + } + }else numberOfObjects = 0; + + double speed_L = -100, speed_R = -100; + motor.getPosition(&x, &y, &theta); + pc.printf("\r x is %.2lf, y is %.2lf, theta is %.2lf",x,y,theta); + switch (current_state) { - motor.setSpeed(speed_L,speed_R); - motor.getPosition(&x, &y, &theta); - pc.printf("\r x = %lf", x); - } - motor.setSpeed(0,0); + case IDLE : + motor.setSpeed(0,0); + if (bp == 0) current_state = FORWARD; + 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 + break; + } } return 0;