navigation updated with completed dijkstra's algo
Dependents: R5 2016 Robotics Team 1
navigation.h@12:6d37ea71da65, 2017-02-27 (annotated)
- Committer:
- j_j205
- Date:
- Mon Feb 27 12:02:07 2017 +0000
- Revision:
- 12:6d37ea71da65
- Parent:
- 8:290a110bcf0e
removed debug print statements
Who changed what in which revision?
User | Revision | Line number | New 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 | 6:d2da4d4b5112 | 4 | #include "LongRangeSensor.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 | 6:d2da4d4b5112 | 13 | Navigation(Serial &_pc, StepperDrive &_drive, LongRangeSensor |
j_j205 | 6:d2da4d4b5112 | 14 | &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green, int size); |
j_j205 | 6:d2da4d4b5112 | 15 | void addGraphNode(uint8_t src, uint8_t target, float dist, |
j_j205 | 6:d2da4d4b5112 | 16 | float angle); |
j_j205 | 6:d2da4d4b5112 | 17 | void addGraphNode(uint8_t src); |
j_j205 | 0:fd72f6df078c | 18 | int graphSize() { return graph.size(); } |
j_j205 | 6:d2da4d4b5112 | 19 | uint8_t getVertex() { return vertex; } |
j_j205 | 2:17bd430aeca1 | 20 | float getAngle() { return angle; } |
j_j205 | 6:d2da4d4b5112 | 21 | void setVertex(uint8_t target) { vertex = target; } |
j_j205 | 2:17bd430aeca1 | 22 | void setAngle(float target) { angle = target; } |
j_j205 | 0:fd72f6df078c | 23 | int numNeighbors(int src) { return graph[src].size(); } |
j_j205 | 0:fd72f6df078c | 24 | int loadMap(char* inputFile); |
j_j205 | 6:d2da4d4b5112 | 25 | void getShortestPath(uint8_t destination); |
j_j205 | 6:d2da4d4b5112 | 26 | void executeRoute(); |
j_j205 | 6:d2da4d4b5112 | 27 | void localizeRight(); |
j_j205 | 6:d2da4d4b5112 | 28 | void localizeLeft(); |
j_j205 | 6:d2da4d4b5112 | 29 | void localizeRightReverse(); |
j_j205 | 6:d2da4d4b5112 | 30 | void localizeLeftReverse(); |
j_j205 | 7:f7489797746b | 31 | void newLocalizeRight(); |
j_j205 | 7:f7489797746b | 32 | void newLocalizeLeft(); |
j_j205 | 8:290a110bcf0e | 33 | void newLocalizeRightReverse(); |
j_j205 | 8:290a110bcf0e | 34 | void newLocalizeLeftReverse(); |
j_j205 | 7:f7489797746b | 35 | void newLocalize(); |
j_j205 | 6:d2da4d4b5112 | 36 | |
j_j205 | 0:fd72f6df078c | 37 | // utility functions |
j_j205 | 6:d2da4d4b5112 | 38 | float getMinDist(uint8_t target) { return minDistance[target]; } |
j_j205 | 0:fd72f6df078c | 39 | void printPrevious(Serial &pc); |
j_j205 | 0:fd72f6df078c | 40 | void printRoute(Serial &pc); |
j_j205 | 0:fd72f6df078c | 41 | void printGraph(Serial &pc); |
j_j205 | 0:fd72f6df078c | 42 | |
j_j205 | 0:fd72f6df078c | 43 | private: |
j_j205 | 2:17bd430aeca1 | 44 | static const float MAX_DIST = 65535; // infinity |
j_j205 | 6:d2da4d4b5112 | 45 | static const uint8_t DEFAULT_VERTEX = 0; |
j_j205 | 2:17bd430aeca1 | 46 | static const float DEFAULT_ANGLE = 0.0; |
j_j205 | 0:fd72f6df078c | 47 | static const int DEFAULT_SIZE = 1; |
j_j205 | 2:17bd430aeca1 | 48 | //static const int STEPS_PER_INCH = 20; // per wheel diameter and stepper specs |
j_j205 | 2:17bd430aeca1 | 49 | static const float PI = 3.14159; |
j_j205 | 6:d2da4d4b5112 | 50 | Serial &pc; |
j_j205 | 6:d2da4d4b5112 | 51 | StepperDrive &drive; |
j_j205 | 6:d2da4d4b5112 | 52 | LongRangeSensor &longRangeL; |
j_j205 | 6:d2da4d4b5112 | 53 | LongRangeSensor &longRangeR; |
j_j205 | 6:d2da4d4b5112 | 54 | DigitalOut &led_red; |
j_j205 | 6:d2da4d4b5112 | 55 | DigitalOut &led_green; |
j_j205 | 6:d2da4d4b5112 | 56 | float distLocalL; |
j_j205 | 6:d2da4d4b5112 | 57 | float distLocalR; |
j_j205 | 0:fd72f6df078c | 58 | |
j_j205 | 6:d2da4d4b5112 | 59 | uint8_t vertex; // current vertex |
j_j205 | 2:17bd430aeca1 | 60 | float angle; // current angle |
j_j205 | 0:fd72f6df078c | 61 | |
j_j205 | 0:fd72f6df078c | 62 | struct graphNode |
j_j205 | 0:fd72f6df078c | 63 | { |
j_j205 | 6:d2da4d4b5112 | 64 | uint8_t neighbor; |
j_j205 | 2:17bd430aeca1 | 65 | float distance; // in inches |
j_j205 | 2:17bd430aeca1 | 66 | float angle; |
j_j205 | 2:17bd430aeca1 | 67 | graphNode(float arg_neighbor = MAX_DIST, |
j_j205 | 2:17bd430aeca1 | 68 | float arg_distance = MAX_DIST, |
j_j205 | 2:17bd430aeca1 | 69 | float arg_angle = MAX_DIST) |
j_j205 | 0:fd72f6df078c | 70 | : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){} |
j_j205 | 0:fd72f6df078c | 71 | }; |
j_j205 | 0:fd72f6df078c | 72 | |
j_j205 | 0:fd72f6df078c | 73 | std::vector<std::vector<graphNode> > graph; |
j_j205 | 2:17bd430aeca1 | 74 | std::vector<float> minDistance; |
j_j205 | 0:fd72f6df078c | 75 | std::vector<int> previous; |
j_j205 | 6:d2da4d4b5112 | 76 | std::stack<uint8_t> route; |
j_j205 | 0:fd72f6df078c | 77 | }; |
j_j205 | 0:fd72f6df078c | 78 | |
j_j205 | 4:a5d44517c65c | 79 | #endif // NAVIGATION_H |