Tuk workshop
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- tuk4
- Date:
- 2019-10-25
- Revision:
- 11:31564089b41c
- Parent:
- 10:6c3653c53eca
- Child:
- 12:548cdc49cdba
File content as of revision 11:31564089b41c:
#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 = -50, speed_R = -50; while (1) { 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,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 { 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) { 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; }