view main/robots/odr-sim/SonarFrontStatus.cpp @ 234:5657cf2325c9 main

Updates to the simulator. Minor tweaks to the controls.
author Bob Cook <bob@bobcookdev.com>
date Sun, 13 Jul 2014 09:13:15 -0700
parents 5e91f5877f1f
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr-sim/SonarFrontStatus.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Thread that periodically sends the front sonar pod "heartbeat" message.
//
//  Copyright (c) 2011 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 "SonarFrontStatus.h"

#include "SimDisplay.h"

#include "packages/common/can/can_helpers.h"
#include "packages/common/can/can_messages.h"
#include "packages/common/can/can_nodes.h"

#include "packages/linux/can/CANMessage.h"

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

static void SendSonarFrontStatusMessage()
{
    uint32_t msgid;

    if ( SonarFrontStatus::isSonarFrontEnabled() )
    {
        msgid = can_build_message_id( can_node_odr_sonar_front,
                                      can_node_broadcast,
                                      can_dataid_sonar_front_state_enabled );
    }
    else
    {
        msgid = can_build_message_id( can_node_odr_sonar_front,
                                      can_node_broadcast,
                                      can_dataid_sonar_front_state_disabled );
    }

    CANMessage::QueueToSend( new CANMessage( msgid ) );
}

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

static void SendSonarFrontValueMessage()
{
    uint32_t msgid = can_build_message_id( can_node_odr_sonar_front,
                                           can_node_broadcast,
                                           can_dataid_sonar_front );

    can_data_sonar_front data;

    data.left         = SonarFrontStatus::sonarFrontLeft();
    data.center_left  = SonarFrontStatus::sonarFrontCenterLeft();
    data.center_right = SonarFrontStatus::sonarFrontCenterRight();
    data.right        = SonarFrontStatus::sonarFrontRight();

    CANMessage::QueueToSend( new CANMessage( 
                msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) ) );
}

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

Poco::RWLock SonarFrontStatus::sm_rwLock;
bool         SonarFrontStatus::sm_isEnabled   = false;
uint8_t      SonarFrontStatus::sm_left        = 60;
uint8_t      SonarFrontStatus::sm_centerLeft  = 60;
uint8_t      SonarFrontStatus::sm_centerRight = 60;
uint8_t      SonarFrontStatus::sm_right       = 60;

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

bool SonarFrontStatus::isSonarFrontEnabled()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isEnabled;
}

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

void SonarFrontStatus::setSonarFrontEnabled()
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isEnabled = true;
}

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

void SonarFrontStatus::setSonarFrontDisabled()
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isEnabled = false;
}

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

uint8_t SonarFrontStatus::sonarFrontLeft()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_left;
}

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

uint8_t SonarFrontStatus::sonarFrontCenterLeft()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_centerLeft;
}

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

uint8_t SonarFrontStatus::sonarFrontCenterRight()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_centerRight;
}

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

uint8_t SonarFrontStatus::sonarFrontRight()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_right;
}

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

void SonarFrontStatus::setSonarFrontLeft( uint8_t value )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_left = value;
}

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

void SonarFrontStatus::setSonarFrontCenterLeft( uint8_t value )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_centerLeft = value;
}

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

void SonarFrontStatus::setSonarFrontCenterRight( uint8_t value )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_centerRight = value;
}

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

void SonarFrontStatus::setSonarFrontRight( uint8_t value )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_right = value;
}

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

SonarFrontStatus::SonarFrontStatus()
    : Poco::Runnable(),
      m_quitEvent( true /* auto-reset */ )
{
}

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

SonarFrontStatus::~SonarFrontStatus()
{
}

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

void SonarFrontStatus::timeToQuit()
{
    m_quitEvent.set();
}

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

void SonarFrontStatus::run()
{
    int loopCount = 0;

    for ( ;; )
    {
        try
        {
            if ( loopCount == 0 )   // every 2.5 seconds...
            {
                SendSonarFrontStatusMessage();
            }

            if ( isSonarFrontEnabled() )
            {
                SendSonarFrontValueMessage();
            }

            loopCount++;
            loopCount %= 5;   // 0..5

        }
        catch ( ... )
        {
            // nothing to do, but don't stop the thread
        }

        if ( m_quitEvent.tryWait( 500 ) ) // 500ms = 1/2 second
        {
            return;
        }
    }
}

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