Qingshu Zhang / Ultrasonic_final

TDPS3

main.cpp@0:32705e4af4b9, 2019-03-19 (annotated)

Committer:
QingshuZhang
Date:
Tue Mar 19 12:38:17 2019 +0000
Revision:
0:32705e4af4b9
TDPS3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
QingshuZhang 0:32705e4af4b9 1 #include <L298_H.h>
QingshuZhang 0:32705e4af4b9 2 #include <mbed.h>
QingshuZhang 0:32705e4af4b9 3 Ticker tic;
QingshuZhang 0:32705e4af4b9 4 Timer t1; //Timer used to record the time duration
QingshuZhang 0:32705e4af4b9 5 DigitalOut trigpin(E2);
QingshuZhang 0:32705e4af4b9 6 DigitalIn echopin(E3);
QingshuZhang 0:32705e4af4b9 7 DigitalOut trigpin2(E4);
QingshuZhang 0:32705e4af4b9 8 DigitalIn echopin2(E5);
QingshuZhang 0:32705e4af4b9 9 bool dirL;
QingshuZhang 0:32705e4af4b9 10 bool dirR;
QingshuZhang 0:32705e4af4b9 11 float speed;
QingshuZhang 0:32705e4af4b9 12 bool echo_flag;
QingshuZhang 0:32705e4af4b9 13
QingshuZhang 0:32705e4af4b9 14 int getDis (DigitalIn echopin) {
QingshuZhang 0:32705e4af4b9 15 Timer t;
QingshuZhang 0:32705e4af4b9 16 int distance;
QingshuZhang 0:32705e4af4b9 17 float duration;
QingshuZhang 0:32705e4af4b9 18 while (!echopin); // read the duration time
QingshuZhang 0:32705e4af4b9 19 t.start();
QingshuZhang 0:32705e4af4b9 20 while (echopin);
QingshuZhang 0:32705e4af4b9 21 t.stop();
QingshuZhang 0:32705e4af4b9 22 duration = t.read_us();
QingshuZhang 0:32705e4af4b9 23 distance = (duration/2) / 29.1;
QingshuZhang 0:32705e4af4b9 24 t.reset();
QingshuZhang 0:32705e4af4b9 25 //printf("distance: %d\n",distance); //read the distance
QingshuZhang 0:32705e4af4b9 26 return distance;
QingshuZhang 0:32705e4af4b9 27 }
QingshuZhang 0:32705e4af4b9 28
QingshuZhang 0:32705e4af4b9 29 void inter_r(){
QingshuZhang 0:32705e4af4b9 30 echo_flag=1;
QingshuZhang 0:32705e4af4b9 31 }
QingshuZhang 0:32705e4af4b9 32
QingshuZhang 0:32705e4af4b9 33 int main() {
QingshuZhang 0:32705e4af4b9 34 L298 controlLeft(PTA12,PTA4,PTA5);
QingshuZhang 0:32705e4af4b9 35 L298 controlRight(PTA13,PTD5,PTD0);
QingshuZhang 0:32705e4af4b9 36 dirR=1;
QingshuZhang 0:32705e4af4b9 37 dirL=1;
QingshuZhang 0:32705e4af4b9 38 speed=0.2;
QingshuZhang 0:32705e4af4b9 39 int distance;
QingshuZhang 0:32705e4af4b9 40 int distance2;
QingshuZhang 0:32705e4af4b9 41 int totalT;
QingshuZhang 0:32705e4af4b9 42 controlLeft.SetDirection(dirL);
QingshuZhang 0:32705e4af4b9 43 controlRight.SetDirection(dirR);
QingshuZhang 0:32705e4af4b9 44 controlLeft.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 45 controlRight.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 46 tic.attach(&inter_r,1.0);// call the function every 1 second
QingshuZhang 0:32705e4af4b9 47 while (1) {
QingshuZhang 0:32705e4af4b9 48 if(echo_flag == true){
QingshuZhang 0:32705e4af4b9 49 __disable_irq();
QingshuZhang 0:32705e4af4b9 50 t1.start(); //t1 used to indicate the total time of this code
QingshuZhang 0:32705e4af4b9 51 //Trig sensor 1
QingshuZhang 0:32705e4af4b9 52 distance = 0;
QingshuZhang 0:32705e4af4b9 53 trigpin = 0;
QingshuZhang 0:32705e4af4b9 54 wait_us(2);
QingshuZhang 0:32705e4af4b9 55 trigpin = 1;
QingshuZhang 0:32705e4af4b9 56 wait_us(10);
QingshuZhang 0:32705e4af4b9 57 trigpin = 0;
QingshuZhang 0:32705e4af4b9 58 distance = getDis(echopin);
QingshuZhang 0:32705e4af4b9 59 //Trig sensor 2
QingshuZhang 0:32705e4af4b9 60 distance2=0;
QingshuZhang 0:32705e4af4b9 61 trigpin2 = 0;
QingshuZhang 0:32705e4af4b9 62 wait_us(2);
QingshuZhang 0:32705e4af4b9 63 trigpin2 = 1;
QingshuZhang 0:32705e4af4b9 64 wait_us(10);
QingshuZhang 0:32705e4af4b9 65 trigpin2 = 0;
QingshuZhang 0:32705e4af4b9 66 distance2 = getDis(echopin2);
QingshuZhang 0:32705e4af4b9 67
QingshuZhang 0:32705e4af4b9 68 __enable_irq();
QingshuZhang 0:32705e4af4b9 69
QingshuZhang 0:32705e4af4b9 70 if (distance < 20|| distance2 <20) {
QingshuZhang 0:32705e4af4b9 71 speed = 0;
QingshuZhang 0:32705e4af4b9 72 controlLeft.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 73 controlRight.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 74 if (distance>distance2){
QingshuZhang 0:32705e4af4b9 75 dirR=1;
QingshuZhang 0:32705e4af4b9 76 dirL=0;
QingshuZhang 0:32705e4af4b9 77 controlLeft.SetDirection(dirL);
QingshuZhang 0:32705e4af4b9 78 controlRight.SetDirection(dirR); //obstacle on the right, turn left
QingshuZhang 0:32705e4af4b9 79 speed = 0.2;
QingshuZhang 0:32705e4af4b9 80 controlLeft.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 81 controlRight.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 82 }
QingshuZhang 0:32705e4af4b9 83 else { //obstacle on the left or in the middle, turn right
QingshuZhang 0:32705e4af4b9 84 dirR=0;
QingshuZhang 0:32705e4af4b9 85 dirL=1;
QingshuZhang 0:32705e4af4b9 86 controlLeft.SetDirection(dirL);
QingshuZhang 0:32705e4af4b9 87 controlRight.SetDirection(dirR);
QingshuZhang 0:32705e4af4b9 88 speed = 0.2;
QingshuZhang 0:32705e4af4b9 89 controlLeft.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 90 controlRight.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 91 }
QingshuZhang 0:32705e4af4b9 92 }
QingshuZhang 0:32705e4af4b9 93 else {
QingshuZhang 0:32705e4af4b9 94 dirR=1;
QingshuZhang 0:32705e4af4b9 95 dirL=1;
QingshuZhang 0:32705e4af4b9 96 controlLeft.SetDirection(dirL);
QingshuZhang 0:32705e4af4b9 97 controlRight.SetDirection(dirR);
QingshuZhang 0:32705e4af4b9 98 speed = 0.2;
QingshuZhang 0:32705e4af4b9 99 controlLeft.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 100 controlRight.SetSpeed(speed);
QingshuZhang 0:32705e4af4b9 101 }
QingshuZhang 0:32705e4af4b9 102 t1.stop();
QingshuZhang 0:32705e4af4b9 103 totalT= t1.read_us();
QingshuZhang 0:32705e4af4b9 104 //printf("totalTime: %d \n",totalT);
QingshuZhang 0:32705e4af4b9 105 t1.reset();
QingshuZhang 0:32705e4af4b9 106 echo_flag=0;
QingshuZhang 0:32705e4af4b9 107 }
QingshuZhang 0:32705e4af4b9 108 }
QingshuZhang 0:32705e4af4b9 109 }
QingshuZhang 0:32705e4af4b9 110
QingshuZhang 0:32705e4af4b9 111
QingshuZhang 0:32705e4af4b9 112