![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- tuk4
- Date:
- 2019-10-25
- Revision:
- 14:899c6d29c0a8
- Parent:
- 13:f81c8451d601
- Child:
- 15:a0a88aeb4fe1
File content as of revision 14:899c6d29c0a8:
#include "mbed.h" #include "CMPS03.h" #include "CNY70.h" #include "GP2A.h" #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); CNY70 ligneD (PC_3); CNY70 ligneG (PC_2); CNY70 exterior (PA_7); VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); PIXY pixy (PA_0, PA_1, 50000); DigitalIn bp (PC_13); DigitalOut led1 (PA_5); DigitalOut led2 (PD_2); DigitalOut unused1 (PB_10); DigitalOut unused2 (PA_15); DigitalOut unused3 (PA_12); DigitalIn unused4 (PA_4, PullUp); DigitalIn unused5 (PB_0, PullUp); DigitalIn unused6 (PC_1, PullUp); DigitalIn unused7 (PC_0, PullUp); Timer temps; motor_state current_state = IDLE; int main () { int numberOfObjects=0, dummy=0; T_pixyNMBloc file; 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; 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; }