File content as of revision 12:6d37ea71da65:

#include "StepperDrive.h"
#include "LongRangeSensor.h"
#include "navigation.h"
#include "stdint.h"
#include <cmath>
#include <fstream>
#include <stack>
#include <set>

// FUNCTION:
//      Navigation(int size = DEFAULT_SIZE)
// IN-PARAMETERS:
//      Number of initial vertices (DEFAULT = 1)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Default and 1-parameter constructor.
Navigation::Navigation(Serial &_pc, StepperDrive &_drive, 
    LongRangeSensor &_longRangeL, LongRangeSensor &_longRangeR, DigitalOut &_led_red, DigitalOut &_led_green,
    int size = DEFAULT_SIZE) : pc(_pc), drive(_drive), 
    longRangeL(_longRangeL), longRangeR(_longRangeR), led_red(_led_red), led_green(_led_green),
    vertex(DEFAULT_VERTEX), angle(DEFAULT_ANGLE)
{
    graph.resize(size);
    distLocalL = 0.0;
}

// FUNCTION:
//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
//                   uint16_t angle)
// IN-PARAMETERS:
//      Source node (uint16_t), target node (uint16_t), edge distance
//      (uint16_t), and angle (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Adds a graph node to the graph.
void Navigation::addGraphNode(uint8_t src, uint8_t target, float dist,
                                 float angle)
{
    graph[src].push_back(graphNode(target, dist, angle));
}

// FUNCTION:
//      addGraphNode(uint16_t src, uint16_t target, uint16_t dist,
//                   uint16_t angle)
// IN-PARAMETERS:
//      Source node (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Adds a graph node to the graph, with target, dist, and angle
//      initialized to infinity
void Navigation::addGraphNode(uint8_t src)
{
    graph[src].push_back(graphNode());
}

// FUNCTION:
//      int loadMap(char* infile);
// IN-PARAMETERS:
//      Input file name (*char[])
// OUT-PARAMETERS:
//      Returns 1 if file doesn't open
//      Returns 0 if file opens succesfully
// DESCRIPTION:
//      Loads contents of a file as a weighted graph. First entry in the file
//      should be the number of vertices. All subsequent entries indicate edges
//      by listing a source vertex, a target vertex, the distance between them
//      and the angle from source to target (0 - 359 degrees) . Only edges
//      are listed. Assumes that inverse angles/edges are included in file.
int Navigation::loadMap(char* inputFile)
{
    std::ifstream inFile;
    inFile.open(inputFile);
    if (!inFile)
        return 1; // returns 1 if file fails to open

    int vertices;
    inFile >> vertices; // reads first entry as number of vertices
    graph.resize(vertices); // resizes vector as necessary

    uint8_t src;
    uint8_t target;
    float dist;
    float angle;

    while (!inFile.eof() )
    {
        inFile >> src;
        inFile >> target;
        inFile >> dist;
        inFile >> angle;

        addGraphNode(src, target, dist, angle);
    } // loads all data from file into graph

    return 0;
}

// FUNCTION:
//      void getShortestPath(uint16_t src, uint16_t destination)
// IN-PARAMETERS:
//      Target vertex (uint16_t)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Implementation of Dijkstra's algorithm. Uses current vertex as source
//      and calculates shortest path information for all other vertices.
//      Subroutine ends when shortest path for target vertex has been found
//      and then loads the path into route.
void Navigation::getShortestPath(uint8_t destination)
{
    uint8_t src = vertex;
    uint8_t v; // stores temporary vertex
    int n = graph.size();
    minDistance.clear();
    minDistance.resize(n, MAX_DIST);
    minDistance[src] = 0;
    previous.clear();
    previous.resize(n, -1);
    std::set<std::pair<float, uint8_t> > vertex_queue;
    vertex_queue.insert(std::make_pair(minDistance[src], src));

    while (!vertex_queue.empty())
    {
        float dist = vertex_queue.begin()->first;
        uint8_t u = vertex_queue.begin()->second;
        vertex_queue.erase(vertex_queue.begin());

        // Visit each edge exiting u
        const std::vector<graphNode> &neighbors = graph[u];
        for (std::vector<graphNode>::const_iterator neighbor_iter = neighbors.begin();
             neighbor_iter != neighbors.end();
             neighbor_iter++)
        {
            v = neighbor_iter->neighbor;
            float weight = neighbor_iter->distance;
            float distance_through_u = dist + weight;

            if (distance_through_u < minDistance[v])
            {
                vertex_queue.erase(std::make_pair(minDistance[v], v));

                minDistance[v] = distance_through_u;
                previous[v] = u;
                vertex_queue.insert(std::make_pair(minDistance[v], v));
            }
            if (v == destination)
                break;
        }
        if (v == destination)
            break;
    }
    while (v != src)
    {
        route.push(v);
        v = previous[v];
    }
}

// FUNCTION:
//      void executeRoute()
// IN-PARAMETERS:
//      Address of left and right steppers (StepperMotor &)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Sends command to steppers to execute route.
void Navigation::executeRoute()
{
    uint8_t target;
    float targetAngle;
    float targetDist;
    float differenceAngle;

    while (!route.empty() )
    {
        target = route.top();
        route.pop();

        for (int i = 0; i < graph[vertex].size(); i++)
        {
            if (graph[vertex][i].neighbor == target)
            {
                targetAngle = graph[vertex][i].angle;
                targetDist = graph[vertex][i].distance;
                break;
            }
        }

        if (targetAngle != angle)
        {
            differenceAngle = targetAngle - angle;

            if (abs(differenceAngle) > 180)
            {
                if (differenceAngle < 0)
                    differenceAngle = 360 - abs(differenceAngle);
                else
                    differenceAngle = -(360 - differenceAngle);
            }

            else
            {
                /* do nothing */
            }

            /* send -(differenceAngle) to motors */
            int returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) );
            // wait for move to complete
            while(!drive.isMoveDone())
            {
                wait(1e-6);
            }
        }

        /* send targetDist to motors */
        drive.move(targetDist, 0);
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }

        vertex = target;
        angle = targetAngle;
        
        distLocalL = longRangeL.distInchesLOne();
        distLocalR = longRangeR.distInchesROne();
             
        if (distLocalL <= 18.0) // wall in range
        {
            newLocalizeRight();
        }
            
        else if (distLocalR <= 13.0) // wall in range
        {
            newLocalizeLeft();
        }
    }
}

// utility function
void Navigation::printPrevious(Serial &pc)
{
    for(int i = 0; i < previous.size(); i++)
    {
        pc.printf("Node %f is prev to node %f\n", previous[i], i);
    }
}

// utility function
void Navigation::printRoute(Serial &pc)
{
    while (!route.empty() )
    {
        pc.printf("Go to node %i\n", route.top() );
        route.pop();
    }
}

// utility function
void Navigation::printGraph(Serial &pc)
{
    for (int i = 0; i < graph.size(); i++)
        for (int j = 0; j < graph[i].size(); j++)
        {
            pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle);
        }
}

// FUNCTION:
//      void localizeRight()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeL this method will localize to the wall
//      to the right
void Navigation::localizeRight()
{   
    if (distLocalL >= 7.8) // very close left, turn more right
    {
        drive.move(0, (5.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
        
    else if (distLocalL >= 6.9) // too close left, turn right
    {
        drive.move(0, (2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else if (distLocalL <= 5.5) // too close right, turn left
    {
        drive.move(0, (-2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else
    {
        /* do nothing */
    }
}

// FUNCTION:
//      void localizeLeft()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeR this method will localize to the wall
//      to the left during stretches of forward motion where robot 
//      should remain xxx distance from the left wall. 
void Navigation::localizeLeft()
{   
    if (distLocalR >= 9.7) // too close right, turn left
    {
        drive.move(0.0, (-2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else if (distLocalR <= 8.3) // too close left, turn right
    {
        drive.move(0.0, (2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else
    {
        /* do nothing */
    }
}

// FUNCTION:
//      void localizeRightReverse()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeL this method will localize to the wall
//      to the right
void Navigation::localizeRightReverse()
{   
    if (distLocalL >= 7.2) // too close left, turn left
    {
        drive.move(0, (-2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else if (distLocalL <= 6.2) // too close right, turn right
    {
        drive.move(0, (2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else
    {
        /* do nothing */
    }
}

// FUNCTION:
//      void localizeLeftReverse()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Using sensor longRangeR this method will localize to the wall
//      to the left during stretches of forward motion where robot 
//      should remain xxx distance from the left wall. 
void Navigation::localizeLeftReverse()
{   
    if (distLocalR >= 9.7) // toos close right, turn right
    {
        drive.move(0.0, (2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else if (distLocalR <= 8.3) // too close left, turn left
    {
        drive.move(0.0, (-2.0)*(3.14159 / 180.0));
        // wait for move to complete
        while(!drive.isMoveDone())
        {
            wait(1e-6);
        }
    }
    
    else
    {
        /* do nothing */
    }
}

// FUNCTION:
//      void newLocalize()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Uses difference between sensors to adjust angle
void Navigation::newLocalize()
{   
    float k = 2.5;
    float angleAdjust = k*(distLocalL - distLocalR);
    
    drive.move(0, angleAdjust*(PI / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    angle += angleAdjust;
}

// FUNCTION:
//      void newLocalizeRight()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Left sensor to adjust off right wall
void Navigation::newLocalizeRight()
{   
    float k = 2.0;
        
    float angleAdjust = k*(distLocalL - 13.0);
    
    drive.move(0, angleAdjust*(PI / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    //angle += (-1*angleAdjust);
}

// FUNCTION:
//      void newLocalizeLeft()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Right sensor to adjust off Left wall
void Navigation::newLocalizeLeft()
{   
    float k = 3.5; //  1.125
    float angleAdjust = k*(9.1 - distLocalR);
    
    drive.move(0, angleAdjust*(PI / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    //angle += (-1*angleAdjust);
}

// FUNCTION:
//      void newLocalizeLeftReverse()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Right sensor to adjust off Left wall
void Navigation::newLocalizeLeftReverse()
{   
    float k = 3.5;
    float angleAdjust = k*(distLocalR - 9.1);
    
    drive.move(0, angleAdjust*(PI / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    //angle += (-1*angleAdjust);
}

// FUNCTION:
//      void newLocalizeRightReverse()
// IN-PARAMETERS:
//      None
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Left sensor to adjust off right wall
void Navigation::newLocalizeRightReverse()
{   
    float k = 3.0;
        
    float angleAdjust = k*(13.0 - distLocalL);
    
    drive.move(0, angleAdjust*(PI / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    //angle += (-1*angleAdjust);
}