Texas State IEEE / navigation

navigation updated with completed dijkstra's algo

Dependents:   R5 2016 Robotics Team 1

navigation.h@8:290a110bcf0e, 2016-04-08 (annotated)

Committer:
j_j205
Date:
Fri Apr 08 04:35:44 2016 +0000
Revision:
8:290a110bcf0e
Parent:
7:f7489797746b 
latest

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 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