Tuk workshop
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp
- Committer:
- tuk4
- Date:
- 2019-10-31
- Revision:
- 17:961a91037658
- Parent:
- 16:c0b098c83233
- Child:
- 18:96264a9ddaf6
File content as of revision 17:961a91037658:
#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 right_center (PC_3); CNY70 left_center (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 state is %d x is %.2lf, y is %.2lf, theta is %.2lf",current_state,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 (left_center.whatAmIOn()== 0) { led1==1; motor.resetPosition(); } if (x<=-1300) current_state = STUPID1; 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"); } } break; case STUPID1 : wait(0.2); current_state = HARVEST1; break; case STUPID2 : wait(0.2); current_state = HARVEST2; break; case STUPID3 : wait(0.2); current_state = HARVEST3; break; case HARVEST1 : motor.setSpeed(-speed_L,0.0); if (theta < (-2*PI)) current_state = STUPID2; break; case HARVEST2 : motor.setSpeed(0.0,-speed_R); if (theta > 0) current_state = STUPID3; break; case HARVEST3 : motor.setSpeed(speed_L,-speed_R); if (theta > (2*PI)) current_state = IDLE; break; default : //do something break; } } return 0; }