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?
User | Revision | Line number | New 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 | } |