Diff: navigation.cpp

Revision:

2:17bd430aeca1

Parent:

1:a53d97b74fab

Child:

3:27cc2bacd6af

--- a/navigation.cpp	Sun Nov 22 19:32:45 2015 +0000
+++ b/navigation.cpp	Wed Jan 27 20:48:42 2016 +0000
@@ -1,6 +1,7 @@
-#include "StepperMotor.h"
+#include "StepperDrive.h"
 #include "navigation.h"
 #include "stdint.h"
+#include <cmath>
 #include <fstream>
 #include <stack>
 #include <set>
@@ -29,8 +30,8 @@
 //      None
 // DESCRIPTION:
 //      Adds a graph node to the graph.
-void Navigation::addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
-                                 uint16_t angle)
+void Navigation::addGraphNode(uint16_t src, uint16_t target, float dist,
+                                 float angle)
 {
     graph[src].push_back(graphNode(target, dist, angle));
 }
@@ -76,8 +77,8 @@
 
     uint16_t src;
     uint16_t target;
-    uint16_t dist;
-    uint16_t angle;
+    float dist;
+    float angle;
 
     while (!inFile.eof() )
     {
@@ -113,12 +114,12 @@
     minDistance[src] = 0;
     previous.clear();
     previous.resize(n, -1);
-    std::set<std::pair<uint16_t, uint16_t> > vertex_queue;
+    std::set<std::pair<float, uint16_t> > vertex_queue;
     vertex_queue.insert(std::make_pair(minDistance[src], src));
 
     while (!vertex_queue.empty())
     {
-        uint16_t dist = vertex_queue.begin()->first;
+        float dist = vertex_queue.begin()->first;
         uint16_t u = vertex_queue.begin()->second;
         vertex_queue.erase(vertex_queue.begin());
 
@@ -129,8 +130,8 @@
              neighbor_iter++)
         {
             v = neighbor_iter->neighbor;
-            uint16_t weight = neighbor_iter->distance;
-            uint16_t distance_through_u = dist + weight;
+            float weight = neighbor_iter->distance;
+            float distance_through_u = dist + weight;
 
             if (distance_through_u < minDistance[v])
             {
@@ -161,18 +162,74 @@
 //      None
 // DESCRIPTION:
 //      Sends command to steppers to execute route.
-void Navigation::executeRoute(StepperMotor &leftMotor, StepperMotor &rightMotor)
+void Navigation::executeRoute(Serial &pc, StepperDrive &drive)
 {
     uint16_t target;
-    uint16_t targetAngle;
-    uint16_t targetDist;
+    float targetAngle;
+    float targetDist;
+    float differenceAngle;
     
     while (!route.empty() )
     {
         target = route.top();
         route.pop();
         
-        for (int i = 0; graph[vertex][i].neighbor == target; i++)
+        for (int i = 0; i < graph[vertex].size(); i++)
+        {
+            if (graph[vertex][i].neighbor == target)
+            {
+                targetAngle = graph[vertex][i].angle;
+                targetDist = graph[vertex][i].distance;
+                break;
+            }
+        }
+        
+        if (targetAngle != angle)
+        {
+            differenceAngle = targetAngle - angle;
+            
+            if (abs(differenceAngle) > 180)
+            {
+                if (differenceAngle < 0) // 
+                    differenceAngle = 360 - abs(differenceAngle);
+                else
+                    differenceAngle = -(360 - differenceAngle);
+            }
+            
+            else
+            {
+                /* do nothing */
+            }
+int returnVal = 0;              
+            /* send -(differenceAngle) to motors */
+            returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) );
+            pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle);
+pc.printf("\n   ReturnVal = %i\n", returnVal);           
+            // wait for move to complete
+            while(!drive.isMoveDone())
+            {
+pc.printf("\n   Value of isMoveDone = %i", drive.isMoveDone());
+pc.printf("\n   leftSteps - %i", drive.getLeftSteps() );
+pc.printf("\n   rightSteps - %i", drive.getRightSteps() );
+                wait(1);
+            }
+        }
+int returnVal = 0;       
+        /* send targetDist to motors */
+        returnVal = drive.move(targetDist, 0);
+        pc.printf("\nMoved forward a distance of %f to node %i", targetDist, target);
+pc.printf("\n   ReturnVal = %i\n", returnVal);
+        // wait for move to complete
+        while(!drive.isMoveDone())
+        {
+pc.printf("\n   Value of isMoveDone = %i", drive.isMoveDone());
+pc.printf("\n   leftSteps - %i", drive.getLeftSteps() );
+pc.printf("\n   rightSteps - %i", drive.getRightSteps() );
+            wait(1);
+        }
+pc.printf("\n   Made it past the wait for move to complete");
+        vertex = target;
+        angle = targetAngle;
     }
 }
 
@@ -181,7 +238,7 @@
 {
     for(int i = 0; i < previous.size(); i++)
     {
-        pc.printf("Node %i is prev to node %i\n", previous[i], i);
+        pc.printf("Node %f is prev to node %f\n", previous[i], i);
     }
 }
 
@@ -201,6 +258,6 @@
     for (int i = 0; i < graph.size(); i++)
         for (int j = 0; j < graph[i].size(); j++)
         {
-            pc.printf("Node %i to %i = dist - %i, angle - %i\n", i, j, graph[i][j].distance, graph[i][j].angle);
+            pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle);
         }
 }