Diff: navigation.h

Revision:

6:d2da4d4b5112

Parent:

5:d0954e0aecc9

Child:

7:f7489797746b

--- a/navigation.h	Mon Mar 21 01:32:00 2016 +0000
+++ b/navigation.h	Wed Apr 06 22:01:59 2016 +0000
@@ -1,7 +1,7 @@
 #ifndef NAVIGATION_H
 #define NAVIGATION_H
 #include "StepperDrive.h"
-#include "scanner.h"
+#include "LongRangeSensor.h"
 #include "stdint.h"
 #include "mbed.h"
 #include <vector>
@@ -10,40 +10,53 @@
 class Navigation
 {
 public:
-    Navigation(Scanner& _scanner, int size);
-    void addGraphNode(uint16_t src, uint16_t target, float dist,
-                      float angle);
-    void addGraphNode(uint16_t src);
+    Navigation(Serial &_pc, StepperDrive &_drive, LongRangeSensor 
+        &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green, int size);
+    void addGraphNode(uint8_t src, uint8_t target, float dist,
+        float angle);
+    void addGraphNode(uint8_t src);
     int graphSize() { return graph.size(); }
-    uint16_t getVertex() { return vertex; }
+    uint8_t getVertex() { return vertex; }
     float getAngle() { return angle; }
-    void setVertex(uint16_t target) { vertex = target; }
+    void setVertex(uint8_t target) { vertex = target; }
     void setAngle(float target) { angle = target; }
     int numNeighbors(int src) { return graph[src].size(); }
     int loadMap(char* inputFile);
-    void getShortestPath(uint16_t destination);
-    void executeRoute(Serial &pc, StepperDrive &drive);
+    void getShortestPath(uint8_t destination);
+    void executeRoute();
+    void localizeRight();
+    void localizeLeft();
+    void localizeRightReverse();
+    void localizeLeftReverse();
+    
     // utility functions
-    uint16_t getMinDist(float target) { return minDistance[target]; }
+    float getMinDist(uint8_t target) { return minDistance[target]; }
     void printPrevious(Serial &pc);
     void printRoute(Serial &pc);
     void printGraph(Serial &pc);
 
 private:
     static const float MAX_DIST = 65535; // infinity
-    static const uint16_t DEFAULT_VERTEX = 0;
+    static const uint8_t DEFAULT_VERTEX = 0;
     static const float DEFAULT_ANGLE = 0.0;
     static const int DEFAULT_SIZE = 1;
     //static const int STEPS_PER_INCH = 20; //  per wheel diameter and stepper specs
     static const float PI = 3.14159;
-    Scanner& scanner;
+    Serial &pc;
+    StepperDrive &drive;
+    LongRangeSensor &longRangeL;
+    LongRangeSensor &longRangeR;
+    DigitalOut &led_red;
+    DigitalOut &led_green;
+    float distLocalL;
+    float distLocalR;
 
-    uint16_t vertex; // current vertex
+    uint8_t vertex; // current vertex
     float angle; // current angle
 
     struct graphNode
     {
-        uint16_t neighbor;
+        uint8_t neighbor;
         float distance; // in inches
         float angle;
         graphNode(float arg_neighbor = MAX_DIST,
@@ -55,7 +68,7 @@
     std::vector<std::vector<graphNode> > graph;
     std::vector<float> minDistance;
     std::vector<int> previous;
-    std::stack<uint16_t> route;
+    std::stack<uint8_t> route;
 };
 
 #endif // NAVIGATION_H
\ No newline at end of file