![](/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@16:08bd2d5d21fb, 2019-10-31 (annotated)
- Committer:
- ephymugo
- Date:
- Thu Oct 31 09:00:36 2019 +0000
- Revision:
- 16:08bd2d5d21fb
- Parent:
- 15:a0a88aeb4fe1
- Child:
- 17:631f978121c2
pixy camera tracking an object
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 | 16:08bd2d5d21fb | 39 | typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN} 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 | 16:08bd2d5d21fb | 45 | double 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 | 16:08bd2d5d21fb | 49 | motor.setSpeed(VL,VR); |
ephymugo | 15:a0a88aeb4fe1 | 50 | switch (state) { |
ephymugo | 15:a0a88aeb4fe1 | 51 | case IDLE: |
ephymugo | 16:08bd2d5d21fb | 52 | if (pixy.checkPixy()==0)state= WAIT1; |
ephymugo | 16:08bd2d5d21fb | 53 | break; |
ephymugo | 16:08bd2d5d21fb | 54 | case WAIT1: |
ephymugo | 16:08bd2d5d21fb | 55 | if (button==0) state= WAIT2; |
ephymugo | 16:08bd2d5d21fb | 56 | break; |
ephymugo | 16:08bd2d5d21fb | 57 | case WAIT2: |
ephymugo | 15:a0a88aeb4fe1 | 58 | VL=0; |
ephymugo | 15:a0a88aeb4fe1 | 59 | VR=0; |
ephymugo | 16:08bd2d5d21fb | 60 | if (pixy.checkNewImage()) state= HOW; |
ephymugo | 16:08bd2d5d21fb | 61 | |
ephymugo | 15:a0a88aeb4fe1 | 62 | break; |
ephymugo | 16:08bd2d5d21fb | 63 | case HOW: |
ephymugo | 16:08bd2d5d21fb | 64 | pixy.detectedObject(&nbNM,&nbCC); |
ephymugo | 16:08bd2d5d21fb | 65 | if(nbNM==1)state= GET; |
ephymugo | 16:08bd2d5d21fb | 66 | else state= WAIT2; |
ephymugo | 15:a0a88aeb4fe1 | 67 | break; |
ephymugo | 16:08bd2d5d21fb | 68 | case GET: |
ephymugo | 16:08bd2d5d21fb | 69 | NMBloc=pixy.getNMBloc(); |
ephymugo | 16:08bd2d5d21fb | 70 | state=TURN; |
ephymugo | 16:08bd2d5d21fb | 71 | timer.reset(); |
ephymugo | 16:08bd2d5d21fb | 72 | break; |
ephymugo | 16:08bd2d5d21fb | 73 | case TURN: |
ephymugo | 16:08bd2d5d21fb | 74 | VL=NMBloc.x-159; |
ephymugo | 16:08bd2d5d21fb | 75 | VR=159-NMBloc.x; |
ephymugo | 16:08bd2d5d21fb | 76 | if(pixy.checkNewImage()) state=HOW; |
ephymugo | 16:08bd2d5d21fb | 77 | if(timer.read()>0.5f) state= WAIT2; |
ephymugo | 15:a0a88aeb4fe1 | 78 | |
ephymugo | 16:08bd2d5d21fb | 79 | break; |
ephymugo | 16:08bd2d5d21fb | 80 | } |
ephymugo | 16:08bd2d5d21fb | 81 | } |
ephymugo | 16:08bd2d5d21fb | 82 | } |