Diff: main.cpp

Revision:

0:aed6099bd419

Child:

1:60ad9c191ea9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 14 09:28:30 2019 +0000
@@ -0,0 +1,118 @@
+
+#include <RobotControl_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 test(PTE30);
+DigitalOut led1(PTD2);
+DigitalOut led2(PTD3);  //two LED indicates if there's any obstacles nearby
+//initialise the class twice, one controls the left part and the other the right
+bool dirL;
+bool dirR;
+float speed;
+
+int main() {
+RobotControl controlLeft(PTA12,PTA4,PTA5);
+RobotControl controlRight(PTA13,PTD5,PTD0);
+dirR=0;
+dirL=0;
+speed=0.2;
+controlLeft.SetDirection(dirL);
+controlRight.SetDirection(dirR);
+controlLeft.SetSpeed(speed);
+controlRight.SetSpeed(speed);
+
+  while (1) {
+    t1.start();
+    int totalT;
+    float duration;
+    float duration2;
+    int distance = 0;
+    int distance2 = 0;
+    test = 0;
+    trigpin = 0; // low trig signal for 2us
+    trigpin2 = 0;
+    wait_us(2);
+    trigpin = 1;
+    trigpin2= 1; // high trig signal for 10us
+    wait_us(10);
+    trigpin = 0; // stay low trig level
+    trigpin2 = 0;
+    while (!echopin);         // read the duration time
+    t.start();
+    while (echopin);
+    t.stop();
+    duration = t.read_us(); 
+    distance = (duration/2) / 29.1;    //Measure the Digital Signal high level
+    while (!echopin2);         // read the duration time of second sensor
+    t2.start();
+    while (echopin2);
+    t2.stop();
+    duration2 = t2.read_us(); 
+    distance2 = (duration2/2) / 29.1;    //Measure the Digital Signal high level
+ 
+    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;
+   
+        
+
+    if (distance < 20|| distance2 <20) { // This is where the LED On/Off happens
+      led1 = 1;
+      led2 = 0;// When the Red condition is met, the Green LED should turn off
+      speed = 0;
+      controlLeft.SetSpeed(speed);
+      controlRight.SetSpeed(speed);
+      if (distance>distance2){
+          speed = 0.2;
+          controlLeft.SetSpeed(speed);
+          controlRight.SetSpeed(speed);
+          dirR=1;
+          dirL=0;
+          controlLeft.SetDirection(dirl);
+          controlRight.SetDirection(dirR);   //obstacle on the right, turn left
+          }
+      else {                 //obstacle on the left or in the middle, turn right
+          speed = 0.2;
+          controlLeft.SetSpeed(speed);
+          controlRight.SetSpeed(speed);
+          dirR=0;
+          dirL=1;          
+          controlLeft.SetDirection(dirL);
+          controlRight.SetDirection(dirR); 
+          }
+    }
+    else {
+      led1 = 0;
+      led2 = 1;
+      speed = 0.2;
+      controlLeft.SetSpeed(speed);
+      controlRight.SetSpeed(speed);
+      dirR=0;
+      dirL=0;
+      controlLeft.SetDirection(dirL);
+      controlRight.SetDirection(dirR);
+    }
+    
+    
+    wait_us(10);
+    t1.stop();
+    totalT= t1.read_us();
+    printf("totalTime: %d \n",totalT);
+    t1.reset();
+             }
+            }
+   
+    
+
+