Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@1:a53d97b74fab, 2015-11-22 (annotated)

Committer:
j_j205
Date:
Sun Nov 22 19:32:45 2015 +0000
Revision:
1:a53d97b74fab
Parent:
0:fd72f6df078c
Child:
2:17bd430aeca1
jj 1/22/15;

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 1:a53d97b74fab 3 #include "StepperMotor.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 0:fd72f6df078c 13 void addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
j_j205 0:fd72f6df078c 14 uint16_t 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 0:fd72f6df078c 18 uint16_t getAngle() { return angle; }
j_j205 0:fd72f6df078c 19 void setVertex(uint16_t target) { vertex = target; }
j_j205 0:fd72f6df078c 20 int numNeighbors(int src) { return graph[src].size(); }
j_j205 0:fd72f6df078c 21 int loadMap(char* inputFile);
j_j205 0:fd72f6df078c 22 void getShortestPath(uint16_t destination);
j_j205 0:fd72f6df078c 23 uint16_t getMinDist(uint16_t target) { return minDistance[target]; }
j_j205 1:a53d97b74fab 24 void executeRoute(StepperMotor &leftMotor, StepperMotor &rightMotor);
j_j205 0:fd72f6df078c 25 // utility functions
j_j205 0:fd72f6df078c 26 void printPrevious(Serial &pc);
j_j205 0:fd72f6df078c 27 void printRoute(Serial &pc);
j_j205 0:fd72f6df078c 28 void printGraph(Serial &pc);
j_j205 0:fd72f6df078c 29
j_j205 0:fd72f6df078c 30 private:
j_j205 0:fd72f6df078c 31 static const uint16_t MAX_DIST = 65535; // infinity
j_j205 0:fd72f6df078c 32 static const uint16_t DEFAULT_VERTEX = 0;
j_j205 0:fd72f6df078c 33 static const uint16_t DEFAULT_ANGLE = 0;
j_j205 0:fd72f6df078c 34 static const int DEFAULT_SIZE = 1;
j_j205 1:a53d97b74fab 35 static const int STEPS_PER_INCH = 20; // per wheel diameter and stepper specs
j_j205 0:fd72f6df078c 36
j_j205 1:a53d97b74fab 37 uint16_t vertex; // current vertex
j_j205 1:a53d97b74fab 38 uint16_t angle; // current angle
j_j205 0:fd72f6df078c 39
j_j205 0:fd72f6df078c 40 struct graphNode
j_j205 0:fd72f6df078c 41 {
j_j205 0:fd72f6df078c 42 uint16_t neighbor;
j_j205 1:a53d97b74fab 43 uint16_t distance; // in inches
j_j205 0:fd72f6df078c 44 uint16_t angle;
j_j205 0:fd72f6df078c 45 graphNode(uint16_t arg_neighbor = MAX_DIST,
j_j205 0:fd72f6df078c 46 uint16_t arg_distance = MAX_DIST,
j_j205 0:fd72f6df078c 47 uint16_t arg_angle = MAX_DIST)
j_j205 0:fd72f6df078c 48 : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
j_j205 0:fd72f6df078c 49 };
j_j205 0:fd72f6df078c 50
j_j205 0:fd72f6df078c 51 std::vector<std::vector<graphNode> > graph;
j_j205 0:fd72f6df078c 52 std::vector<uint16_t> minDistance;
j_j205 0:fd72f6df078c 53 std::vector<int> previous;
j_j205 0:fd72f6df078c 54 std::stack<uint16_t> route;
j_j205 0:fd72f6df078c 55 };
j_j205 0:fd72f6df078c 56
j_j205 0:fd72f6df078c 57 #endif // NAVIGATION_H
j_j205 0:fd72f6df078c 58
j_j205 0:fd72f6df078c 59