Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@0:fd72f6df078c, 2015-10-30 (annotated)

Committer:
j_j205
Date:
Fri Oct 30 15:48:48 2015 +0000
Revision:
0:fd72f6df078c
Child:
1:a53d97b74fab
complete navigation with dijkstra

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 0:fd72f6df078c 1 #ifndef NAVIGATION_H
j_j205 0:fd72f6df078c 2 #define NAVIGATION_H
j_j205 0:fd72f6df078c 3 #include <vector>
j_j205 0:fd72f6df078c 4 #include <stack>
j_j205 0:fd72f6df078c 5 #include "stdint.h"
j_j205 0:fd72f6df078c 6 #include "mbed.h"
j_j205 0:fd72f6df078c 7
j_j205 0:fd72f6df078c 8 class Navigation
j_j205 0:fd72f6df078c 9 {
j_j205 0:fd72f6df078c 10 public:
j_j205 0:fd72f6df078c 11 Navigation(int size);
j_j205 0:fd72f6df078c 12 void addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 13 uint16_t angle);
j_j205 0:fd72f6df078c 14 void addGraphNode(uint16_t src);
j_j205 0:fd72f6df078c 15 int graphSize() { return graph.size(); }
j_j205 0:fd72f6df078c 16 uint16_t getVertex() { return vertex; }
j_j205 0:fd72f6df078c 17 uint16_t getAngle() { return angle; }
j_j205 0:fd72f6df078c 18 void setVertex(uint16_t target) { vertex = target; }
j_j205 0:fd72f6df078c 19 int numNeighbors(int src) { return graph[src].size(); }
j_j205 0:fd72f6df078c 20 int loadMap(char* inputFile);
j_j205 0:fd72f6df078c 21 void getShortestPath(uint16_t destination);
j_j205 0:fd72f6df078c 22 uint16_t getMinDist(uint16_t target) { return minDistance[target]; }
j_j205 0:fd72f6df078c 23 std::stack<uint16_t> getRoute(uint16_t src, uint16_t target);
j_j205 0:fd72f6df078c 24 // utility functions
j_j205 0:fd72f6df078c 25 void printPrevious(Serial &pc);
j_j205 0:fd72f6df078c 26 void printRoute(Serial &pc);
j_j205 0:fd72f6df078c 27 void printGraph(Serial &pc);
j_j205 0:fd72f6df078c 28
j_j205 0:fd72f6df078c 29 private:
j_j205 0:fd72f6df078c 30 static const uint16_t MAX_DIST = 65535; // infinity
j_j205 0:fd72f6df078c 31 static const uint16_t DEFAULT_VERTEX = 0;
j_j205 0:fd72f6df078c 32 static const uint16_t DEFAULT_ANGLE = 0;
j_j205 0:fd72f6df078c 33 static const int DEFAULT_SIZE = 1;
j_j205 0:fd72f6df078c 34
j_j205 0:fd72f6df078c 35 uint16_t vertex;
j_j205 0:fd72f6df078c 36 uint16_t angle;
j_j205 0:fd72f6df078c 37
j_j205 0:fd72f6df078c 38 struct graphNode
j_j205 0:fd72f6df078c 39 {
j_j205 0:fd72f6df078c 40 uint16_t neighbor;
j_j205 0:fd72f6df078c 41 uint16_t distance;
j_j205 0:fd72f6df078c 42 uint16_t angle;
j_j205 0:fd72f6df078c 43 graphNode(uint16_t arg_neighbor = MAX_DIST,
j_j205 0:fd72f6df078c 44 uint16_t arg_distance = MAX_DIST,
j_j205 0:fd72f6df078c 45 uint16_t arg_angle = MAX_DIST)
j_j205 0:fd72f6df078c 46 : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
j_j205 0:fd72f6df078c 47 };
j_j205 0:fd72f6df078c 48
j_j205 0:fd72f6df078c 49 std::vector<std::vector<graphNode> > graph;
j_j205 0:fd72f6df078c 50 std::vector<uint16_t> minDistance;
j_j205 0:fd72f6df078c 51 std::vector<int> previous;
j_j205 0:fd72f6df078c 52 std::stack<uint16_t> route;
j_j205 0:fd72f6df078c 53 };
j_j205 0:fd72f6df078c 54
j_j205 0:fd72f6df078c 55 #endif // NAVIGATION_H
j_j205 0:fd72f6df078c 56
j_j205 0:fd72f6df078c 57