Diff: main.cpp

Revision:

9:ec0ceec8f5f5

Parent:

8:2d0fc244cc65

Child:

10:2a1c8ce9d76c

--- a/main.cpp	Wed Nov 28 18:28:04 2018 +0000
+++ b/main.cpp	Mon Dec 03 17:01:09 2018 +0000
@@ -8,8 +8,8 @@
 // Global Variables:
 bool tr = true;
 bool turnflag = false;
-int leftdistance= 0;
-int rightdistance = 0;
+int leftdistancestarting= 0;
+int leftdistanceending= 0;
 //---------------------|
 // PIN INITIALIZATIONS |
 //---------------------|
@@ -85,6 +85,10 @@
         stop();
         //wait(0.02);
     };
+    
+    
+            
+        
 } mctrl;
 
 //------------------|
@@ -93,37 +97,40 @@
 void dist(int distance)  // NOTE: by default "distance" is in mm...
 {
     if (distance < 150) {
+        
+        mctrl.stop();
     // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
     // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
     // 2) Go forward 1.5 seconds.
     // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
     // 4) Continue forward until wall.
+    
+            
         // [Step 1]
-        float currt = t.read();
+        /*float currt = t.read();
         if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
         // [Step 2]
         currt = t.read();
-        while ((t.read()-currt) < .5) {mctrl.fwd();}
+        while ((t.read()-currt) < 1) {mctrl.fwd();}
         // [Step 3]
         currt = t.read();
         if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
         // [End]
-        tr = !tr;
+        tr = !tr;*/
     }
     else {
         mctrl.fwd();
+        
     }
 }
 
 void dist2(int distance)   // left sensor interrupt
 {
-    /*if (distance < 150) {
-     pc.printf("Left Sensor trigg"); 
-    }
-    else {
-        pc.printf("Left Sensor echo");
-    }*/
-    leftdistance = distance; 
+    
+     leftdistanceending = distance; 
+    //pc.printf(" Distance %d mm\r\n", distance);
+
+
 }
 
 void dist3(int distance)  // right sensor interrupt
@@ -134,7 +141,7 @@
     else {
         pc.printf("Right Sensor echo");
     }*/
-    rightdistance = distance;
+    //rightdistance = distance;
 }
 
 
@@ -145,8 +152,8 @@
 ultrasonic usensor(p13, p14, .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(p13, p26, .07, 1, &dist2);  // left trigger
-ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right trigger
+ultrasonic usensor2(p13, p26, .07, 1, &dist2);  // left sensor trigger
+ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right sensor trigger
 
 //---------------|
 // MAIN FUNCTION |
@@ -158,16 +165,26 @@
     usensor.startUpdates();
     usensor2.startUpdates();
     usensor3.startUpdates(); 
+    wait(0.5);
+    int leftdistancestarting = usensor2.getCurrentDistance();
+    wait(0.5);
+    pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting);
+
     // Initialize the Timer:
     t.start();
-    while(!sw) {
+    
+   while(1) {
+        while (!sw)
+        {
     
         usensor.checkDistance();
         usensor2.checkDistance();
         usensor3.checkDistance();
-        pc.printf("Left Distance %d mm\r\n", leftdistance);
-        pc.printf("Right Distance %d mm\r\n", rightdistance);
-        wait(1);
+        pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
+
+        }
+        mctrl.stop();
+
     }
 }