Qingshu Zhang / Mbed 2 deprecated new_HC-SR504

TDPS3

Dependencies:   mbed HC_SR04_Ultrasonic_Library

main.cpp@1:60ad9c191ea9, 2019-03-14 (annotated)

Committer:
QingshuZhang
Date:
Thu Mar 14 14:37:20 2019 +0000
Revision:
1:60ad9c191ea9
Parent:
0:aed6099bd419 
TDPS3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
QingshuZhang 0:aed6099bd419 1
QingshuZhang 1:60ad9c191ea9 2 #include <L298_H.h>
QingshuZhang 0:aed6099bd419 3 #include <mbed.h>
QingshuZhang 0:aed6099bd419 4
QingshuZhang 1:60ad9c191ea9 5
QingshuZhang 0:aed6099bd419 6 Timer t1; //Timer used to record the time duration
QingshuZhang 1:60ad9c191ea9 7
QingshuZhang 0:aed6099bd419 8 DigitalOut trigpin(PTD4);
QingshuZhang 0:aed6099bd419 9 DigitalIn echopin(PTE20);
QingshuZhang 1:60ad9c191ea9 10 DigitalOut trigpin2(PTD1);
QingshuZhang 1:60ad9c191ea9 11 DigitalIn echopin2(PTC9);
QingshuZhang 0:aed6099bd419 12 DigitalOut test(PTE30);
QingshuZhang 0:aed6099bd419 13 DigitalOut led1(PTD2);
QingshuZhang 0:aed6099bd419 14 DigitalOut led2(PTD3); //two LED indicates if there's any obstacles nearby
QingshuZhang 0:aed6099bd419 15 //initialise the class twice, one controls the left part and the other the right
QingshuZhang 0:aed6099bd419 16 bool dirL;
QingshuZhang 0:aed6099bd419 17 bool dirR;
QingshuZhang 0:aed6099bd419 18 float speed;
QingshuZhang 0:aed6099bd419 19
QingshuZhang 1:60ad9c191ea9 20 int getDis (DigitalIn echopin) {
QingshuZhang 1:60ad9c191ea9 21 Timer t;
QingshuZhang 1:60ad9c191ea9 22 int distance;
QingshuZhang 1:60ad9c191ea9 23 float duration;
QingshuZhang 1:60ad9c191ea9 24 while (!echopin); // read the duration time
QingshuZhang 1:60ad9c191ea9 25 t.start();
QingshuZhang 1:60ad9c191ea9 26 while (echopin);
QingshuZhang 1:60ad9c191ea9 27 t.stop();
QingshuZhang 1:60ad9c191ea9 28 duration = t.read_us();
QingshuZhang 1:60ad9c191ea9 29 distance = (duration/2) / 29.1;
QingshuZhang 1:60ad9c191ea9 30 t.reset();
QingshuZhang 1:60ad9c191ea9 31 printf("distance: %d\n",distance); //read the distance
QingshuZhang 1:60ad9c191ea9 32 test = 1;
QingshuZhang 1:60ad9c191ea9 33 wait_ms(100);
QingshuZhang 1:60ad9c191ea9 34 test=0;
QingshuZhang 1:60ad9c191ea9 35 return distance;
QingshuZhang 1:60ad9c191ea9 36 }
QingshuZhang 1:60ad9c191ea9 37
QingshuZhang 1:60ad9c191ea9 38
QingshuZhang 0:aed6099bd419 39 int main() {
QingshuZhang 1:60ad9c191ea9 40 L298 controlLeft(PTA12,PTA4,PTA5);
QingshuZhang 1:60ad9c191ea9 41 L298 controlRight(PTA13,PTD5,PTD0);
QingshuZhang 1:60ad9c191ea9 42 dirR=1;
QingshuZhang 1:60ad9c191ea9 43 dirL=1;
QingshuZhang 0:aed6099bd419 44 speed=0.2;
QingshuZhang 1:60ad9c191ea9 45 int distance;
QingshuZhang 1:60ad9c191ea9 46 int distance2;
QingshuZhang 1:60ad9c191ea9 47 int totalT;
QingshuZhang 0:aed6099bd419 48 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 49 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 50 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 51 controlRight.SetSpeed(speed);
QingshuZhang 1:60ad9c191ea9 52 test =1;
QingshuZhang 1:60ad9c191ea9 53 led1=1;
QingshuZhang 1:60ad9c191ea9 54 led2 =1;
QingshuZhang 0:aed6099bd419 55
QingshuZhang 0:aed6099bd419 56 while (1) {
QingshuZhang 0:aed6099bd419 57 t1.start();
QingshuZhang 1:60ad9c191ea9 58 distance = 0;
QingshuZhang 1:60ad9c191ea9 59 distance2 = 0;
QingshuZhang 0:aed6099bd419 60 test = 0;
QingshuZhang 0:aed6099bd419 61 trigpin = 0; // low trig signal for 2us
QingshuZhang 0:aed6099bd419 62 trigpin2 = 0;
QingshuZhang 0:aed6099bd419 63 wait_us(2);
QingshuZhang 0:aed6099bd419 64 trigpin = 1;
QingshuZhang 0:aed6099bd419 65 trigpin2= 1; // high trig signal for 10us
QingshuZhang 0:aed6099bd419 66 wait_us(10);
QingshuZhang 0:aed6099bd419 67 trigpin = 0; // stay low trig level
QingshuZhang 0:aed6099bd419 68 trigpin2 = 0;
QingshuZhang 1:60ad9c191ea9 69 /* while (!echopin); // read the duration time
QingshuZhang 0:aed6099bd419 70 t.start();
QingshuZhang 0:aed6099bd419 71 while (echopin);
QingshuZhang 0:aed6099bd419 72 t.stop();
QingshuZhang 0:aed6099bd419 73 duration = t.read_us();
QingshuZhang 0:aed6099bd419 74 distance = (duration/2) / 29.1; //Measure the Digital Signal high level
QingshuZhang 0:aed6099bd419 75 while (!echopin2); // read the duration time of second sensor
QingshuZhang 0:aed6099bd419 76 t2.start();
QingshuZhang 0:aed6099bd419 77 while (echopin2);
QingshuZhang 0:aed6099bd419 78 t2.stop();
QingshuZhang 0:aed6099bd419 79 duration2 = t2.read_us();
QingshuZhang 0:aed6099bd419 80 distance2 = (duration2/2) / 29.1; //Measure the Digital Signal high level
QingshuZhang 0:aed6099bd419 81
QingshuZhang 0:aed6099bd419 82 t.reset ();
QingshuZhang 0:aed6099bd419 83 t2.rest();
QingshuZhang 0:aed6099bd419 84 printf("distance: %d\n",distance); //read the distance
QingshuZhang 1:60ad9c191ea9 85 printf("distance2: %d\n",distance2); //read the distance */
QingshuZhang 1:60ad9c191ea9 86
QingshuZhang 1:60ad9c191ea9 87 distance = getDis(echopin);
QingshuZhang 1:60ad9c191ea9 88 distance2 = getDis(echopin2);
QingshuZhang 0:aed6099bd419 89
QingshuZhang 0:aed6099bd419 90
QingshuZhang 0:aed6099bd419 91 if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
QingshuZhang 0:aed6099bd419 92 led1 = 1;
QingshuZhang 0:aed6099bd419 93 led2 = 0;// When the Red condition is met, the Green LED should turn off
QingshuZhang 0:aed6099bd419 94 speed = 0;
QingshuZhang 0:aed6099bd419 95 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 96 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 97 if (distance>distance2){
QingshuZhang 0:aed6099bd419 98 speed = 0.2;
QingshuZhang 0:aed6099bd419 99 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 100 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 101 dirR=1;
QingshuZhang 0:aed6099bd419 102 dirL=0;
QingshuZhang 1:60ad9c191ea9 103 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 104 controlRight.SetDirection(dirR); //obstacle on the right, turn left
QingshuZhang 0:aed6099bd419 105 }
QingshuZhang 0:aed6099bd419 106 else { //obstacle on the left or in the middle, turn right
QingshuZhang 0:aed6099bd419 107 speed = 0.2;
QingshuZhang 0:aed6099bd419 108 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 109 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 110 dirR=0;
QingshuZhang 0:aed6099bd419 111 dirL=1;
QingshuZhang 0:aed6099bd419 112 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 113 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 114 }
QingshuZhang 0:aed6099bd419 115 }
QingshuZhang 0:aed6099bd419 116 else {
QingshuZhang 0:aed6099bd419 117 led1 = 0;
QingshuZhang 0:aed6099bd419 118 led2 = 1;
QingshuZhang 0:aed6099bd419 119 speed = 0.2;
QingshuZhang 0:aed6099bd419 120 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 121 controlRight.SetSpeed(speed);
QingshuZhang 1:60ad9c191ea9 122 dirR=1;
QingshuZhang 1:60ad9c191ea9 123 dirL=1;
QingshuZhang 0:aed6099bd419 124 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 125 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 126 }
QingshuZhang 0:aed6099bd419 127
QingshuZhang 0:aed6099bd419 128
QingshuZhang 0:aed6099bd419 129 wait_us(10);
QingshuZhang 0:aed6099bd419 130 t1.stop();
QingshuZhang 0:aed6099bd419 131 totalT= t1.read_us();
QingshuZhang 0:aed6099bd419 132 printf("totalTime: %d \n",totalT);
QingshuZhang 0:aed6099bd419 133 t1.reset();
QingshuZhang 0:aed6099bd419 134 }
QingshuZhang 0:aed6099bd419 135 }
QingshuZhang 0:aed6099bd419 136
QingshuZhang 0:aed6099bd419 137
QingshuZhang 0:aed6099bd419 138
QingshuZhang 0:aed6099bd419 139