Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.cpp@5:d0954e0aecc9, 2016-03-21 (annotated)

Committer:
j_j205
Date:
Mon Mar 21 01:32:00 2016 +0000
Revision:
5:d0954e0aecc9
Parent:
4:a5d44517c65c
Child:
6:d2da4d4b5112
3/20/16 updated code jj

Who changed what in which revision?

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