Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@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 0:fd72f6df078c 1 #ifndef NAVIGATION_H
j_j205 0:fd72f6df078c 2 #define NAVIGATION_H
j_j205 2:17bd430aeca1 3 #include "StepperDrive.h"
j_j205 5:d0954e0aecc9 4 #include "scanner.h"
j_j205 1:a53d97b74fab 5 #include "stdint.h"
j_j205 1:a53d97b74fab 6 #include "mbed.h"
j_j205 0:fd72f6df078c 7 #include <vector>
j_j205 0:fd72f6df078c 8 #include <stack>
j_j205 0:fd72f6df078c 9
j_j205 0:fd72f6df078c 10 class Navigation
j_j205 0:fd72f6df078c 11 {
j_j205 0:fd72f6df078c 12 public:
j_j205 5:d0954e0aecc9 13 Navigation(Scanner& _scanner, int size);
j_j205 2:17bd430aeca1 14 void addGraphNode(uint16_t src, uint16_t target, float dist,
j_j205 2:17bd430aeca1 15 float angle);
j_j205 0:fd72f6df078c 16 void addGraphNode(uint16_t src);
j_j205 0:fd72f6df078c 17 int graphSize() { return graph.size(); }
j_j205 0:fd72f6df078c 18 uint16_t getVertex() { return vertex; }
j_j205 2:17bd430aeca1 19 float getAngle() { return angle; }
j_j205 0:fd72f6df078c 20 void setVertex(uint16_t target) { vertex = target; }
j_j205 2:17bd430aeca1 21 void setAngle(float target) { angle = target; }
j_j205 0:fd72f6df078c 22 int numNeighbors(int src) { return graph[src].size(); }
j_j205 0:fd72f6df078c 23 int loadMap(char* inputFile);
j_j205 0:fd72f6df078c 24 void getShortestPath(uint16_t destination);
j_j205 2:17bd430aeca1 25 void executeRoute(Serial &pc, StepperDrive &drive);
j_j205 0:fd72f6df078c 26 // utility functions
j_j205 2:17bd430aeca1 27 uint16_t getMinDist(float target) { return minDistance[target]; }
j_j205 0:fd72f6df078c 28 void printPrevious(Serial &pc);
j_j205 0:fd72f6df078c 29 void printRoute(Serial &pc);
j_j205 0:fd72f6df078c 30 void printGraph(Serial &pc);
j_j205 0:fd72f6df078c 31
j_j205 0:fd72f6df078c 32 private:
j_j205 2:17bd430aeca1 33 static const float MAX_DIST = 65535; // infinity
j_j205 0:fd72f6df078c 34 static const uint16_t DEFAULT_VERTEX = 0;
j_j205 2:17bd430aeca1 35 static const float DEFAULT_ANGLE = 0.0;
j_j205 0:fd72f6df078c 36 static const int DEFAULT_SIZE = 1;
j_j205 2:17bd430aeca1 37 //static const int STEPS_PER_INCH = 20; // per wheel diameter and stepper specs
j_j205 2:17bd430aeca1 38 static const float PI = 3.14159;
j_j205 5:d0954e0aecc9 39 Scanner& scanner;
j_j205 0:fd72f6df078c 40
j_j205 1:a53d97b74fab 41 uint16_t vertex; // current vertex
j_j205 2:17bd430aeca1 42 float angle; // current angle
j_j205 0:fd72f6df078c 43
j_j205 0:fd72f6df078c 44 struct graphNode
j_j205 0:fd72f6df078c 45 {
j_j205 0:fd72f6df078c 46 uint16_t neighbor;
j_j205 2:17bd430aeca1 47 float distance; // in inches
j_j205 2:17bd430aeca1 48 float angle;
j_j205 2:17bd430aeca1 49 graphNode(float arg_neighbor = MAX_DIST,
j_j205 2:17bd430aeca1 50 float arg_distance = MAX_DIST,
j_j205 2:17bd430aeca1 51 float arg_angle = MAX_DIST)
j_j205 0:fd72f6df078c 52 : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
j_j205 0:fd72f6df078c 53 };
j_j205 0:fd72f6df078c 54
j_j205 0:fd72f6df078c 55 std::vector<std::vector<graphNode> > graph;
j_j205 2:17bd430aeca1 56 std::vector<float> minDistance;
j_j205 0:fd72f6df078c 57 std::vector<int> previous;
j_j205 0:fd72f6df078c 58 std::stack<uint16_t> route;
j_j205 0:fd72f6df078c 59 };
j_j205 0:fd72f6df078c 60
j_j205 4:a5d44517c65c 61 #endif // NAVIGATION_H