Qingshu Zhang / Mbed 2 deprecated new_HC-SR504

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed HC_SR04_Ultrasonic_Library

main.cpp@0:aed6099bd419, 2019-03-14 (annotated)

Committer:
QingshuZhang
Date:
Thu Mar 14 09:28:30 2019 +0000
Revision:
0:aed6099bd419
Child:
1:60ad9c191ea9
TDPS3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
QingshuZhang 0:aed6099bd419 1
QingshuZhang 0:aed6099bd419 2 #include <RobotControl_H.h>
QingshuZhang 0:aed6099bd419 3 #include <mbed.h>
QingshuZhang 0:aed6099bd419 4
QingshuZhang 0:aed6099bd419 5 Timer t;
QingshuZhang 0:aed6099bd419 6 Timer t1; //Timer used to record the time duration
QingshuZhang 0:aed6099bd419 7 Timer t2;
QingshuZhang 0:aed6099bd419 8 DigitalOut trigpin(PTD4);
QingshuZhang 0:aed6099bd419 9 DigitalIn echopin(PTE20);
QingshuZhang 0:aed6099bd419 10 DigitalOut trigpin2();
QingshuZhang 0:aed6099bd419 11 DigitalIn echopin2();
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 0:aed6099bd419 20 int main() {
QingshuZhang 0:aed6099bd419 21 RobotControl controlLeft(PTA12,PTA4,PTA5);
QingshuZhang 0:aed6099bd419 22 RobotControl controlRight(PTA13,PTD5,PTD0);
QingshuZhang 0:aed6099bd419 23 dirR=0;
QingshuZhang 0:aed6099bd419 24 dirL=0;
QingshuZhang 0:aed6099bd419 25 speed=0.2;
QingshuZhang 0:aed6099bd419 26 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 27 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 28 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 29 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 30
QingshuZhang 0:aed6099bd419 31 while (1) {
QingshuZhang 0:aed6099bd419 32 t1.start();
QingshuZhang 0:aed6099bd419 33 int totalT;
QingshuZhang 0:aed6099bd419 34 float duration;
QingshuZhang 0:aed6099bd419 35 float duration2;
QingshuZhang 0:aed6099bd419 36 int distance = 0;
QingshuZhang 0:aed6099bd419 37 int distance2 = 0;
QingshuZhang 0:aed6099bd419 38 test = 0;
QingshuZhang 0:aed6099bd419 39 trigpin = 0; // low trig signal for 2us
QingshuZhang 0:aed6099bd419 40 trigpin2 = 0;
QingshuZhang 0:aed6099bd419 41 wait_us(2);
QingshuZhang 0:aed6099bd419 42 trigpin = 1;
QingshuZhang 0:aed6099bd419 43 trigpin2= 1; // high trig signal for 10us
QingshuZhang 0:aed6099bd419 44 wait_us(10);
QingshuZhang 0:aed6099bd419 45 trigpin = 0; // stay low trig level
QingshuZhang 0:aed6099bd419 46 trigpin2 = 0;
QingshuZhang 0:aed6099bd419 47 while (!echopin); // read the duration time
QingshuZhang 0:aed6099bd419 48 t.start();
QingshuZhang 0:aed6099bd419 49 while (echopin);
QingshuZhang 0:aed6099bd419 50 t.stop();
QingshuZhang 0:aed6099bd419 51 duration = t.read_us();
QingshuZhang 0:aed6099bd419 52 distance = (duration/2) / 29.1; //Measure the Digital Signal high level
QingshuZhang 0:aed6099bd419 53 while (!echopin2); // read the duration time of second sensor
QingshuZhang 0:aed6099bd419 54 t2.start();
QingshuZhang 0:aed6099bd419 55 while (echopin2);
QingshuZhang 0:aed6099bd419 56 t2.stop();
QingshuZhang 0:aed6099bd419 57 duration2 = t2.read_us();
QingshuZhang 0:aed6099bd419 58 distance2 = (duration2/2) / 29.1; //Measure the Digital Signal high level
QingshuZhang 0:aed6099bd419 59
QingshuZhang 0:aed6099bd419 60 t.reset ();
QingshuZhang 0:aed6099bd419 61 t2.rest();
QingshuZhang 0:aed6099bd419 62 printf("distance: %d\n",distance); //read the distance
QingshuZhang 0:aed6099bd419 63 printf("distance2: %d\n",distance2); //read the distance
QingshuZhang 0:aed6099bd419 64 test = 1;
QingshuZhang 0:aed6099bd419 65 wait_ms(100);
QingshuZhang 0:aed6099bd419 66 test=0;
QingshuZhang 0:aed6099bd419 67
QingshuZhang 0:aed6099bd419 68
QingshuZhang 0:aed6099bd419 69
QingshuZhang 0:aed6099bd419 70 if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
QingshuZhang 0:aed6099bd419 71 led1 = 1;
QingshuZhang 0:aed6099bd419 72 led2 = 0;// When the Red condition is met, the Green LED should turn off
QingshuZhang 0:aed6099bd419 73 speed = 0;
QingshuZhang 0:aed6099bd419 74 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 75 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 76 if (distance>distance2){
QingshuZhang 0:aed6099bd419 77 speed = 0.2;
QingshuZhang 0:aed6099bd419 78 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 79 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 80 dirR=1;
QingshuZhang 0:aed6099bd419 81 dirL=0;
QingshuZhang 0:aed6099bd419 82 controlLeft.SetDirection(dirl);
QingshuZhang 0:aed6099bd419 83 controlRight.SetDirection(dirR); //obstacle on the right, turn left
QingshuZhang 0:aed6099bd419 84 }
QingshuZhang 0:aed6099bd419 85 else { //obstacle on the left or in the middle, turn right
QingshuZhang 0:aed6099bd419 86 speed = 0.2;
QingshuZhang 0:aed6099bd419 87 controlLeft.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 88 controlRight.SetSpeed(speed);
QingshuZhang 0:aed6099bd419 89 dirR=0;
QingshuZhang 0:aed6099bd419 90 dirL=1;
QingshuZhang 0:aed6099bd419 91 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 92 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 93 }
QingshuZhang 0:aed6099bd419 94 }
QingshuZhang 0:aed6099bd419 95 else {
QingshuZhang 0:aed6099bd419 96 led1 = 0;
QingshuZhang 0:aed6099bd419 97 led2 = 1;
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=0;
QingshuZhang 0:aed6099bd419 102 dirL=0;
QingshuZhang 0:aed6099bd419 103 controlLeft.SetDirection(dirL);
QingshuZhang 0:aed6099bd419 104 controlRight.SetDirection(dirR);
QingshuZhang 0:aed6099bd419 105 }
QingshuZhang 0:aed6099bd419 106
QingshuZhang 0:aed6099bd419 107
QingshuZhang 0:aed6099bd419 108 wait_us(10);
QingshuZhang 0:aed6099bd419 109 t1.stop();
QingshuZhang 0:aed6099bd419 110 totalT= t1.read_us();
QingshuZhang 0:aed6099bd419 111 printf("totalTime: %d \n",totalT);
QingshuZhang 0:aed6099bd419 112 t1.reset();
QingshuZhang 0:aed6099bd419 113 }
QingshuZhang 0:aed6099bd419 114 }
QingshuZhang 0:aed6099bd419 115
QingshuZhang 0:aed6099bd419 116
QingshuZhang 0:aed6099bd419 117
QingshuZhang 0:aed6099bd419 118