Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@4:a5d44517c65c, 2016-02-23 (annotated)

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