navigation updated with completed dijkstra's algo
Dependents: R5 2016 Robotics Team 1
navigation.cpp
- Committer:
- j_j205
- Date:
- 2016-04-08
- Revision:
- 9:e24b50c1741b
- Parent:
- 8:290a110bcf0e
- Child:
- 10:23e21c19b48a
File content as of revision 9:e24b50c1741b:
#include "StepperDrive.h" #include "LongRangeSensor.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(Serial &_pc, StepperDrive &_drive, LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green, int size = DEFAULT_SIZE) : pc(_pc), drive(_drive), longRangeL(_longRangeL), longRangeR(_longRangeR), led_red(_led_red), led_green(_led_green), vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE) { graph.resize(size); distLocalL = 0.0; } // 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(uint8_t src, uint8_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(uint8_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 uint8_t src; uint8_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(uint8_t destination) { uint8_t src = vertex; uint8_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, uint8_t> > vertex_queue; vertex_queue.insert(std::make_pair(minDistance[src], src)); while (!vertex_queue.empty()) { float dist = vertex_queue.begin()->first; uint8_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() { uint8_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); } /* distLocalL = longRangeL.distInchesLOne(); distLocalR = longRangeR.distInchesROne(); pc.printf("\n distLocalL = %f", distLocalL); pc.printf("\n distLocalR = %f", distLocalR); if ((distLocalL <= 8.7) && (distLocalL >= 3.0)) // wall in range { localizeRight(); } else if ((distLocalR <= 10.5) && (distLocalR >= 5.0)) // wall in range { localizeLeft(); }*/ } /* 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; distLocalL = longRangeL.distInchesLOne(); distLocalR = longRangeR.distInchesROne(); pc.printf("\n distLocalL = %f", distLocalL); pc.printf("\n distLocalR = %f", distLocalR); if (distLocalL <= 18.0) // wall in range { newLocalizeRight(); } else if (distLocalR <= 13.0) // wall in range { newLocalizeLeft(); } } } // 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); } } // FUNCTION: // void localizeRight() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Using sensor longRangeL this method will localize to the wall // to the right void Navigation::localizeRight() { if (distLocalL >= 7.8) // very close left, turn more right { drive.move(0, (5.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle more right, in localizeRight"); } else if (distLocalL >= 6.9) // too close left, turn right { drive.move(0, (2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle right, in localizeRight"); } else if (distLocalL <= 5.5) // too close right, turn left { drive.move(0, (-2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle left , in localizeRight"); } else { /* do nothing */ pc.printf("\n No need to adjust, in localizeRight"); } } // FUNCTION: // void localizeLeft() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Using sensor longRangeR this method will localize to the wall // to the left during stretches of forward motion where robot // should remain xxx distance from the left wall. void Navigation::localizeLeft() { if (distLocalR >= 9.7) // too close right, turn left { drive.move(0.0, (-2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle left, in localizeLeft"); } else if (distLocalR <= 8.3) // too close left, turn right { drive.move(0.0, (2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle right, in localizeLeft"); } else { /* do nothing */ pc.printf("\n No need to adjust, in localizeLeft"); } } // FUNCTION: // void localizeRightReverse() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Using sensor longRangeL this method will localize to the wall // to the right void Navigation::localizeRightReverse() { if (distLocalL >= 7.2) // too close left, turn left { drive.move(0, (-2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle left, in localizeRightReverse"); } else if (distLocalL <= 6.2) // too close right, turn right { drive.move(0, (2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle right, in localizeRightReverse"); } else { /* do nothing */ pc.printf("\n No need to adjust, in localizeRightReverse"); } } // FUNCTION: // void localizeLeftReverse() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Using sensor longRangeR this method will localize to the wall // to the left during stretches of forward motion where robot // should remain xxx distance from the left wall. void Navigation::localizeLeftReverse() { if (distLocalR >= 9.7) // toos close right, turn right { drive.move(0.0, (2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle right, in localizeLeftReverse"); } else if (distLocalR <= 8.3) // too close left, turn left { drive.move(0.0, (-2.0)*(3.14159 / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } pc.printf("\n Adjusted angle left, in localizeLeftReverse"); } else { /* do nothing */ pc.printf("\n No need to adjust, in localizeLeftReverse"); } } // FUNCTION: // void newLocalize() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Uses difference between sensors to adjust angle void Navigation::newLocalize() { float k = 2.5; float angleAdjust = k*(distLocalL - distLocalR); drive.move(0, angleAdjust*(PI / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } angle += angleAdjust; pc.printf("\n Adjusted angle by %f in newLocalize", angleAdjust); } // FUNCTION: // void newLocalizeRight() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Left sensor to adjust off right wall void Navigation::newLocalizeRight() { float k = 2.0; float angleAdjust = k*(distLocalL - 13.0); drive.move(0, angleAdjust*(PI / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } //angle += (-1*angleAdjust); pc.printf("\n Adjusted angle by %f in newLocalizeRight", -1*angleAdjust); } // FUNCTION: // void newLocalizeLeft() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Right sensor to adjust off Left wall void Navigation::newLocalizeLeft() { float k = 3.5; float angleAdjust = k*(9.1 - distLocalR); drive.move(0, angleAdjust*(PI / 180.0)); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } //angle += (-1*angleAdjust); pc.printf("\n Adjusted angle by %f in newLocalizeLeft", -1*angleAdjust); } // FUNCTION: // void newLocalizeLeftReverse() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Right sensor to adjust off Left wall void Navigation::newLocalizeLeftReverse() { float k = 3.5; float angleAdjust = k*(9.1 - distLocalR); drive.move(0, -1*(angleAdjust*(PI / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } //angle += (-1*angleAdjust); pc.printf("\n Adjusted angle by %f in newLocalizeLeftReverse", angleAdjust); } // FUNCTION: // void newLocalizeRightReverse() // IN-PARAMETERS: // None // OUT-PARAMETERS: // None // DESCRIPTION: // Left sensor to adjust off right wall void Navigation::newLocalizeRightReverse() { float k = 2.0; float angleAdjust = k*(distLocalL - 13.0); drive.move(0, -1*(angleAdjust*(PI / 180.0))); // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } //angle += (-1*angleAdjust); pc.printf("\n Adjusted angle by %f in newLocalizeRight", angleAdjust); }