Diff: navigation.cpp

Revision:

6:d2da4d4b5112

Parent:

5:d0954e0aecc9

Child:

7:f7489797746b

diff -r d0954e0aecc9 -r d2da4d4b5112 navigation.cpp
--- a/navigation.cpp	Mon Mar 21 01:32:00 2016 +0000
+++ b/navigation.cpp	Wed Apr 06 22:01:59 2016 +0000
@@ -1,6 +1,6 @@
 #include "StepperDrive.h"
+#include "LongRangeSensor.h"
 #include "navigation.h"
-#include "scanner.h"
 #include "stdint.h"
 #include <cmath>
 #include <fstream>
@@ -15,10 +15,14 @@
 //      None
 // DESCRIPTION:
 //      Default and 1-parameter constructor.
-Navigation::Navigation(Scanner& _scanner, int size = DEFAULT_SIZE) :
-    scanner(_scanner), vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE)
+Navigation::Navigation(Serial &_pc, StepperDrive &_drive, 
+    LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green,
+    int size = DEFAULT_SIZE) : pc(_pc), drive(_drive), 
+    longRangeL(_longRangeL), longRangeR(_longRangeR), led_red(_led_red), led_green(_led_green),
+    vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE)
 {
     graph.resize(size);
+    distLocalL = 0.0;
 }
 
 // FUNCTION:
@@ -31,7 +35,7 @@
 //      None
 // DESCRIPTION:
 //      Adds a graph node to the graph.
-void Navigation::addGraphNode(uint16_t src, uint16_t target, float dist,
+void Navigation::addGraphNode(uint8_t src, uint8_t target, float dist,
                                  float angle)
 {
     graph[src].push_back(graphNode(target, dist, angle));
@@ -47,7 +51,7 @@
 // DESCRIPTION:
 //      Adds a graph node to the graph, with target, dist, and angle
 //      initialized to infinity
-void Navigation::addGraphNode(uint16_t src)
+void Navigation::addGraphNode(uint8_t src)
 {
     graph[src].push_back(graphNode());
 }
@@ -76,8 +80,8 @@
     inFile >> vertices; // reads first entry as number of vertices
     graph.resize(vertices); // resizes vector as necessary
 
-    uint16_t src;
-    uint16_t target;
+    uint8_t src;
+    uint8_t target;
     float dist;
     float angle;
 
@@ -105,23 +109,23 @@
 //      and calculates shortest path information for all other vertices.
 //      Subroutine ends when shortest path for target vertex has been found
 //      and then loads the path into route.
-void Navigation::getShortestPath(uint16_t destination)
+void Navigation::getShortestPath(uint8_t destination)
 {
-    uint16_t src = vertex;
-    uint16_t v; // stores temporary vertex
+    uint8_t src = vertex;
+    uint8_t v; // stores temporary vertex
     int n = graph.size();
     minDistance.clear();
     minDistance.resize(n, MAX_DIST);
     minDistance[src] = 0;
     previous.clear();
     previous.resize(n, -1);
-    std::set<std::pair<float, uint16_t> > vertex_queue;
+    std::set<std::pair<float, uint8_t> > vertex_queue;
     vertex_queue.insert(std::make_pair(minDistance[src], src));
 
     while (!vertex_queue.empty())
     {
         float dist = vertex_queue.begin()->first;
-        uint16_t u = vertex_queue.begin()->second;
+        uint8_t u = vertex_queue.begin()->second;
         vertex_queue.erase(vertex_queue.begin());
 
         // Visit each edge exiting u
@@ -163,9 +167,9 @@
 //      None
 // DESCRIPTION:
 //      Sends command to steppers to execute route.
-void Navigation::executeRoute(Serial &pc, StepperDrive &drive)
+void Navigation::executeRoute()
 {
-    uint16_t target;
+    uint8_t target;
     float targetAngle;
     float targetDist;
     float differenceAngle;
@@ -191,7 +195,7 @@
 
             if (abs(differenceAngle) > 180)
             {
-                if (differenceAngle < 0) //
+                if (differenceAngle < 0)
                     differenceAngle = 360 - abs(differenceAngle);
                 else
                     differenceAngle = -(360 - differenceAngle);
@@ -211,6 +215,20 @@
             {
                 wait(1e-6);
             }
+            
+            distLocalL = longRangeL.distInchesLOne();
+            distLocalR = longRangeR.distInchesROne();
+            pc.printf("\n    distLocalL = %f", distLocalL);
+            pc.printf("\n    distLocalR = %f", distLocalR);
+            if ((distLocalL <= 8.0) && (distLocalL >= 3.0)) // wall in range
+                {
+                    localizeRight();
+                }
+                
+            else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
+                {
+                    localizeLeft();
+                }
         }
 
         /* send targetDist to motors */
@@ -225,6 +243,20 @@
 
         vertex = target;
         angle = targetAngle;
+        
+        distLocalL = longRangeL.distInchesLOne();
+        distLocalR = longRangeR.distInchesROne();
+        pc.printf("\n    distLocalL = %f", distLocalL);
+        pc.printf("\n    distLocalR = %f", distLocalR);
+        if ((distLocalL <= 8.5) && (distLocalL >= 3.0)) // wall in range
+            {
+                localizeRight();
+            }
+            
+        else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range
+            {
+                localizeLeft();
+            }
     }
 }
 
@@ -256,3 +288,165 @@
             pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle);
         }
 }
+
+// FUNCTION:
+//      void localizeRight()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeL this method will localize to the wall
+//      to the right
+void Navigation::localizeRight()
+{   
+    if (distLocalL >= 7.1) // too close left, turn right
+    {
+        drive.move(0, (2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle right, in localizeRight");
+    }
+    
+    else if (distLocalL <= 5.9) // too close right, turn left
+    {
+        drive.move(0, (-2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle left , in localizeRight");
+    }
+    
+    else
+    {
+        /* do nothing */
+        pc.printf("\n    No need to adjust, in localizeRight");
+    }
+}
+
+// FUNCTION:
+//      void localizeLeft()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeR this method will localize to the wall
+//      to the left during stretches of forward motion where robot 
+//      should remain xxx distance from the left wall. 
+void Navigation::localizeLeft()
+{   
+    if (distLocalR >= 9.7) // too close right, turn left
+    {
+        drive.move(0.0, (-2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle left, in localizeLeft");
+    }
+    
+    else if (distLocalR <= 8.3) // too close left, turn right
+    {
+        drive.move(0.0, (2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle right, in localizeLeft");
+    }
+    
+    else
+    {
+        /* do nothing */
+        pc.printf("\n    No need to adjust, in localizeLeft");
+    }
+}
+
+// FUNCTION:
+//      void localizeRightReverse()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeL this method will localize to the wall
+//      to the right
+void Navigation::localizeRightReverse()
+{   
+    if (distLocalL >= 7.2) // too close left, turn left
+    {
+        drive.move(0, (-2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle left, in localizeRightReverse");
+    }
+    
+    else if (distLocalL <= 6.2) // too close right, turn right
+    {
+        drive.move(0, (2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle right, in localizeRightReverse");
+    }
+    
+    else
+    {
+        /* do nothing */
+        pc.printf("\n    No need to adjust, in localizeRightReverse");
+    }
+}
+
+// FUNCTION:
+//      void localizeLeftReverse()
+// IN-PARAMETERS:
+//      None
+// OUT-PARAMETERS:
+//      None
+// DESCRIPTION:
+//      Using sensor longRangeR this method will localize to the wall
+//      to the left during stretches of forward motion where robot 
+//      should remain xxx distance from the left wall. 
+void Navigation::localizeLeftReverse()
+{   
+    if (distLocalR >= 9.7) // toos close right, turn right
+    {
+        drive.move(0.0, (2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle right, in localizeLeftReverse");
+    }
+    
+    else if (distLocalR <= 8.3) // too close left, turn left
+    {
+        drive.move(0.0, (-2.0)*(3.14159 / 180.0));
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+            wait(1e-6);
+        }
+        pc.printf("\n    Adjusted angle left, in localizeLeftReverse");
+    }
+    
+    else
+    {
+        /* do nothing */
+        pc.printf("\n    No need to adjust, in localizeLeftReverse");
+    }
+}