Diff: main.cpp

Revision:

13:dd2ac39ba5ba

Parent:

12:f57a51ec8399

diff -r f57a51ec8399 -r dd2ac39ba5ba main.cpp
--- a/main.cpp	Mon Dec 10 17:57:37 2018 +0000
+++ b/main.cpp	Wed Dec 12 16:13:08 2018 +0000
@@ -8,8 +8,13 @@
 // Global Variables:
 bool tr = true;
 bool turnflag = false;
-int leftdistancestarting= 0;
-int leftdistanceending= 0;
+ 
+int leftDist = 0;
+int rightDist = 0;
+int forwardDist = 0; 
+int leftForwardDist = 0;
+int rightForwardDist = 0;
+
 //---------------------|
 // PIN INITIALIZATIONS |
 //---------------------|
@@ -58,7 +63,7 @@
         Lback.speed(-1);
         Rfront.speed(-1);
         Rback.speed(-1);
-        wait(0.02);
+        wait(0.25);
     };
     
     // Turn left 90 degrees:
@@ -70,14 +75,15 @@
             while(imu.gyroAvailable());
             imu.readGyro();
             angularV = imu.gz;
-            Lfront.speed(-1);
-            Lback.speed(-1);
-            Rfront.speed(1);
-            Rback.speed(1);
-            if(angularV > 50.0 || angularV <-50.0) {
+            Lfront.speed(1);
+            Lback.speed(1);
+            Rfront.speed(-1);
+            Rback.speed(-1);
+            if(angularV > 50.0 || angularV <-50.0) 
+            {
                 degree += (abs(angularV))/100.00;
             }
-            wait(.45);
+            wait(.4);
         }
         stop();
         wait(0.02);
@@ -99,7 +105,7 @@
             if(angularV > 50.0 || angularV <-50.0) {
                 degree += (abs(angularV))/100.00;
             }
-            wait(.45);
+            wait(.4);
         }
         stop();
         wait(0.02);
@@ -135,60 +141,85 @@
 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
 void dist(int distance)  // NOTE: by default "distance" is in mm...
 {
-    if (distance < 150) {
+    if (distance < 200) {
         
         mctrl.stop();
-
-        // Turn 90 degrees
-        float currt = t.read();
-        if (tr) {
+        
+        if ( leftDist < rightDist)
+        {
             mctrl.turnRight();
-        } else {
-            mctrl.turnLeft();
+            mctrl.stop();
+            //mctrl.fwd(); 
+            
         }
         
-        // Go forward 1.25 seconds... is it though???
-        // change dstance???? globals?? to make sure it doesnt hit wall later on
-        currt = t.read();
-        while ((t.read() - currt) < 1.25) {
-            mctrl.fwd();
+        else 
+        {
+            mctrl.turnLeft(); 
+            mctrl.stop();
+            //mctrl.fwd();
         }
-        
-        // Turn 90 degrees
-        currt = t.read();
-        if (tr) {
-            mctrl.turnRight();
-        } else {
-            mctrl.turnLeft();
-        }
+    }
+    
+    else {
+        mctrl.fwd();
+    }
+};
+
+void dist2(int distance)   // left sensor interrupt
+{
+    if (distance < 150)
+    {
+        mctrl.stop();
+        mctrl.turnRight();
+        mctrl.stop();
+    }
+    
+    else
+    {
+
+        mctrl.fwd(); 
+    }
+
+}
 
-        tr = !tr;
-    } else {
-        // Continue forward until wall.
-        mctrl.fwd();  
+void dist3(int distance)  // right sensor interrupt
+{
+    if (distance < 150)
+    {
+        mctrl.stop();
+        mctrl.turnLeft();
+        mctrl.stop();
+    }
+    
+    else
+    {
+        mctrl.fwd(); 
+    }
+
+}
+
+void dist4(int distance) {
+    if (distance < 130) {
+        mctrl.stop();
+        mctrl.turnRight();
+        mctrl.stop();
+    } 
+    
+    else {
+        mctrl.fwd();
     }
 }
 
-// nonsense so far
-void dist2(int distance)   // left sensor interrupt
-{
-    
-     leftdistanceending = distance; 
-    //pc.printf(" Distance %d mm\r\n", distance);
-
-
-}
-
-// nonsense so far
-void dist3(int distance)  // right sensor interrupt
-{
-    /*if (distance < 150) {
-     pc.printf("Right Sensor trigg"); 
+void dist5(int distance) {
+    if (distance < 130) {
+        mctrl.stop();
+        mctrl.turnLeft();
+        mctrl.stop();
+    } else {
+        
+        mctrl.fwd();
     }
-    else {
-        pc.printf("Right Sensor echo");
-    }*/
-    //rightdistance = distance;
 }
 
 //------------------------------|
@@ -201,6 +232,10 @@
 ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
 
+// new sonars added
+ultrasonic usensor4(p15, p29, .07, 1, &dist4); //sonar to the left of the forward sensor
+ultrasonic usensor5(p15, p30, .07, 1, &dist5); //sonar to the right of the forward sensor
+
 
 //---------------|
 // MAIN FUNCTION |
@@ -213,87 +248,70 @@
     usensor.startUpdates();
     usensor2.startUpdates();
     usensor3.startUpdates(); 
+    usensor4.startUpdates();
+    usensor5.startUpdates();
     wait(0.5);
     
     // obtain and print starting distances
-    int forwardDist = usensor.getCurrentDistance();
-    int leftDist = usensor2.getCurrentDistance();
-    int rightDist = usensor3.getCurrentDistance();
+    forwardDist = usensor.getCurrentDistance();
+    leftDist = usensor2.getCurrentDistance();
+    rightDist = usensor3.getCurrentDistance();
+    leftForwardDist = usensor4.getCurrentDistance();
+    rightForwardDist = usensor5.getCurrentDistance();
     wait(0.5);
-    
-    pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
-    pc.printf("Current Left Starting Distance %d mm\r\n", leftDist);
-    pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);
 
     // Initialize the Timer:
     t.start();
+    int starttime = t.read();
     
-   //while(1) {
-        while (!sw) {
-            // if front sonar detects a change in the distace measured, dist is called
-            usensor.checkDistance();
+   while(1) {
+        while (!sw && t.read() - starttime < 60) {
             
-            // obtain the distance measurements for the left and right sonars
-            int currLeftDist = usensor2.getCurrentDistance();
-            int currRightDist = usensor3.getCurrentDistance();
+            
             
-            // if the left and right sonars detect distances greater than 100mm
-            // (10 cm or 3.93 inches), just go forward aka ignore path corr.
-            if (currLeftDist > 100 && currRightDist > 100) {
-                mctrl.fwd();
-            } else {
-                // if left side of robot is nearer to the wall...
-                if (currLeftDist < currRightDist) {
-                    //if robot has drifted toward the wall by a lot, scale R turn proportionally
-                    if ((leftDist - currLeftDist) > 40) {
-                        // take a right turn with a greater speed
-                        // (trying to emulate a "big" turn, may need to be changed)
-                        mctrl.turnRightScaled(1);
-                        // this is an attempt to go forward (may need to change)
-                        mctrl.fwd();
-                    //if drifted toward wall a little (less than 40 but greater than 5 mm), scale accordingly
-                    } else if ((leftDist - currLeftDist) > 5) {
-                        mctrl.turnRightScaled(0.75);
-                        mctrl.fwd();
-                    //if drift away from wall a lot, scale L turn accordingly
-                    } else if ((currLeftDist - leftDist) > 40) {
-                        mctrl.turnLeftScaled(1);
-                        mctrl.fwd();
-                    } else if ((currLeftDist - leftDist) > 5) {
-                        mctrl.turnLeftScaled(0.75);
-                        mctrl.fwd();
-                    // if drift if less than 5 mm in either direction (away or toward wall), go forward normally...
-                    // last meeting i had this commented out, see how the robot behaves this time!!
-                    } else {
-                        mctrl.fwd();
-                    }
-                } else if (currLeftDist > currRightDist) {
-                    //if robot has drifted toward the wall by a lot, scale L turn by a lot
-                    if ((rightDist - currRightDist) > 40) {
-                        mctrl.turnLeftScaled(1);
-                        mctrl.fwd();
-                    //if drifted toward wall a little, scale accordingly
-                    } else if ((rightDist - currRightDist) > 5) {
-                        mctrl.turnLeftScaled(0.75);
-                        mctrl.fwd();
-                    //if drift away a lot, scale R turn accordingly
-                    } else if ((currRightDist - rightDist) > 40) {
-                        mctrl.turnRightScaled(1);
-                        mctrl.fwd();
-                    } else if ((currRightDist - rightDist) > 5) {
-                        mctrl.turnRightScaled(0.75);
-                        mctrl.fwd();
-                    } else {
-                        mctrl.fwd();
-                    }
-                }
-                
-                // update the left and right distances that will the next values will be compared to. may be weird so maybe wait/etc.
-                leftDist = currLeftDist;
-                rightDist = currRightDist;
-            }       
+             usensor.checkDistance(); 
+             usensor2.checkDistance();
+             usensor3.checkDistance();
+             usensor4.checkDistance();
+             usensor5.checkDistance();
+             forwardDist = usensor.getCurrentDistance();
+             leftDist = usensor2.getCurrentDistance(); 
+             rightDist = usensor3.getCurrentDistance();
+             leftForwardDist = usensor4.getCurrentDistance();
+             rightForwardDist = usensor5.getCurrentDistance(); 
+             
+             if ( leftForwardDist > 100 && rightForwardDist > 100 ) 
+             {
+             mctrl.fwd(); 
+             }
+             
+             else{
+                 
+                 mctrl.rev();
+                 mctrl.turnRight();
+                 mctrl.turnRight();
+                 mctrl.fwd();
+                 
+            }
+             //rightForwardDist = usensor5.getCurrentDistance(); 
+             /*
+             pc.printf("fowardDist: %d\n ", usensor.getCurrentDistance());
+             wait (1);
+             pc.printf("LeftDist: %d\n", usensor2.getCurrentDistance());
+             wait(1);
+             pc.printf("RightDist: %d\n", usensor3.getCurrentDistance());
+             wait(1);
+             pc.printf("leftForwardDist: %d\n ", usensor4.getCurrentDistance());
+             wait (1);
+             pc.printf("rightForwardDist: %d\n", usensor5.getCurrentDistance());
+             wait(1);*/
+
+             
+    
+             
         }
         // if switch flipped, stop robot
         mctrl.stop();
+    }
+
 }
-