Ephantus Mugo / Mbed 2 deprecated Kenya_2019

Test code 29-10-2019

Dependencies:   mbed RC_Servo VMA306 PID CNY70 CMPS03 GP2A Pixy

main.cpp@15:a0a88aeb4fe1, 2019-10-31 (annotated)

Committer:
ephymugo
Date:
Thu Oct 31 07:45:20 2019 +0000
Revision:
15:a0a88aeb4fe1
Parent:
14:899c6d29c0a8
Child:
16:08bd2d5d21fb
forward-reverse motion

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
haarkon 8:2d4887624f94 37 Timer temps;
ephymugo 15:a0a88aeb4fe1 38 typedef enum {IDLE,FORWARD,REVERSE} T_state;
haarkon 0:4902b3f17137 39
ephymugo 15:a0a88aeb4fe1 40 main ()
haarkon 3:0221daeeaa86 41 {
ephymugo 15:a0a88aeb4fe1 42 double x,y,theta, VL=0, VR=0;
ephymugo 15:a0a88aeb4fe1 43 T_state state= IDLE;
tuk4 9:4053b5217339 44 motor.resetPosition();
ephymugo 15:a0a88aeb4fe1 45
haarkon 3:0221daeeaa86 46 while (1) {
ephymugo 15:a0a88aeb4fe1 47 pc.printf("\r x is %lf ",x);
ephymugo 15:a0a88aeb4fe1 48 motor.getPosition(&x,&y,&theta);
ephymugo 15:a0a88aeb4fe1 49 motor.setSpeed (VL,VR);
ephymugo 15:a0a88aeb4fe1 50 switch (state) {
ephymugo 15:a0a88aeb4fe1 51 case IDLE:
ephymugo 15:a0a88aeb4fe1 52 if (button==0)state= FORWARD;
ephymugo 15:a0a88aeb4fe1 53 VL=0;
ephymugo 15:a0a88aeb4fe1 54 VR=0;
ephymugo 15:a0a88aeb4fe1 55 break;
ephymugo 15:a0a88aeb4fe1 56 case FORWARD:
ephymugo 15:a0a88aeb4fe1 57 if (x<-300) state= REVERSE;
ephymugo 15:a0a88aeb4fe1 58 VL=-100;
ephymugo 15:a0a88aeb4fe1 59 VR=-100;
ephymugo 15:a0a88aeb4fe1 60 break;
ephymugo 15:a0a88aeb4fe1 61 case REVERSE:
ephymugo 15:a0a88aeb4fe1 62 if (x>0) state= IDLE;
ephymugo 15:a0a88aeb4fe1 63 VL=100;
ephymugo 15:a0a88aeb4fe1 64 VR=100;
ephymugo 15:a0a88aeb4fe1 65 break;
tuk4 12:548cdc49cdba 66
ephymugo 15:a0a88aeb4fe1 67
ephymugo 15:a0a88aeb4fe1 68
ephymugo 15:a0a88aeb4fe1 69 }// switch
ephymugo 15:a0a88aeb4fe1 70 }// while 1
ephymugo 15:a0a88aeb4fe1 71
ephymugo 15:a0a88aeb4fe1 72
ephymugo 15:a0a88aeb4fe1 73 }// main
ephymugo 15:a0a88aeb4fe1 74
ephymugo 15:a0a88aeb4fe1 75
ephymugo 15:a0a88aeb4fe1 76