Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@7:f7489797746b, 2016-04-08 (annotated)

Committer:
j_j205
Date:
Fri Apr 08 01:35:28 2016 +0000
Revision:
7:f7489797746b
Parent:
6:d2da4d4b5112
Child:
8:290a110bcf0e
latest localization

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 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 7:f7489797746b 33 void newLocalize();
j_j205 6:d2da4d4b5112 34
j_j205 0:fd72f6df078c 35 // utility functions
j_j205 6:d2da4d4b5112 36 float getMinDist(uint8_t target) { return minDistance[target]; }
j_j205 0:fd72f6df078c 37 void printPrevious(Serial &pc);
j_j205 0:fd72f6df078c 38 void printRoute(Serial &pc);
j_j205 0:fd72f6df078c 39 void printGraph(Serial &pc);
j_j205 0:fd72f6df078c 40
j_j205 0:fd72f6df078c 41 private:
j_j205 2:17bd430aeca1 42 static const float MAX_DIST = 65535; // infinity
j_j205 6:d2da4d4b5112 43 static const uint8_t DEFAULT_VERTEX = 0;
j_j205 2:17bd430aeca1 44 static const float DEFAULT_ANGLE = 0.0;
j_j205 0:fd72f6df078c 45 static const int DEFAULT_SIZE = 1;
j_j205 2:17bd430aeca1 46 //static const int STEPS_PER_INCH = 20; // per wheel diameter and stepper specs
j_j205 2:17bd430aeca1 47 static const float PI = 3.14159;
j_j205 6:d2da4d4b5112 48 Serial &pc;
j_j205 6:d2da4d4b5112 49 StepperDrive &drive;
j_j205 6:d2da4d4b5112 50 LongRangeSensor &longRangeL;
j_j205 6:d2da4d4b5112 51 LongRangeSensor &longRangeR;
j_j205 6:d2da4d4b5112 52 DigitalOut &led_red;
j_j205 6:d2da4d4b5112 53 DigitalOut &led_green;
j_j205 6:d2da4d4b5112 54 float distLocalL;
j_j205 6:d2da4d4b5112 55 float distLocalR;
j_j205 0:fd72f6df078c 56
j_j205 6:d2da4d4b5112 57 uint8_t vertex; // current vertex
j_j205 2:17bd430aeca1 58 float angle; // current angle
j_j205 0:fd72f6df078c 59
j_j205 0:fd72f6df078c 60 struct graphNode
j_j205 0:fd72f6df078c 61 {
j_j205 6:d2da4d4b5112 62 uint8_t neighbor;
j_j205 2:17bd430aeca1 63 float distance; // in inches
j_j205 2:17bd430aeca1 64 float angle;
j_j205 2:17bd430aeca1 65 graphNode(float arg_neighbor = MAX_DIST,
j_j205 2:17bd430aeca1 66 float arg_distance = MAX_DIST,
j_j205 2:17bd430aeca1 67 float arg_angle = MAX_DIST)
j_j205 0:fd72f6df078c 68 : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
j_j205 0:fd72f6df078c 69 };
j_j205 0:fd72f6df078c 70
j_j205 0:fd72f6df078c 71 std::vector<std::vector<graphNode> > graph;
j_j205 2:17bd430aeca1 72 std::vector<float> minDistance;
j_j205 0:fd72f6df078c 73 std::vector<int> previous;
j_j205 6:d2da4d4b5112 74 std::stack<uint8_t> route;
j_j205 0:fd72f6df078c 75 };
j_j205 0:fd72f6df078c 76
j_j205 4:a5d44517c65c 77 #endif // NAVIGATION_H