Texas State IEEE / navigation

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   R5 2016 Robotics Team 1

navigation.h@6:d2da4d4b5112, 2016-04-06 (annotated)

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