view main/robots/odr/AvoidBySonarTask.cpp @ 221:8e6ca2ead01a main

Latest updates.
author Bob Cook <bob@bobcookdev.com>
date Mon, 09 Sep 2013 20:03:31 -0700
parents e23198d929e9
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/AvoidBySonarTask.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Virtual base class for a subsumption task object.
//
//  Copyright (c) 2011-2013 Bob Cook
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
// ----------------------------------------------------------------------------------------

#include "AvoidBySonarTask.h"

#include <Poco/Logger.h>
#include <Poco/NumberFormatter.h>

#include "MotorsAndServos.h"
#include "Scoreboard.h"
#include "Sonar.h"

// ----------------------------------------------------------------------------------------

static const unsigned CENTER_AVOIDANCE_ZONE = 0x2000 / 300;
static const unsigned CENTER_MAX_TURN_ZONE  = 0x1000 / 300;

static const unsigned SIDE_AVOIDANCE_ZONE  = 0x1800 / 300;
static const unsigned SIDE_MAX_TURN_ZONE   = 0x1000 / 300;

static const unsigned TRAPPED_ZONE = 0x0700 / 300;

// ----------------------------------------------------------------------------------------

static int8_t CalculateTurnFromCenterSensor( unsigned sensorValue )
{
    if ( sensorValue >= CENTER_AVOIDANCE_ZONE )
    {
        return 0; // no turn
    }

    if ( sensorValue <= CENTER_MAX_TURN_ZONE )
    {
        return 12; // max turn
    }

    // the wheels have a range of -12 to 12, but we are only calculating the positive
    // angle - the caller will figure out the rest

    //const unsigned turnPerSensorInc = ( CENTER_AVOIDANCE_ZONE - CENTER_MAX_TURN_ZONE ) / 12;
    unsigned rawTurn = ( CENTER_AVOIDANCE_ZONE - sensorValue );// / turnPerSensorInc;

    if ( rawTurn > 12 )
    {
        return 12;
    }

    return static_cast< int8_t >( rawTurn );
}

// ----------------------------------------------------------------------------------------

static int8_t CalculateTurnFromSideSensor( unsigned sensorValue )
{
    if ( sensorValue >= SIDE_AVOIDANCE_ZONE )
    {
        return 0; // no turn
    }

    if ( sensorValue <= SIDE_MAX_TURN_ZONE )
    {
        return 12; // max turn
    }

    // the wheels have a range of -12 to 12, but we are only calculating the positive
    // angle - the caller will figure out the rest

    //const unsigned turnPerSensorInc = ( SIDE_AVOIDANCE_ZONE - SIDE_MAX_TURN_ZONE ) / 12;
    unsigned rawTurn = ( SIDE_AVOIDANCE_ZONE - sensorValue ); // / turnPerSensorInc;

    if ( rawTurn > 12 )
    {
        return 12;
    }

    return static_cast< int8_t >( rawTurn );
}

// ----------------------------------------------------------------------------------------

AvoidBySonarTask::AvoidBySonarTask( const std::string& loggerName )
    : TaskObject( loggerName ),
      m_frontLeft( 0 ),
      m_frontCenterLeft( 0 ),
      m_frontCenterRight( 0 ),
      m_frontRight( 0 ),
      m_isAvoiding( false )
{
}

// ----------------------------------------------------------------------------------------

AvoidBySonarTask::~AvoidBySonarTask()
{
}

// ----------------------------------------------------------------------------------------

void AvoidBySonarTask::update()
{
    if ( Scoreboard::isSonarFrontAlive() && ! Scoreboard::isSonarFrontEnabled() )
    {
        Sonar::setFrontEnabled();
    }

    m_frontLeft        = Scoreboard::sonarFrontLeft();
    m_frontCenterLeft  = Scoreboard::sonarFrontCenterLeft();
    m_frontCenterRight = Scoreboard::sonarFrontCenterRight();
    m_frontRight       = Scoreboard::sonarFrontRight();
}

// ----------------------------------------------------------------------------------------

bool AvoidBySonarTask::wantsControl()
{
    if ( m_frontLeft < SIDE_AVOIDANCE_ZONE
                || m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
                || m_frontCenterRight < CENTER_AVOIDANCE_ZONE
                || m_frontRight < SIDE_AVOIDANCE_ZONE )
    {
        if ( ! m_isAvoiding )
        {
            m_isAvoiding = true;

            log().information(
                Poco::Logger::format(
                    "AvoidBySonarTask: now avoiding: $0 $1 $2 $3",
                    Poco::NumberFormatter::formatHex( m_frontLeft ),
                    Poco::NumberFormatter::formatHex( m_frontCenterLeft ),
                    Poco::NumberFormatter::formatHex( m_frontCenterRight ),
                    Poco::NumberFormatter::formatHex( m_frontRight ) ) );
        }
    }
    else
    {
        if ( m_isAvoiding )
        {
            m_isAvoiding = false;

            log().information(
                Poco::Logger::format(
                    "AvoidBySonarTask: no longer avoiding: $0 $1 $2 $3",
                    Poco::NumberFormatter::formatHex( m_frontLeft ),
                    Poco::NumberFormatter::formatHex( m_frontCenterLeft ),
                    Poco::NumberFormatter::formatHex( m_frontCenterRight ),
                    Poco::NumberFormatter::formatHex( m_frontRight ) ) );
        }
    }

    return m_isAvoiding;
//    return ( m_frontLeft < SIDE_AVOIDANCE_ZONE
//                || m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
//                || m_frontCenterRight < CENTER_AVOIDANCE_ZONE
//                || m_frontRight < SIDE_AVOIDANCE_ZONE );
}

// ----------------------------------------------------------------------------------------

bool AvoidBySonarTask::isTrapped() const
{
    return ( m_frontCenterLeft < TRAPPED_ZONE && m_frontCenterRight < TRAPPED_ZONE );
}

// ----------------------------------------------------------------------------------------

void AvoidBySonarTask::takeControl()
{
    Scoreboard::sendManagerMessage( "Avoiding" );

    if ( isTrapped() )
    {
        MotorsAndServos::motorSpeeds( 0, 0 );
        return; // stuck!
    }

    int8_t adjustedSpeed = Scoreboard::navMaximumSpeed();

    if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
                                         && m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
    {
        if ( m_frontLeft < m_frontRight )
        {
            int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
            MotorsAndServos::servoPositions( -turn, turn );
            adjustedSpeed -= ( 2 * turn );
        }
        else
        {
            int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
            MotorsAndServos::servoPositions( turn, -turn );
            adjustedSpeed -= ( 2 * turn );
        }
    }
    else if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE )
    {
        int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterLeft );
        MotorsAndServos::servoPositions( -turn, turn );
        adjustedSpeed -= turn;
    }
    else if ( m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
    {
        int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterRight );
        MotorsAndServos::servoPositions( turn, -turn );
        adjustedSpeed -= turn;
    }
    else if ( m_frontLeft < SIDE_AVOIDANCE_ZONE )
    {
        int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
        MotorsAndServos::servoPositions( -turn, turn );
        adjustedSpeed -= turn;
    }
    else if ( m_frontRight < SIDE_AVOIDANCE_ZONE )
    {
        int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
        MotorsAndServos::servoPositions( turn, -turn );
        adjustedSpeed -= turn;
    }
    else
    {
        MotorsAndServos::servoPositions( 0, 0 ); // unexpected...
    }

    if ( adjustedSpeed < 5 ) { adjustedSpeed = 5; }
    MotorsAndServos::motorSpeeds( adjustedSpeed, adjustedSpeed );
}

// ----------------------------------------------------------------------------------------