navigation updated with completed dijkstra's algo
Dependents: R5 2016 Robotics Team 1
navigation.cpp
- Committer:
- j_j205
- Date:
- 2016-02-17
- Revision:
- 3:27cc2bacd6af
- Parent:
- 2:17bd430aeca1
- Child:
- 4:a5d44517c65c
File content as of revision 3:27cc2bacd6af:
#include "StepperDrive.h" #include "navigation.h" #include "stdint.h" #include <cmath> #include <fstream> #include <stack> #include <set> // FUNCTION: // Navigation(int size = DEFAULT_SIZE) // IN-PARAMETERS: // Number of initial vertices (DEFAULT = 1) // OUT-PARAMETERS: // None // DESCRIPTION: // Default and 1-parameter constructor. Navigation::Navigation(int size = DEFAULT_SIZE) : vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE) { graph.resize(size); } // FUNCTION: // addGraphNode(uint16_t src, uint16_t target, uint16_t dist, // uint16_t angle) // IN-PARAMETERS: // Source node (uint16_t), target node (uint16_t), edge distance // (uint16_t), and angle (uint16_t) // OUT-PARAMETERS: // None // DESCRIPTION: // Adds a graph node to the graph. void Navigation::addGraphNode(uint16_t src, uint16_t target, float dist, float angle) { graph[src].push_back(graphNode(target, dist, angle)); } // FUNCTION: // addGraphNode(uint16_t src, uint16_t target, uint16_t dist, // uint16_t angle) // IN-PARAMETERS: // Source node (uint16_t) // OUT-PARAMETERS: // None // DESCRIPTION: // Adds a graph node to the graph, with target, dist, and angle // initialized to infinity void Navigation::addGraphNode(uint16_t src) { graph[src].push_back(graphNode()); } // FUNCTION: // int loadMap(char* infile); // IN-PARAMETERS: // Input file name (*char[]) // OUT-PARAMETERS: // Returns 1 if file doesn't open // Returns 0 if file opens succesfully // DESCRIPTION: // Loads contents of a file as a weighted graph. First entry in the file // should be the number of vertices. All subsequent entries indicate edges // by listing a source vertex, a target vertex, the distance between them // and the angle from source to target (0 - 359 degrees) . Only edges // are listed. Assumes that inverse angles/edges are included in file. int Navigation::loadMap(char* inputFile) { std::ifstream inFile; inFile.open(inputFile); if (!inFile) return 1; // returns 1 if file fails to open int vertices; inFile >> vertices; // reads first entry as number of vertices graph.resize(vertices); // resizes vector as necessary uint16_t src; uint16_t target; float dist; float angle; while (!inFile.eof() ) { inFile >> src; inFile >> target; inFile >> dist; inFile >> angle; addGraphNode(src, target, dist, angle); } // loads all data from file into graph return 0; } // FUNCTION: // void getShortestPath(uint16_t src, uint16_t destination) // IN-PARAMETERS: // Target vertex (uint16_t) // OUT-PARAMETERS: // None // DESCRIPTION: // Implementation of Dijkstra's algorithm. Uses current vertex as source // and calculates shortest path information for all other vertices. // Subroutine ends when shortest path for target vertex has been found // and then loads the path into route. void Navigation::getShortestPath(uint16_t destination) { uint16_t src = vertex; uint16_t v; // stores temporary vertex int n = graph.size(); minDistance.clear(); minDistance.resize(n, MAX_DIST); minDistance[src] = 0; previous.clear(); previous.resize(n, -1); std::set<std::pair<float, uint16_t> > vertex_queue; vertex_queue.insert(std::make_pair(minDistance[src], src)); while (!vertex_queue.empty()) { float dist = vertex_queue.begin()->first; uint16_t u = vertex_queue.begin()->second; vertex_queue.erase(vertex_queue.begin()); // Visit each edge exiting u const std::vector<graphNode> &neighbors = graph[u]; for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin(); neighbor_iter != neighbors.end(); neighbor_iter++) { v = neighbor_iter->neighbor; float weight = neighbor_iter->distance; float distance_through_u = dist + weight; if (distance_through_u < minDistance[v]) { vertex_queue.erase(std::make_pair(minDistance[v], v)); minDistance[v] = distance_through_u; previous[v] = u; vertex_queue.insert(std::make_pair(minDistance[v], v)); } if (v == destination) break; } if (v == destination) break; } while (v != src) { route.push(v); v = previous[v]; } } // FUNCTION: // void executeRoute() // IN-PARAMETERS: // Address of left and right steppers (StepperMotor &) // OUT-PARAMETERS: // None // DESCRIPTION: // Sends command to steppers to execute route. void Navigation::executeRoute(Serial &pc, StepperDrive &drive) { uint16_t target; float targetAngle; float targetDist; float differenceAngle; while (!route.empty() ) { target = route.top(); route.pop(); 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 */ } /* send -(differenceAngle) to motors */ int returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) ); pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } } /* send targetDist to motors */ int returnVal = drive.move(targetDist, 0); pc.printf("\nMoved forward a distance of %f to node %i", targetDist, target); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } vertex = target; angle = targetAngle; } } // utility function void Navigation::printPrevious(Serial &pc) { for(int i = 0; i < previous.size(); i++) { pc.printf("Node %f is prev to node %f\n", previous[i], i); } } // utility function void Navigation::printRoute(Serial &pc) { while (!route.empty() ) { pc.printf("Go to node %i\n", route.top() ); route.pop(); } } // utility function void Navigation::printGraph(Serial &pc) { for (int i = 0; i < graph.size(); i++) for (int j = 0; j < graph[i].size(); j++) { pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle); } }