Diff: main.cpp

Revision:

0:32705e4af4b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 19 12:38:17 2019 +0000
@@ -0,0 +1,112 @@
+#include <L298_H.h>
+#include <mbed.h>
+Ticker tic;
+Timer t1;      //Timer used to record the time duration
+DigitalOut trigpin(E2);
+DigitalIn echopin(E3);
+DigitalOut trigpin2(E4);
+DigitalIn echopin2(E5);
+bool dirL;
+bool dirR;
+float speed;
+bool echo_flag;
+
+int getDis (DigitalIn echopin) {
+Timer t;
+int distance;
+float duration;
+while (!echopin);         // read the duration time
+    t.start();
+    while (echopin);
+    t.stop();
+    duration = t.read_us(); 
+    distance = (duration/2) / 29.1;  
+    t.reset();
+    //printf("distance: %d\n",distance); //read the distance
+    return distance;
+}
+
+void inter_r(){
+    echo_flag=1;
+    }
+
+int main() {
+L298 controlLeft(PTA12,PTA4,PTA5);
+L298 controlRight(PTA13,PTD5,PTD0);
+dirR=1;
+dirL=1;
+speed=0.2;
+int distance;
+int distance2;
+int totalT;
+controlLeft.SetDirection(dirL);
+controlRight.SetDirection(dirR);
+controlLeft.SetSpeed(speed);
+controlRight.SetSpeed(speed);
+tic.attach(&inter_r,1.0);// call the function every 1 second
+  while (1) {
+    if(echo_flag == true){
+    __disable_irq(); 
+    t1.start();   //t1 used to indicate the total time of this code
+    //Trig sensor 1 
+    distance = 0;
+    trigpin = 0; 
+    wait_us(2);
+    trigpin = 1;
+    wait_us(10);
+    trigpin = 0; 
+    distance = getDis(echopin);
+    //Trig sensor 2   
+    distance2=0;
+    trigpin2 = 0;
+    wait_us(2);
+    trigpin2 = 1;
+    wait_us(10);
+    trigpin2 = 0;
+    distance2 = getDis(echopin2);
+    
+    __enable_irq(); 
+        
+    if (distance < 20|| distance2 <20) { 
+      speed = 0;
+      controlLeft.SetSpeed(speed);
+      controlRight.SetSpeed(speed);
+      if (distance>distance2){
+          dirR=1;
+          dirL=0;
+          controlLeft.SetDirection(dirL);
+          controlRight.SetDirection(dirR);   //obstacle on the right, turn left
+          speed = 0.2;
+          controlLeft.SetSpeed(speed);
+          controlRight.SetSpeed(speed);
+          }
+      else {                 //obstacle on the left or in the middle, turn right
+          dirR=0;
+          dirL=1;          
+          controlLeft.SetDirection(dirL);
+          controlRight.SetDirection(dirR);
+          speed = 0.2;
+          controlLeft.SetSpeed(speed);
+          controlRight.SetSpeed(speed);
+          }
+    }
+    else {
+      dirR=1;
+      dirL=1;
+      controlLeft.SetDirection(dirL);
+      controlRight.SetDirection(dirR);
+      speed = 0.2;
+      controlLeft.SetSpeed(speed);
+      controlRight.SetSpeed(speed);
+    }
+    t1.stop();
+    totalT= t1.read_us();
+    //printf("totalTime: %d \n",totalT);
+    t1.reset();
+    echo_flag=0;
+    }
+}
+}   
+    
+
+