File content as of revision 12:6d37ea71da65:

#ifndef NAVIGATION_H
#define NAVIGATION_H
#include "StepperDrive.h"
#include "LongRangeSensor.h"
#include "stdint.h"
#include "mbed.h"
#include <vector>
#include <stack>

class Navigation
{
public:
    Navigation(Serial &_pc, StepperDrive &_drive, LongRangeSensor 
        &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green, int size);
    void addGraphNode(uint8_t src, uint8_t target, float dist,
        float angle);
    void addGraphNode(uint8_t src);
    int graphSize() { return graph.size(); }
    uint8_t getVertex() { return vertex; }
    float getAngle() { return angle; }
    void setVertex(uint8_t target) { vertex = target; }
    void setAngle(float target) { angle = target; }
    int numNeighbors(int src) { return graph[src].size(); }
    int loadMap(char* inputFile);
    void getShortestPath(uint8_t destination);
    void executeRoute();
    void localizeRight();
    void localizeLeft();
    void localizeRightReverse();
    void localizeLeftReverse();
    void newLocalizeRight();
    void newLocalizeLeft();
    void newLocalizeRightReverse();
    void newLocalizeLeftReverse();
    void newLocalize();
    
    // utility functions
    float getMinDist(uint8_t target) { return minDistance[target]; }
    void printPrevious(Serial &pc);
    void printRoute(Serial &pc);
    void printGraph(Serial &pc);

private:
    static const float MAX_DIST = 65535; // infinity
    static const uint8_t DEFAULT_VERTEX = 0;
    static const float DEFAULT_ANGLE = 0.0;
    static const int DEFAULT_SIZE = 1;
    //static const int STEPS_PER_INCH = 20; //  per wheel diameter and stepper specs
    static const float PI = 3.14159;
    Serial &pc;
    StepperDrive &drive;
    LongRangeSensor &longRangeL;
    LongRangeSensor &longRangeR;
    DigitalOut &led_red;
    DigitalOut &led_green;
    float distLocalL;
    float distLocalR;

    uint8_t vertex; // current vertex
    float angle; // current angle

    struct graphNode
    {
        uint8_t neighbor;
        float distance; // in inches
        float angle;
        graphNode(float arg_neighbor = MAX_DIST,
                  float arg_distance = MAX_DIST,
                  float arg_angle = MAX_DIST)
           : neighbor(arg_neighbor), distance(arg_distance), angle(arg_angle){}
    };

    std::vector<std::vector<graphNode> > graph;
    std::vector<float> minDistance;
    std::vector<int> previous;
    std::stack<uint8_t> route;
};

#endif // NAVIGATION_H