Ephantus Mugo / Mbed 2 deprecated Kenya_2019

Test code 29-10-2019

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

main.cpp@17:631f978121c2, 2019-10-31 (annotated)

Committer:
ephymugo
Date:
Thu Oct 31 09:52:11 2019 +0000
Revision:
17:631f978121c2
Parent:
16:08bd2d5d21fb
Child:
18:ede80eb1e6d5
pixy detecting and tracking object

Who changed what in which revision?

UserRevisionLine numberNew 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 17:631f978121c2 39 typedef enum {IDLE,WAIT1,WAIT2,HOW,GET,TURN,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 17:631f978121c2 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 17:631f978121c2 84
ephymugo 17:631f978121c2 85 case FOLLOW :
ephymugo 17:631f978121c2 86 if(x<-300)state= WAIT2;
ephymugo 17:631f978121c2 87 VL=-100;
ephymugo 17:631f978121c2 88 VR=-100;
ephymugo 17:631f978121c2 89 break;
ephymugo 16:08bd2d5d21fb 90 }
ephymugo 16:08bd2d5d21fb 91 }
ephymugo 16:08bd2d5d21fb 92 }