EMC Group 2 - 2015 _ Navigation Class

navigationClass_simplified_05

/*!
********************************************************************************
\file
\author   Garbi Singla - Group 2, EMC 2015
\date
\last revision
\version  0.5
\project  Navigation
\distrib  Internal

\brief  This file contains the main function of the program.
********************************************************************************
*/

#include "navigation.h"

/*!
 ********************************************************************************
 \brief Constructor
 ********************************************************************************
 */
Navigation::Navigation()
{
    ////Initialize parameters ////
    k = K;              		//Proporcional control for Drive
    range_max = RANGE_MAX;		//Range max for PF
    safetyLim = SLMIT;    		//Safety limit
    DefaultOpening.angle = DO_AGLE; 	//Default Opening, angle
    DefaultOpening.radius = DO_radius;	//Default Opening,
    DefaultOpening.width = DO_WIDTH;	//Default Opening,

    ////Initialize Flags ////
    flag_180 = 0;       //Initialize flag for 180-turn
    flag_turnControl = TURN_CONTROL;
    flag_left = 0;
    flag_right = 0;
    flag_fwd = 0;
    lastDir = 0;        //Last direction of rotation
    safetyMode = 0;    //Safety Boolean
    actionCompleted = 0;
    lastMovement = 0;
}

/*!
 ********************************************************************************
 \brief Calculates the Potential Fields. Attraction Field from the Target
        position (rhoTarget & thethaTarget) and Repulsive Field from laser data.
 \param LaserData	sensor values, LRF
 \param opening		structure with detected openings
 ********************************************************************************
 */
double Navigation::estPotFields(emc::LaserData& scan, Detection::opening target)
{
    /* Declare local variables*/
    ...

    //Initialize accumulators
    ...

    //Caluculate Attractive Field: Target___________________________________

    //1. Changing coordinates
    ...
    //2. Calculates gradient  (f=r²)
    (e.g.) 	gxt=2*WT*xt; //df/dx
    ...

    //Caluculate Repulsive Field: Objects__________________________________

    for(i=PF_firstAng; i<scan.ranges.size()-PF_firstAng;i++)
    {
        //1. checks for valid data
        ...
        //2. Changing coordinates
        ...
        //3. Check safety limit
        ...
        //4. Calculates Gradient  (f=1/r)
        (e.g.) 	gxo-=WO*xo*sqrt(pow(1/(pow(xo,2)+pow(yo,2)),3)); //df/dx
        ...
    }

    //Total gradient
    (e.g.) 	gradient.x=gxt+gxo;
    ...

    //Direction
    (e.g.) 	gradient.x=gradient.x/(sqrt(pow(gradient.x,2)+pow(gradient.y,2)));
    ...


    //Generate output (correct atan)
    ...

    //Save last angle
    lastTarget = target;
    lastAngle = target.angle;

    return direction;
}

/*!
 ********************************************************************************
 \brief Coordinator function, high level function of Robot Navigation.
 \param IO
 \param	LaserData
 \param Detection
 \param int - movement
 ********************************************************************************
 */
int Navigation::Coordinator(emc::IO& io, emc::LaserData& scan,  Detection& pEnv, int movement)
{
    /* Declare local variables*/
    ...

    //Safety Check

    if(safetyMode) movement = -1;  //Safety check


    //FSM with different actions

    if (actionCompleted == 0)
    {

        switch (movement)
        {
        case 1: ///FORWARD///
            ...

        case 2:  ///LEFT///
            //1. Detects angle to follow
            //2. Track previous angle
            //3. Check if turn has ended
            //4. Set flag for action completed
            ...

        case 3: ///RIGHT///
            ...

        case 4: ///Turn 180///
            ...

        case 5: ///DOOR///
            //1. Stop inside Door Area (1st time)
            //2. Send open door request
            //3. Turn Right till heading the opening or continue turning
            //4. Send action completed
            ...

        case 10: ///Automatic-Forward///
            //Automatic forward movement using a fixed target (center of bot)

        default: ///STOP///
            Drive(io,0,'s');
            break;

        }

        ///------ Safety algorithm ------///
        if (movement == -1)
        {
            ... //Safety algorithm procedure
        }

    }

    /*!
 ********************************************************************************
 \brief Low level Drive function
 \param IO
 \param dir
 \param command
 ********************************************************************************
 */
    void Navigation::Drive(emc::IO& io,double dir, char command)
    {

        ///%%%% Progressive acceleration and max velocity check %%%%///
        ...

        ///%%%% Progressive velocity when turning %%%%///
        ...


        ///%%%% Check maximum rotation vel %%%%///
        ...

        ///%%%% FSM %%%%///

        switch (command){
        case 'f': //movement forward
            ...
            break;
        case 'r': //rotates to find openings
            ...
            break;
        case 'l': //slow movement
            ...
            break;
        case 'o': //rotate to turn 180
            ...
            break;
        case 's': //stop
            ...
            break;
        case 'e': // Safety, drive slow forward
            ...
            break;
        default:
            break;
        }


        lastDir = dir;  //Save direction of rotation
    }

    /*!
 ********************************************************************************
 \brief Destructor
 ********************************************************************************
 */
    Navigation::~Navigation()
    {

    }