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