Test code 29-10-2019
Dependencies: mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy
main.cpp@18:ede80eb1e6d5, 2019-10-31 (annotated)
- Committer:
- ephymugo
- Date:
- Thu Oct 31 12:27:29 2019 +0000
- Revision:
- 18:ede80eb1e6d5
- Parent:
- 17:631f978121c2
ALIGNING ROBOT
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
haarkon | 0:4902b3f17137 | 1 | #include "mbed.h" |
haarkon | 0:4902b3f17137 | 2 | #include "CMPS03.h" |
haarkon | 0:4902b3f17137 | 3 | #include "CNY70.h" |
haarkon | 0:4902b3f17137 | 4 | #include "GP2A.h" |
haarkon | 0:4902b3f17137 | 5 | #include "VMA306.h" |
haarkon | 3:0221daeeaa86 | 6 | #include "Pixy.h" |
haarkon | 4:5038b4cd1088 | 7 | #include "PID.h" |
haarkon | 4:5038b4cd1088 | 8 | |
haarkon | 4:5038b4cd1088 | 9 | #define PI 3.1415926535898 |
haarkon | 0:4902b3f17137 | 10 | |
haarkon | 8:2d4887624f94 | 11 | Serial pc (PA_2, PA_3, 115200); |
haarkon | 7:6d58adc26b78 | 12 | PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5); |
haarkon | 0:4902b3f17137 | 13 | |
haarkon | 3:0221daeeaa86 | 14 | CMPS03 boussole (PC_4); |
haarkon | 0:4902b3f17137 | 15 | |
haarkon | 3:0221daeeaa86 | 16 | CNY70 ligneD (PC_3); |
haarkon | 3:0221daeeaa86 | 17 | CNY70 ligneG (PC_2); |
haarkon | 3:0221daeeaa86 | 18 | CNY70 exterior (PA_7); |
haarkon | 0:4902b3f17137 | 19 | |
haarkon | 8:2d4887624f94 | 20 | VMA306 ultraSon (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2); |
haarkon | 0:4902b3f17137 | 21 | |
ephymugo | 15:a0a88aeb4fe1 | 22 | PIXY pixy (PA_0, PA_1, 115200); |
haarkon | 0:4902b3f17137 | 23 | |
ephymugo | 15:a0a88aeb4fe1 | 24 | InterruptIn button (PC_13); |
haarkon | 4:5038b4cd1088 | 25 | |
haarkon | 0:4902b3f17137 | 26 | DigitalOut led1 (PA_5); |
haarkon | 0:4902b3f17137 | 27 | DigitalOut led2 (PD_2); |
haarkon | 8:2d4887624f94 | 28 | DigitalOut unused1 (PB_10); |
haarkon | 8:2d4887624f94 | 29 | DigitalOut unused2 (PA_15); |
haarkon | 8:2d4887624f94 | 30 | DigitalOut unused3 (PA_12); |
haarkon | 8:2d4887624f94 | 31 | |
haarkon | 8:2d4887624f94 | 32 | DigitalIn unused4 (PA_4, PullUp); |
haarkon | 8:2d4887624f94 | 33 | DigitalIn unused5 (PB_0, PullUp); |
haarkon | 8:2d4887624f94 | 34 | DigitalIn unused6 (PC_1, PullUp); |
haarkon | 8:2d4887624f94 | 35 | DigitalIn unused7 (PC_0, PullUp); |
haarkon | 8:2d4887624f94 | 36 | |
ephymugo | 16:08bd2d5d21fb | 37 | Timer timer; |
ephymugo | 16:08bd2d5d21fb | 38 | |
ephymugo | 18:ede80eb1e6d5 | 39 | typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,ALIGN,FOLLOW} T_state; |
haarkon | 0:4902b3f17137 | 40 | |
ephymugo | 15:a0a88aeb4fe1 | 41 | main () |
haarkon | 3:0221daeeaa86 | 42 | { |
ephymugo | 16:08bd2d5d21fb | 43 | T_pixyNMBloc NMBloc; |
ephymugo | 16:08bd2d5d21fb | 44 | int nbNM,nbCC; |
ephymugo | 17:631f978121c2 | 45 | double x,y,theta,VL=0,VR=0; |
ephymugo | 15:a0a88aeb4fe1 | 46 | T_state state= IDLE; |
ephymugo | 16:08bd2d5d21fb | 47 | timer.start(); |
haarkon | 3:0221daeeaa86 | 48 | while (1) { |
ephymugo | 17:631f978121c2 | 49 | motor.getPosition(&x,&y,&theta); |
ephymugo | 16:08bd2d5d21fb | 50 | motor.setSpeed(VL,VR); |
ephymugo | 18:ede80eb1e6d5 | 51 | float distance= ultraSon.readUSB(); |
ephymugo | 15:a0a88aeb4fe1 | 52 | switch (state) { |
ephymugo | 15:a0a88aeb4fe1 | 53 | case IDLE: |
ephymugo | 16:08bd2d5d21fb | 54 | if (pixy.checkPixy()==0)state= WAIT1; |
ephymugo | 16:08bd2d5d21fb | 55 | break; |
ephymugo | 16:08bd2d5d21fb | 56 | case WAIT1: |
ephymugo | 16:08bd2d5d21fb | 57 | if (button==0) state= WAIT2; |
ephymugo | 16:08bd2d5d21fb | 58 | break; |
ephymugo | 16:08bd2d5d21fb | 59 | case WAIT2: |
ephymugo | 15:a0a88aeb4fe1 | 60 | VL=0; |
ephymugo | 15:a0a88aeb4fe1 | 61 | VR=0; |
ephymugo | 16:08bd2d5d21fb | 62 | if (pixy.checkNewImage()) state= HOW; |
ephymugo | 16:08bd2d5d21fb | 63 | |
ephymugo | 15:a0a88aeb4fe1 | 64 | break; |
ephymugo | 16:08bd2d5d21fb | 65 | case HOW: |
ephymugo | 16:08bd2d5d21fb | 66 | pixy.detectedObject(&nbNM,&nbCC); |
ephymugo | 16:08bd2d5d21fb | 67 | if(nbNM==1)state= GET; |
ephymugo | 16:08bd2d5d21fb | 68 | else state= WAIT2; |
ephymugo | 15:a0a88aeb4fe1 | 69 | break; |
ephymugo | 16:08bd2d5d21fb | 70 | case GET: |
ephymugo | 16:08bd2d5d21fb | 71 | NMBloc=pixy.getNMBloc(); |
ephymugo | 16:08bd2d5d21fb | 72 | state=TURN; |
ephymugo | 16:08bd2d5d21fb | 73 | timer.reset(); |
ephymugo | 16:08bd2d5d21fb | 74 | break; |
ephymugo | 16:08bd2d5d21fb | 75 | case TURN: |
ephymugo | 16:08bd2d5d21fb | 76 | VL=NMBloc.x-159; |
ephymugo | 16:08bd2d5d21fb | 77 | VR=159-NMBloc.x; |
ephymugo | 16:08bd2d5d21fb | 78 | if(pixy.checkNewImage()) state=HOW; |
ephymugo | 16:08bd2d5d21fb | 79 | if(timer.read()>0.5f) state= WAIT2; |
ephymugo | 17:631f978121c2 | 80 | if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW; |
ephymugo | 17:631f978121c2 | 81 | else state = WAIT2; |
ephymugo | 15:a0a88aeb4fe1 | 82 | |
ephymugo | 16:08bd2d5d21fb | 83 | break; |
ephymugo | 18:ede80eb1e6d5 | 84 | case ALIGN: |
ephymugo | 18:ede80eb1e6d5 | 85 | VL=NMBloc.x-159; |
ephymugo | 18:ede80eb1e6d5 | 86 | VR=159-NMBloc.x; |
ephymugo | 18:ede80eb1e6d5 | 87 | if(pixy.checkNewImage()) state=HOW; |
ephymugo | 18:ede80eb1e6d5 | 88 | if(timer.read()>0.5f) state= WAIT2; |
ephymugo | 18:ede80eb1e6d5 | 89 | if(NMBloc.x > 155 && NMBloc.x < 165) state= FOLLOW; |
ephymugo | 18:ede80eb1e6d5 | 90 | else state = WAIT2; |
ephymugo | 18:ede80eb1e6d5 | 91 | |
ephymugo | 18:ede80eb1e6d5 | 92 | break; |
ephymugo | 17:631f978121c2 | 93 | |
ephymugo | 17:631f978121c2 | 94 | case FOLLOW : |
ephymugo | 17:631f978121c2 | 95 | if(x<-300)state= WAIT2; |
ephymugo | 18:ede80eb1e6d5 | 96 | // if(distance>20)state=WAIT2; |
ephymugo | 17:631f978121c2 | 97 | VL=-100; |
ephymugo | 17:631f978121c2 | 98 | VR=-100; |
ephymugo | 17:631f978121c2 | 99 | break; |
ephymugo | 18:ede80eb1e6d5 | 100 | |
ephymugo | 18:ede80eb1e6d5 | 101 | |
ephymugo | 16:08bd2d5d21fb | 102 | } |
ephymugo | 16:08bd2d5d21fb | 103 | } |
ephymugo | 16:08bd2d5d21fb | 104 | } |