Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.cpp@1:a53d97b74fab, 2015-11-22 (annotated)

Committer:
j_j205
Date:
Sun Nov 22 19:32:45 2015 +0000
Revision:
1:a53d97b74fab
Parent:
0:fd72f6df078c
Child:
2:17bd430aeca1
jj 1/22/15;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 1:a53d97b74fab 1 #include "StepperMotor.h"
j_j205 0:fd72f6df078c 2 #include "navigation.h"
j_j205 0:fd72f6df078c 3 #include "stdint.h"
j_j205 0:fd72f6df078c 4 #include <fstream>
j_j205 0:fd72f6df078c 5 #include <stack>
j_j205 0:fd72f6df078c 6 #include <set>
j_j205 0:fd72f6df078c 7
j_j205 0:fd72f6df078c 8 // FUNCTION:
j_j205 0:fd72f6df078c 9 // Navigation(int size = DEFAULT_SIZE)
j_j205 0:fd72f6df078c 10 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 11 // Number of initial vertices (DEFAULT = 1)
j_j205 0:fd72f6df078c 12 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 13 // None
j_j205 0:fd72f6df078c 14 // DESCRIPTION:
j_j205 0:fd72f6df078c 15 // Default and 1-parameter constructor.
j_j205 0:fd72f6df078c 16 Navigation::Navigation(int size = DEFAULT_SIZE) : vertex(DEFAULT_VERTEX),
j_j205 0:fd72f6df078c 17 angle(DEFAULT_ANGLE)
j_j205 0:fd72f6df078c 18 {
j_j205 0:fd72f6df078c 19 graph.resize(size);
j_j205 0:fd72f6df078c 20 }
j_j205 0:fd72f6df078c 21
j_j205 0:fd72f6df078c 22 // FUNCTION:
j_j205 0:fd72f6df078c 23 // addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 24 // uint16_t angle)
j_j205 0:fd72f6df078c 25 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 26 // Source node (uint16_t), target node (uint16_t), edge distance
j_j205 0:fd72f6df078c 27 // (uint16_t), and angle (uint16_t)
j_j205 0:fd72f6df078c 28 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 29 // None
j_j205 0:fd72f6df078c 30 // DESCRIPTION:
j_j205 0:fd72f6df078c 31 // Adds a graph node to the graph.
j_j205 0:fd72f6df078c 32 void Navigation::addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 33 uint16_t angle)
j_j205 0:fd72f6df078c 34 {
j_j205 0:fd72f6df078c 35 graph[src].push_back(graphNode(target, dist, angle));
j_j205 0:fd72f6df078c 36 }
j_j205 0:fd72f6df078c 37
j_j205 0:fd72f6df078c 38 // FUNCTION:
j_j205 0:fd72f6df078c 39 // addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 40 // uint16_t angle)
j_j205 0:fd72f6df078c 41 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 42 // Source node (uint16_t)
j_j205 0:fd72f6df078c 43 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 44 // None
j_j205 0:fd72f6df078c 45 // DESCRIPTION:
j_j205 0:fd72f6df078c 46 // Adds a graph node to the graph, with target, dist, and angle
j_j205 0:fd72f6df078c 47 // initialized to infinity
j_j205 0:fd72f6df078c 48 void Navigation::addGraphNode(uint16_t src)
j_j205 0:fd72f6df078c 49 {
j_j205 0:fd72f6df078c 50 graph[src].push_back(graphNode());
j_j205 0:fd72f6df078c 51 }
j_j205 0:fd72f6df078c 52
j_j205 0:fd72f6df078c 53 // FUNCTION:
j_j205 0:fd72f6df078c 54 // int loadMap(char* infile);
j_j205 0:fd72f6df078c 55 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 56 // Input file name (*char[])
j_j205 0:fd72f6df078c 57 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 58 // Returns 1 if file doesn't open
j_j205 0:fd72f6df078c 59 // Returns 0 if file opens succesfully
j_j205 0:fd72f6df078c 60 // DESCRIPTION:
j_j205 0:fd72f6df078c 61 // Loads contents of a file as a weighted graph. First entry in the file
j_j205 0:fd72f6df078c 62 // should be the number of vertices. All subsequent entries indicate edges
j_j205 0:fd72f6df078c 63 // by listing a source vertex, a target vertex, the distance between them
j_j205 0:fd72f6df078c 64 // and the angle from source to target (0 - 359 degrees) . Only edges
j_j205 0:fd72f6df078c 65 // are listed. Assumes that inverse angles/edges are included in file.
j_j205 0:fd72f6df078c 66 int Navigation::loadMap(char* inputFile)
j_j205 0:fd72f6df078c 67 {
j_j205 0:fd72f6df078c 68 std::ifstream inFile;
j_j205 0:fd72f6df078c 69 inFile.open(inputFile);
j_j205 0:fd72f6df078c 70 if (!inFile)
j_j205 0:fd72f6df078c 71 return 1; // returns 1 if file fails to open
j_j205 0:fd72f6df078c 72
j_j205 0:fd72f6df078c 73 int vertices;
j_j205 0:fd72f6df078c 74 inFile >> vertices; // reads first entry as number of vertices
j_j205 0:fd72f6df078c 75 graph.resize(vertices); // resizes vector as necessary
j_j205 0:fd72f6df078c 76
j_j205 0:fd72f6df078c 77 uint16_t src;
j_j205 0:fd72f6df078c 78 uint16_t target;
j_j205 0:fd72f6df078c 79 uint16_t dist;
j_j205 0:fd72f6df078c 80 uint16_t angle;
j_j205 0:fd72f6df078c 81
j_j205 0:fd72f6df078c 82 while (!inFile.eof() )
j_j205 0:fd72f6df078c 83 {
j_j205 0:fd72f6df078c 84 inFile >> src;
j_j205 0:fd72f6df078c 85 inFile >> target;
j_j205 0:fd72f6df078c 86 inFile >> dist;
j_j205 0:fd72f6df078c 87 inFile >> angle;
j_j205 0:fd72f6df078c 88
j_j205 0:fd72f6df078c 89 addGraphNode(src, target, dist, angle);
j_j205 0:fd72f6df078c 90 } // loads all data from file into graph
j_j205 0:fd72f6df078c 91
j_j205 0:fd72f6df078c 92 return 0;
j_j205 0:fd72f6df078c 93 }
j_j205 0:fd72f6df078c 94
j_j205 0:fd72f6df078c 95 // FUNCTION:
j_j205 0:fd72f6df078c 96 // void getShortestPath(uint16_t src, uint16_t destination)
j_j205 0:fd72f6df078c 97 // IN-PARAMETERS:
j_j205 0:fd72f6df078c 98 // Target vertex (uint16_t)
j_j205 0:fd72f6df078c 99 // OUT-PARAMETERS:
j_j205 0:fd72f6df078c 100 // None
j_j205 0:fd72f6df078c 101 // DESCRIPTION:
j_j205 0:fd72f6df078c 102 // Implementation of Dijkstra's algorithm. Uses current vertex as source
j_j205 0:fd72f6df078c 103 // and calculates shortest path information for all other vertices.
j_j205 0:fd72f6df078c 104 // Subroutine ends when shortest path for target vertex has been found
j_j205 0:fd72f6df078c 105 // and then loads the path into route.
j_j205 0:fd72f6df078c 106 void Navigation::getShortestPath(uint16_t destination)
j_j205 0:fd72f6df078c 107 {
j_j205 0:fd72f6df078c 108 uint16_t src = vertex;
j_j205 0:fd72f6df078c 109 uint16_t v; // stores temporary vertex
j_j205 0:fd72f6df078c 110 int n = graph.size();
j_j205 0:fd72f6df078c 111 minDistance.clear();
j_j205 0:fd72f6df078c 112 minDistance.resize(n, MAX_DIST);
j_j205 0:fd72f6df078c 113 minDistance[src] = 0;
j_j205 0:fd72f6df078c 114 previous.clear();
j_j205 0:fd72f6df078c 115 previous.resize(n, -1);
j_j205 0:fd72f6df078c 116 std::set<std::pair<uint16_t, uint16_t> > vertex_queue;
j_j205 0:fd72f6df078c 117 vertex_queue.insert(std::make_pair(minDistance[src], src));
j_j205 0:fd72f6df078c 118
j_j205 0:fd72f6df078c 119 while (!vertex_queue.empty())
j_j205 0:fd72f6df078c 120 {
j_j205 0:fd72f6df078c 121 uint16_t dist = vertex_queue.begin()->first;
j_j205 0:fd72f6df078c 122 uint16_t u = vertex_queue.begin()->second;
j_j205 0:fd72f6df078c 123 vertex_queue.erase(vertex_queue.begin());
j_j205 0:fd72f6df078c 124
j_j205 0:fd72f6df078c 125 // Visit each edge exiting u
j_j205 0:fd72f6df078c 126 const std::vector<graphNode> &neighbors = graph[u];
j_j205 0:fd72f6df078c 127 for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin();
j_j205 0:fd72f6df078c 128 neighbor_iter != neighbors.end();
j_j205 0:fd72f6df078c 129 neighbor_iter++)
j_j205 0:fd72f6df078c 130 {
j_j205 0:fd72f6df078c 131 v = neighbor_iter->neighbor;
j_j205 0:fd72f6df078c 132 uint16_t weight = neighbor_iter->distance;
j_j205 0:fd72f6df078c 133 uint16_t distance_through_u = dist + weight;
j_j205 0:fd72f6df078c 134
j_j205 0:fd72f6df078c 135 if (distance_through_u < minDistance[v])
j_j205 0:fd72f6df078c 136 {
j_j205 0:fd72f6df078c 137 vertex_queue.erase(std::make_pair(minDistance[v], v));
j_j205 0:fd72f6df078c 138
j_j205 0:fd72f6df078c 139 minDistance[v] = distance_through_u;
j_j205 0:fd72f6df078c 140 previous[v] = u;
j_j205 0:fd72f6df078c 141 vertex_queue.insert(std::make_pair(minDistance[v], v));
j_j205 0:fd72f6df078c 142 }
j_j205 0:fd72f6df078c 143 if (v == destination)
j_j205 0:fd72f6df078c 144 break;
j_j205 0:fd72f6df078c 145 }
j_j205 0:fd72f6df078c 146 if (v == destination)
j_j205 0:fd72f6df078c 147 break;
j_j205 0:fd72f6df078c 148 }
j_j205 0:fd72f6df078c 149 while (v != src)
j_j205 0:fd72f6df078c 150 {
j_j205 0:fd72f6df078c 151 route.push(v);
j_j205 0:fd72f6df078c 152 v = previous[v];
j_j205 0:fd72f6df078c 153 }
j_j205 0:fd72f6df078c 154 }
j_j205 0:fd72f6df078c 155
j_j205 1:a53d97b74fab 156 // FUNCTION:
j_j205 1:a53d97b74fab 157 // void executeRoute()
j_j205 1:a53d97b74fab 158 // IN-PARAMETERS:
j_j205 1:a53d97b74fab 159 // Address of left and right steppers (StepperMotor &)
j_j205 1:a53d97b74fab 160 // OUT-PARAMETERS:
j_j205 1:a53d97b74fab 161 // None
j_j205 1:a53d97b74fab 162 // DESCRIPTION:
j_j205 1:a53d97b74fab 163 // Sends command to steppers to execute route.
j_j205 1:a53d97b74fab 164 void Navigation::executeRoute(StepperMotor &leftMotor, StepperMotor &rightMotor)
j_j205 1:a53d97b74fab 165 {
j_j205 1:a53d97b74fab 166 uint16_t target;
j_j205 1:a53d97b74fab 167 uint16_t targetAngle;
j_j205 1:a53d97b74fab 168 uint16_t targetDist;
j_j205 1:a53d97b74fab 169
j_j205 1:a53d97b74fab 170 while (!route.empty() )
j_j205 1:a53d97b74fab 171 {
j_j205 1:a53d97b74fab 172 target = route.top();
j_j205 1:a53d97b74fab 173 route.pop();
j_j205 1:a53d97b74fab 174
j_j205 1:a53d97b74fab 175 for (int i = 0; graph[vertex][i].neighbor == target; i++)
j_j205 1:a53d97b74fab 176 }
j_j205 1:a53d97b74fab 177 }
j_j205 1:a53d97b74fab 178
j_j205 0:fd72f6df078c 179 // utility function
j_j205 0:fd72f6df078c 180 void Navigation::printPrevious(Serial &pc)
j_j205 0:fd72f6df078c 181 {
j_j205 0:fd72f6df078c 182 for(int i = 0; i < previous.size(); i++)
j_j205 0:fd72f6df078c 183 {
j_j205 0:fd72f6df078c 184 pc.printf("Node %i is prev to node %i\n", previous[i], i);
j_j205 0:fd72f6df078c 185 }
j_j205 0:fd72f6df078c 186 }
j_j205 0:fd72f6df078c 187
j_j205 0:fd72f6df078c 188 // utility function
j_j205 0:fd72f6df078c 189 void Navigation::printRoute(Serial &pc)
j_j205 0:fd72f6df078c 190 {
j_j205 0:fd72f6df078c 191 while (!route.empty() )
j_j205 0:fd72f6df078c 192 {
j_j205 0:fd72f6df078c 193 pc.printf("Go to node %i\n", route.top() );
j_j205 0:fd72f6df078c 194 route.pop();
j_j205 0:fd72f6df078c 195 }
j_j205 0:fd72f6df078c 196 }
j_j205 0:fd72f6df078c 197
j_j205 0:fd72f6df078c 198 // utility function
j_j205 0:fd72f6df078c 199 void Navigation::printGraph(Serial &pc)
j_j205 0:fd72f6df078c 200 {
j_j205 0:fd72f6df078c 201 for (int i = 0; i < graph.size(); i++)
j_j205 0:fd72f6df078c 202 for (int j = 0; j < graph[i].size(); j++)
j_j205 0:fd72f6df078c 203 {
j_j205 0:fd72f6df078c 204 pc.printf("Node %i to %i = dist - %i, angle - %i\n", i, j, graph[i][j].distance, graph[i][j].angle);
j_j205 0:fd72f6df078c 205 }
j_j205 0:fd72f6df078c 206 }