Zhilin He / Mbed 2 deprecated TDPS-COM1

UNFINISHED

Dependencies:   HCSR04 SRF05 mbed

Distance.h@1:60c79e942c98, 2018-06-08 (annotated)

Committer:
Charlie_He
Date:
Fri Jun 08 22:22:10 2018 +0000
Revision:
1:60c79e942c98
Parent:
0:4ca3e247b86a 
TDPS; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Charlie_He 0:4ca3e247b86a 1 #include "mbed.h"
Charlie_He 0:4ca3e247b86a 2 #include "HCSR04.h"
Charlie_He 0:4ca3e247b86a 3 #include "SRF05.h"
Charlie_He 0:4ca3e247b86a 4 #include "gyro.h"
Charlie_He 0:4ca3e247b86a 5 SRF05 Usound1(p26,p15);
Charlie_He 0:4ca3e247b86a 6 HCSR04 Usound2(p25,p16);
Charlie_He 0:4ca3e247b86a 7 Ticker t1;
Charlie_He 1:60c79e942c98 8
Charlie_He 1:60c79e942c98 9 extern Motor M1;
Charlie_He 1:60c79e942c98 10 extern Motor M2;
Charlie_He 1:60c79e942c98 11
Charlie_He 0:4ca3e247b86a 12 int n=0;
Charlie_He 1:60c79e942c98 13 int turns=0;
Charlie_He 1:60c79e942c98 14 void enable1();
Charlie_He 1:60c79e942c98 15 void enable2();
Charlie_He 1:60c79e942c98 16 void turnR()
Charlie_He 1:60c79e942c98 17 {
Charlie_He 1:60c79e942c98 18 M1=1;M2=-1; //右转90
Charlie_He 1:60c79e942c98 19 wait(0.75);
Charlie_He 1:60c79e942c98 20 }
Charlie_He 1:60c79e942c98 21 void turnL()
Charlie_He 1:60c79e942c98 22 {
Charlie_He 1:60c79e942c98 23 M1=-1;M2=1; //左转90
Charlie_He 1:60c79e942c98 24 wait(0.75);
Charlie_He 1:60c79e942c98 25 }
Charlie_He 0:4ca3e247b86a 26 void distancedetectL()
Charlie_He 0:4ca3e247b86a 27 {
Charlie_He 0:4ca3e247b86a 28 float u= Usound1.read();
Charlie_He 0:4ca3e247b86a 29 if(7<=u<=20) {
Charlie_He 0:4ca3e247b86a 30 n+=1;
Charlie_He 1:60c79e942c98 31 if (n>=3) {
Charlie_He 1:60c79e942c98 32 turnL();
Charlie_He 0:4ca3e247b86a 33 change(90);
Charlie_He 1:60c79e942c98 34 driveit();
Charlie_He 0:4ca3e247b86a 35 n=0;
Charlie_He 1:60c79e942c98 36 enable2();
Charlie_He 0:4ca3e247b86a 37 }
Charlie_He 0:4ca3e247b86a 38 }
Charlie_He 1:60c79e942c98 39 }
Charlie_He 0:4ca3e247b86a 40 void distancedetectR()
Charlie_He 0:4ca3e247b86a 41 {
Charlie_He 0:4ca3e247b86a 42 float u= Usound2.read_cm();
Charlie_He 1:60c79e942c98 43 if(30<=u) {
Charlie_He 0:4ca3e247b86a 44 n+=1;
Charlie_He 1:60c79e942c98 45 if (n>=3) {
Charlie_He 1:60c79e942c98 46 turns+=1;
Charlie_He 1:60c79e942c98 47 turnL();
Charlie_He 1:60c79e942c98 48 if(turns>=3) {
Charlie_He 1:60c79e942c98 49 t1.detach();
Charlie_He 1:60c79e942c98 50 M1=0.5;M2=0.5;
Charlie_He 1:60c79e942c98 51 wait(3);
Charlie_He 1:60c79e942c98 52 M1=0;M2=0;
Charlie_He 1:60c79e942c98 53 } else {
Charlie_He 1:60c79e942c98 54 turnR();
Charlie_He 1:60c79e942c98 55 change(-90);
Charlie_He 1:60c79e942c98 56 driveit();
Charlie_He 1:60c79e942c98 57 n=0;
Charlie_He 1:60c79e942c98 58 enable1();
Charlie_He 0:4ca3e247b86a 59 }
Charlie_He 0:4ca3e247b86a 60 }
Charlie_He 0:4ca3e247b86a 61 }
Charlie_He 1:60c79e942c98 62 }
Charlie_He 0:4ca3e247b86a 63 void enable1()
Charlie_He 0:4ca3e247b86a 64 {
Charlie_He 1:60c79e942c98 65 M1=0;M2=0;
Charlie_He 1:60c79e942c98 66 /* t1.detach();
Charlie_He 1:60c79e942c98 67 t1.attach(&distancedetectR,0.4);*/
Charlie_He 1:60c79e942c98 68 }
Charlie_He 0:4ca3e247b86a 69 void enable2()
Charlie_He 0:4ca3e247b86a 70 {
Charlie_He 1:60c79e942c98 71 t1.detach();
Charlie_He 1:60c79e942c98 72 t1.attach(&distancedetectL,0.4);
Charlie_He 1:60c79e942c98 73 }