Diff: main.cpp

Revision:

1:60ad9c191ea9

Parent:

0:aed6099bd419

--- a/main.cpp	Thu Mar 14 09:28:30 2019 +0000
+++ b/main.cpp	Thu Mar 14 14:37:20 2019 +0000
@@ -1,14 +1,14 @@
 
-#include <RobotControl_H.h>
+#include <L298_H.h>
 #include <mbed.h>
 
-Timer t; 
+ 
 Timer t1;      //Timer used to record the time duration
-Timer t2;
+
 DigitalOut trigpin(PTD4);
 DigitalIn echopin(PTE20);
-DigitalOut trigpin2();
-DigitalIn echopin2();
+DigitalOut trigpin2(PTD1);
+DigitalIn echopin2(PTC9);
 DigitalOut test(PTE30);
 DigitalOut led1(PTD2);
 DigitalOut led2(PTD3);  //two LED indicates if there's any obstacles nearby
@@ -17,24 +17,46 @@
 bool dirR;
 float speed;
 
+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
+    test = 1;
+    wait_ms(100);
+    test=0;
+    return distance;
+}
+
+
 int main() {
-RobotControl controlLeft(PTA12,PTA4,PTA5);
-RobotControl controlRight(PTA13,PTD5,PTD0);
-dirR=0;
-dirL=0;
+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);
+test =1;
+led1=1;
+led2 =1;
 
   while (1) {
     t1.start();
-    int totalT;
-    float duration;
-    float duration2;
-    int distance = 0;
-    int distance2 = 0;
+    distance = 0;
+    distance2 = 0;
     test = 0;
     trigpin = 0; // low trig signal for 2us
     trigpin2 = 0;
@@ -44,7 +66,7 @@
     wait_us(10);
     trigpin = 0; // stay low trig level
     trigpin2 = 0;
-    while (!echopin);         // read the duration time
+   /* while (!echopin);         // read the duration time
     t.start();
     while (echopin);
     t.stop();
@@ -60,11 +82,10 @@
     t.reset ();
     t2.rest();
     printf("distance: %d\n",distance); //read the distance
-    printf("distance2: %d\n",distance2); //read the distance
-    test = 1;
-    wait_ms(100);
-    test=0;
-   
+    printf("distance2: %d\n",distance2); //read the distance */
+    
+    distance = getDis(echopin);
+    distance2 = getDis(echopin2);
         
 
     if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
@@ -79,7 +100,7 @@
           controlRight.SetSpeed(speed);
           dirR=1;
           dirL=0;
-          controlLeft.SetDirection(dirl);
+          controlLeft.SetDirection(dirL);
           controlRight.SetDirection(dirR);   //obstacle on the right, turn left
           }
       else {                 //obstacle on the left or in the middle, turn right
@@ -98,8 +119,8 @@
       speed = 0.2;
       controlLeft.SetSpeed(speed);
       controlRight.SetSpeed(speed);
-      dirR=0;
-      dirL=0;
+      dirR=1;
+      dirL=1;
       controlLeft.SetDirection(dirL);
       controlRight.SetDirection(dirR);
     }