Ephantus Mugo / Mbed 2 deprecated Kenya_2019

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?

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 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 }