Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
Diff: main.cpp
- Revision:
- 15:a0a88aeb4fe1
- Parent:
- 14:899c6d29c0a8
- Child:
- 16:08bd2d5d21fb
diff -r 899c6d29c0a8 -r a0a88aeb4fe1 main.cpp --- a/main.cpp Fri Oct 25 13:53:17 2019 +0000 +++ b/main.cpp Thu Oct 31 07:45:20 2019 +0000 @@ -5,12 +5,10 @@ #include "VMA306.h" #include "Pixy.h" #include "PID.h" -#include "motor_state.h" #define PI 3.1415926535898 Serial pc (PA_2, PA_3, 115200); - PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); CMPS03 boussole (PC_4); @@ -21,9 +19,9 @@ VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); -PIXY pixy (PA_0, PA_1, 50000); +PIXY pixy (PA_0, PA_1, 115200); -DigitalIn bp (PC_13); +InterruptIn button (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); @@ -37,99 +35,42 @@ DigitalIn unused7 (PC_0, PullUp); Timer temps; +typedef enum {IDLE,FORWARD,REVERSE} T_state; -motor_state current_state = IDLE; - -int main () +main () { - int numberOfObjects=0, dummy=0; - T_pixyNMBloc file; + double x,y,theta, VL=0, VR=0; + T_state state= IDLE; motor.resetPosition(); - double x=0, y=0, theta = 0; - motor.setProportionnalValue(2.0); - motor.setintegralValue(0.4); - motor.setDerivativeValue(1.0); - double speed_L = -200, speed_R = -200; + while (1) { - - 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 = TURN_RIGHT; - break; - - case TURN_RIGHT : - motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right - - if (pixy.checkNewImage()) { - pixy.detectedObject(&numberOfObjects,&dummy); - if (numberOfObjects==1) { - current_state = LOOK_FOR_FILE; - } - } - break; - - case SAFEMODE : - motor.setSpeed(0,0); - if (ultraSon.readUSB() >= 20) current_state = FORWARD; - if (ultraSon.readUSB() <= 15) current_state = BACKWARD; - break; + pc.printf("\r x is %lf ",x); + motor.getPosition(&x,&y,&theta); + motor.setSpeed (VL,VR); + switch (state) { + case IDLE: + if (button==0)state= FORWARD; + VL=0; + VR=0; + break; + case FORWARD: + if (x<-300) state= REVERSE; + VL=-100; + VR=-100; + break; + case REVERSE: + if (x>0) state= IDLE; + VL=100; + VR=100; + 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(); - pc.printf("\r x = %d", file.x); - if (file.x >165){ - motor.setSpeed(-speed_L/10.0,speed_R/10.0);// turn right - pc.printf(" - Turning right"); - } - else if (file.x <155){ - motor.setSpeed(speed_L/10.0,-speed_R/10.0);// turn left - pc.printf(" - Turning left"); - } - else { - motor.setSpeed(0,0); - motor.resetPosition(); - current_state = FORWARD; - } - } - else { - motor.setSpeed(-speed_L,speed_R);// turn right - pc.printf(" - No object"); - } - } - else - { - numberOfObjects = 0; - //motor.setSpeed(-speed_L,speed_R);// turn right - // motor.setSpeed(0,0); - } // end of pixy.checkNewImage - - break; - - default : - //do something - break; - } - } - - return 0; -} \ No newline at end of file + + + }// switch + }// while 1 + + +}// main + + +