Diff: main.cpp

Revision:

12:7598d38e7d44

Parent:

11:f95294698901

--- a/main.cpp	Sat Dec 08 19:59:14 2018 +0000
+++ b/main.cpp	Mon Dec 10 23:21:06 2018 +0000
@@ -8,8 +8,9 @@
 // Global Variables:
 bool tr = true;
 bool turnflag = false;
-int leftdistancestarting= 0;
-int leftdistanceending= 0;
+int leftdistancestarting = 0;
+int frontdistance = 0;
+int leftdistanceending = 0;
 //---------------------|
 // PIN INITIALIZATIONS |
 //---------------------|
@@ -127,18 +128,42 @@
     };
 } mctrl;
 
+void dist(int distance) {
+    frontdistance = distance;
+}
+void dist2(int distance)   // left sensor interrupt
+{
+    leftdistanceending = distance;
+}
+void dist3(int distance)  // right sensor interrupt
+{
+    /*if (distance < 150) {
+     pc.printf("Right Sensor trigg"); 
+    }
+    else {
+        pc.printf("Right Sensor echo");
+    }*/
+    //rightdistance = distance;
+}
 //------------------|
 // HELPER FUNCTIONS |
 //------------------|
+
+//------------------------------|
+// PIN INITIALIZATIONS (cont'd) |
+//------------------------------|
+// Setup Ultrasonic Sensor pins:
+ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
+ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
+ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
-void dist(int distance)  // NOTE: by default "distance" is in mm...
+void checkcollision()  // NOTE: by default "distance" is in mm...
 {
-    if (distance < 150) {
+    if (frontdistance < 150) {
         
         mctrl.stop();
 
         // Turn 90 degrees
-        float currt = t.read();
         if (tr) {
             mctrl.turnRight();
         } else {
@@ -147,13 +172,17 @@
         
         // Go forward X (1?) number of seconds.
         // change dstance???? globals??
-        currt = t.read();
-        while ((t.read() - currt) < 1.25 && distance > 150) {
+        float currt = t.read();
+        usensor.startUpdates();
+        wait(.5);
+        usensor.checkDistance();
+        while (((t.read() - currt) < .75) && frontdistance > 150) {
             mctrl.fwd();
+            usensor.startUpdates();
+            wait(.5);
+            usensor.checkDistance();
         }
-        
         // Turn 90 degrees
-        currt = t.read();
         if (tr) {
             mctrl.turnRight();
         } else {
@@ -167,37 +196,6 @@
     }
 }
 
-void dist2(int distance)   // left sensor interrupt
-{
-    
-     leftdistanceending = distance; 
-    //pc.printf(" Distance %d mm\r\n", distance);
-
-
-}
-
-void dist3(int distance)  // right sensor interrupt
-{
-    /*if (distance < 150) {
-     pc.printf("Right Sensor trigg"); 
-    }
-    else {
-        pc.printf("Right Sensor echo");
-    }*/
-    //rightdistance = distance;
-}
-
-//------------------------------|
-// PIN INITIALIZATIONS (cont'd) |
-//------------------------------|
-// Setup Ultrasonic Sensor pins:
-ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
-                                             // update every .07 secs w/ timeout after 1 sec...
-                                              // call "dist" when the distance changes...
-ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
-ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
-
-
 //---------------|
 // MAIN FUNCTION |
 //---------------|
@@ -227,7 +225,7 @@
    //while(1) {
         while (!sw) {
             usensor.checkDistance();
-            
+            checkcollision();            
             //determine which sensor is closest to wall
             int currLeftDist = usensor2.getCurrentDistance();
             int currRightDist = usensor3.getCurrentDistance();
@@ -276,19 +274,14 @@
                 } else if ((currRightDist - rightDist) > 5) {
                     mctrl.turnRightScaled(0.75);
                     mctrl.fwd();
-                } //else {
-//                    mctrl.fwd();
-//                }
+                } 
             }
             // may be weird when updating after turning? maybe wait/etc.
             leftdistancestarting = currLeftDist;
             rightDist = currRightDist;
+            wait(.03);
             }       
-            //usensor2.checkDistance();
-            //usensor3.checkDistance();
-            //pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
         }
         mctrl.stop();
-    //}
 }