view main/robots/odr-sim/SimDisplay.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 783f69f37c64
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr-sim/SimDisplay.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Window display for the ODR platform simulator.
//
//  Copyright (c) 2011-2014 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 "SimDisplay.h"

#include <memory>
#include <string>

#include <FL/Fl.H>
#include <FL/Fl_Box.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_Dial.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Hor_Slider.H>
#include <FL/Fl_Light_Button.H>
#include <FL/Fl_Line_Dial.H>
#include <FL/Fl_Output.H>
#include <FL/Fl_Slider.H>
#include <FL/Fl_Toggle_Button.H>

#include <Poco/Thread.h>

#include "DisplayStatus.h"
#include "GpsStatus.h"
#include "ImuStatus.h"
#include "MotionStatus.h"
#include "SonarFrontStatus.h"

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

static void DoDisplayStatusCB( Fl_Widget* widget, void* value )
{
    static DisplayStatus s_dsRunnable;
    static Poco::Thread  s_dsThread;

    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
    if ( button == 0 )
    {
        return; // oops, not what we thought?!
    }

    if ( button->value() == 0 )
    {
        // off
        s_dsRunnable.timeToQuit();
        s_dsThread.join();
    }
    else
    {
        // on
        s_dsThread.start( s_dsRunnable );
    }
}

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

static void DoGpsStatusCB( Fl_Widget* widget, void* value )
{
    static GpsStatus    s_gsRunnable;
    static Poco::Thread s_gsThread;

    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
    if ( button == 0 )
    {
        return; // oops, not what we thought?!
    }

    if ( button->value() == 0 )
    {
        // off
        s_gsRunnable.timeToQuit();
        s_gsThread.join();
    }
    else
    {
        // on
        s_gsThread.start( s_gsRunnable );
    }
}

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

static void DoMotionStatusCB( Fl_Widget* widget, void* value )
{
    static MotionStatus s_msRunnable;
    static Poco::Thread s_msThread;

    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
    if ( button == 0 )
    {
        return; // oops, not what we thought?!
    }

    if ( button->value() == 0 )
    {
        // off
        s_msRunnable.timeToQuit();
        s_msThread.join();
    }
    else
    {
        // on
        s_msThread.start( s_msRunnable );
    }
}

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

static void ImuKeepAliveCB( Fl_Widget* widget, void* value )
{
    static ImuStatus    s_imuStatusRunnable;
    static Poco::Thread s_imuStatusThread;

    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
    if ( button == 0 )
    {
        return; // opps, not what we thought?!
    }

    if ( button->value() == 0 )
    {
        // off
        s_imuStatusRunnable.timeToQuit();
        s_imuStatusThread.join();
    }
    else
    {
        // on
        s_imuStatusThread.start( s_imuStatusRunnable );
    }
}

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

static void ImuHeadingDialCB( Fl_Widget* widget, void* value )
{
    Fl_Dial* dial = dynamic_cast< Fl_Dial* >( widget );
    if ( dial == 0 )
    {
        return; // opps, not what we thought?!
    }

    ImuStatus::setImuYaw( dial->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateImuValues();
    }
}

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

static void ImuPitchSliderCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    ImuStatus::setImuPitch( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateImuValues();
    }
}

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

static void ImuRollSliderCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    ImuStatus::setImuRoll( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateImuValues();
    }
}

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

static void SonarFrontKeepAliveCB( Fl_Widget* widget, void* value )
{
    static SonarFrontStatus s_sfsRunnable;
    static Poco::Thread     s_sfsThread;

    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
    if ( button == 0 )
    {
        return; // opps, not what we thought?!
    }

    if ( button->value() == 0 )
    {
        // off
        s_sfsRunnable.timeToQuit();
        s_sfsThread.join();
    }
    else
    {
        // on
        s_sfsThread.start( s_sfsRunnable );
    }
}

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

static void SonarFrontSliderLeftCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    SonarFrontStatus::setSonarFrontLeft( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateSonarFrontValues();
    }
}

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

static void SonarFrontSliderCenterLeftCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    SonarFrontStatus::setSonarFrontCenterLeft( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateSonarFrontValues();
    }
}

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

static void SonarFrontSliderCenterRightCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    SonarFrontStatus::setSonarFrontCenterRight( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateSonarFrontValues();
    }
}

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

static void SonarFrontSliderRightCB( Fl_Widget* widget, void* value )
{
    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
    if ( slider == 0 )
    {
        return; // opps, not what we thought?!
    }

    SonarFrontStatus::setSonarFrontRight( slider->value() );

    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
    if ( theDisplay )
    {
        theDisplay->updateSonarFrontValues();
    }
}

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

SimDisplay::SimDisplay()
    : m_window( 0 ),
      m_dialWheelSpeedFront( 0 ),
      m_textWheelSpeedFront( 0 ),
      m_dialWheelSpeedRear( 0 ),
      m_textWheelSpeedRear( 0 ),
      m_sliderWheelPosFront( 0 ),
      m_textWheelPosFront( 0 ),
      m_sliderWheelPosRear( 0 ),
      m_textWheelPosRear( 0 ),
      m_boxMgrHeartbeat( 0 ),
      m_textManagerMsg( 0 ),
      m_boxSonarFrontState( 0 ),
      m_buttonDisplayAlive( 0 ),
      m_buttonGpsAlive( 0 ),
      m_buttonMotionAlive( 0 ),
      m_buttonA( 0 ),
      m_buttonB( 0 ),
      m_buttonSonarFrontKA( 0 ),
      m_sliderSonarFrontL( 0 ),
      m_sliderSonarFrontCL( 0 ),
      m_sliderSonarFrontCR( 0 ),
      m_sliderSonarFrontR( 0 ),
      m_textSonarFront( 0 ),
      m_timerMainHbTimeout()
{
}

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

SimDisplay::~SimDisplay()
{
    delete m_window; // this will delete all contained widgets
}

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

void SimDisplay::createWindow()
{
    m_window = new Fl_Double_Window( 425, 500, "ODR Simulator" );
    if ( m_window )
    {
        //m_window->color( FL_WHITE );
    }
}

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

void SimDisplay::createWheelSpeedDials()
{
    m_window->begin();

    // front
    
    Fl_Box* labelWheelSpeedFront = new Fl_Box( 10, 10, 75, 20 );
    if ( labelWheelSpeedFront )
    {
        labelWheelSpeedFront->box( FL_NO_BOX );
        labelWheelSpeedFront->align( FL_ALIGN_CENTER );
        labelWheelSpeedFront->label( "Front Speed" );
    }

    m_dialWheelSpeedFront = new Fl_Line_Dial( 10, 35, 75, 75 );
    if ( m_dialWheelSpeedFront )
    {
        m_dialWheelSpeedFront->box( FL_ROUND_UP_BOX );
        m_dialWheelSpeedFront->bounds( -128.0, 128.0 );
        m_dialWheelSpeedFront->set_output();
    }

    m_textWheelSpeedFront = new Fl_Box( 10, 115, 75, 20 );
    if ( m_textWheelSpeedFront )
    {
        m_textWheelSpeedFront->box( FL_THIN_DOWN_BOX );
        m_textWheelSpeedFront->align( FL_ALIGN_CENTER );
        m_textWheelSpeedFront->label( "0" );
    }

    // rear

    Fl_Box* labelWheelSpeedRear = new Fl_Box( 100, 10, 75, 20 );
    if ( labelWheelSpeedRear )
    {
        labelWheelSpeedRear->box( FL_NO_BOX );
        labelWheelSpeedRear->align( FL_ALIGN_CENTER );
        labelWheelSpeedRear->label( "Rear Wheels" );
    }

    m_dialWheelSpeedRear = new Fl_Line_Dial( 100, 35, 75, 75 );
    if ( m_dialWheelSpeedRear )
    {
        m_dialWheelSpeedRear->box( FL_ROUND_UP_BOX );
        m_dialWheelSpeedRear->bounds( -128.0, 128.0 );
        m_dialWheelSpeedRear->set_output();
    }

    m_textWheelSpeedRear = new Fl_Box( 100, 115, 75, 20 );
    if ( m_textWheelSpeedRear )
    {
        m_textWheelSpeedRear->box( FL_THIN_DOWN_BOX );
        m_textWheelSpeedRear->align( FL_ALIGN_CENTER );
        m_textWheelSpeedRear->label( "0" );
    }

    m_window->end();
}

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

void SimDisplay::createWheelSliders()
{
    m_window->begin();

    // front
    
    Fl_Box* labelWheelPosFront = new Fl_Box( 10, 150, 75, 20 );
    if ( labelWheelPosFront )
    {
        labelWheelPosFront->box( FL_NO_BOX );
        labelWheelPosFront->align( FL_ALIGN_CENTER );
        labelWheelPosFront->label( "Front Position" );
    }

    m_sliderWheelPosFront = new Fl_Hor_Slider( 10, 175, 75, 20 );
    if ( m_sliderWheelPosFront )
    {
        m_sliderWheelPosFront->slider( FL_UP_BOX );
        m_sliderWheelPosFront->bounds( -12.0, 12.0 );
        m_sliderWheelPosFront->set_output();
    }

    m_textWheelPosFront = new Fl_Box( 10, 200, 75, 20 );
    if ( m_textWheelPosFront )
    {
        m_textWheelPosFront->box( FL_THIN_DOWN_BOX );
        m_textWheelPosFront->align( FL_ALIGN_CENTER );
        m_textWheelPosFront->label( "0" );
    }

    // rear
   
    Fl_Box* labelWheelPosRear = new Fl_Box( 100, 150, 75, 20 );
    if ( labelWheelPosRear )
    {
        labelWheelPosRear->box( FL_NO_BOX );
        labelWheelPosRear->align( FL_ALIGN_CENTER );
        labelWheelPosRear->label( "Rear Wheel" );
    }

    m_sliderWheelPosRear = new Fl_Hor_Slider( 100, 175, 75, 20 );
    if ( m_sliderWheelPosRear )
    {
        m_sliderWheelPosRear->slider( FL_UP_BOX );
        m_sliderWheelPosRear->bounds( -12.0, 12.0 );
        m_sliderWheelPosRear->set_output();
    }

    m_textWheelPosRear = new Fl_Box( 100, 200, 75, 20 );
    if ( m_textWheelPosRear )
    {
        m_textWheelPosRear->box( FL_THIN_DOWN_BOX );
        m_textWheelPosRear->align( FL_ALIGN_CENTER );
        m_textWheelPosRear->label( "0" );
    }

    m_window->end();
}

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

void SimDisplay::createDisplays()
{
    m_window->begin();

    m_boxMgrHeartbeat = new Fl_Box( 10, 240, 165, 20 );
    if ( m_boxMgrHeartbeat )
    {
        m_boxMgrHeartbeat->box( FL_THIN_DOWN_BOX );
        m_boxMgrHeartbeat->align( FL_ALIGN_CENTER );
        m_boxMgrHeartbeat->label( "Mgr Heartbeat" );
        m_boxMgrHeartbeat->color( FL_RED );
    }

    m_textManagerMsg = new Fl_Box( 10, 265, 165, 20 );
    if ( m_textManagerMsg )
    {
        m_textManagerMsg->box( FL_THIN_DOWN_BOX );
        m_textManagerMsg->align( FL_ALIGN_CENTER );
        m_textManagerMsg->label( "" );
    }

    m_timerMainHbTimeout.setPeriodicInterval( 6000 );
    Poco::TimerCallback< SimDisplay > cb( *this, &SimDisplay::timeoutMgrHeartbeat );
    m_timerMainHbTimeout.start( cb );

    m_boxSonarFrontState = new Fl_Box( 10, 305, 165, 20 );
    if ( m_boxSonarFrontState )
    {
        m_boxSonarFrontState->box( FL_THIN_DOWN_BOX );
        m_boxSonarFrontState->align( FL_ALIGN_CENTER );
        m_boxSonarFrontState->label( "Front Sonar Off" );
        m_boxSonarFrontState->color( FL_BACKGROUND_COLOR );
    }
}

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

void SimDisplay::createActionButtons()
{
    m_window->begin();

    m_buttonDisplayAlive = new Fl_Light_Button( 250, 20, 165, 20 );
    if ( m_buttonDisplayAlive )
    {
        m_buttonDisplayAlive->label( "Display Alive" );
        m_buttonDisplayAlive->selection_color( FL_GREEN );
        m_buttonDisplayAlive->callback( &DoDisplayStatusCB );
    }

    m_buttonGpsAlive = new Fl_Light_Button( 250, 50, 165, 20 );
    if ( m_buttonGpsAlive )
    {
        m_buttonGpsAlive->label( "GPS Alive" );
        m_buttonGpsAlive->selection_color( FL_GREEN );
        m_buttonGpsAlive->callback( &DoGpsStatusCB );
    }

    m_buttonMotionAlive = new Fl_Light_Button( 250, 80, 165, 20 );
    if ( m_buttonMotionAlive )
    {
        m_buttonMotionAlive->label( "Motion Control Alive" );
        m_buttonMotionAlive->selection_color( FL_GREEN );
        m_buttonMotionAlive->callback( &DoMotionStatusCB );
    }

#if 0
    m_buttonA = new Fl_Toggle_Button( 250, 110, 80, 20 );
    if ( m_buttonA )
    {
        m_buttonA->label( "A" );
        m_buttonA->callback( &PressButtonOneCB );
    }

    m_buttonB = new Fl_Toggle_Button( 335, 110, 80, 20 );
    if ( m_buttonB )
    {
        m_buttonB->label( "B" );
        m_buttonB->callback( &PressButtonTwoCB );
    }
#endif

    m_window->end();
}

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

void SimDisplay::createImuControls()
{
    m_window->begin();

    m_buttonImuKA = new Fl_Light_Button( 250, 150, 165, 20 );
    if ( m_buttonImuKA )
    {
        m_buttonImuKA->label( "IMU Alive" );
        m_buttonImuKA->selection_color( FL_GREEN );
        m_buttonImuKA->callback( &ImuKeepAliveCB );
    }

    m_textImuValues = new Fl_Box( 250, 180, 165, 20 );
    if ( m_textImuValues )
    {
        m_textImuValues->box( FL_THIN_DOWN_BOX );
        m_textImuValues->align( FL_ALIGN_CENTER );
        m_textImuValues->label( "0.0  0.0  0.0" );
    }

    m_dialImuHeading = new Fl_Dial( 250, 210, 75, 75 );
    if ( m_dialImuHeading )
    {
        m_dialImuHeading->box( FL_ROUND_UP_BOX );
        m_dialImuHeading->bounds( 0.0, 360.0 );
        m_dialImuHeading->value( 180.0 );
        m_dialImuHeading->callback( ImuHeadingDialCB, this );
    }

    m_sliderImuPitch = new Fl_Slider( 335, 210, 35, 60 );
    if ( m_sliderImuPitch )
    {
        m_sliderImuPitch->label( "Pitch" );
        m_sliderImuPitch->slider( FL_UP_BOX );
        m_sliderImuPitch->bounds( 20.0, -20.0 );
        m_sliderImuPitch->value( 0.0 );
        m_sliderImuPitch->callback( ImuPitchSliderCB, this );
    }

    m_sliderImuRoll = new Fl_Slider( 380, 210, 35, 60 );
    if ( m_sliderImuRoll )
    {
        m_sliderImuRoll->label( "Roll" );
        m_sliderImuRoll->slider( FL_UP_BOX );
        m_sliderImuRoll->bounds( 20.0, -20.0 );
        m_sliderImuRoll->value( 0.0 );
        m_sliderImuRoll->callback( ImuRollSliderCB, this );
    }

    m_window->end();
}

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

void SimDisplay::createSonarFrontControls()
{
    m_window->begin();

    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 310, 165, 20 );
    if ( m_buttonSonarFrontKA )
    {
        m_buttonSonarFrontKA->label( "Sonar Front Alive" );
        m_buttonSonarFrontKA->selection_color( FL_GREEN );
        m_buttonSonarFrontKA->callback( &SonarFrontKeepAliveCB );
    }

    m_textSonarFront = new Fl_Box( 250, 340, 165, 20 );
    if ( m_textSonarFront )
    {
        m_textSonarFront->box( FL_THIN_DOWN_BOX );
        m_textSonarFront->align( FL_ALIGN_CENTER );
        m_textSonarFront->label( "0F00 0F00 0F00 0F00" );
    }

    m_sliderSonarFrontL = new Fl_Slider( 250, 370, 35, 100 );
    if ( m_sliderSonarFrontL )
    {
        m_sliderSonarFrontL->label( "Left" );
        m_sliderSonarFrontL->slider( FL_UP_BOX );
        m_sliderSonarFrontL->bounds( 100, 0 );
        m_sliderSonarFrontL->value( SonarFrontStatus::sonarFrontLeft() );
        m_sliderSonarFrontL->callback( SonarFrontSliderLeftCB, this );
    }

    m_sliderSonarFrontCL = new Fl_Slider( 293, 370, 35, 100 );
    if ( m_sliderSonarFrontCL )
    {
        m_sliderSonarFrontCL->label( "C Left" );
        m_sliderSonarFrontCL->slider( FL_UP_BOX );
        m_sliderSonarFrontCL->bounds( 100, 0 );
        m_sliderSonarFrontCL->value( SonarFrontStatus::sonarFrontCenterLeft() );
        m_sliderSonarFrontCL->callback( SonarFrontSliderCenterLeftCB, this );
    }

    m_sliderSonarFrontCR = new Fl_Slider( 336, 370, 35, 100 );
    if ( m_sliderSonarFrontCR )
    {
        m_sliderSonarFrontCR->label( "C Right" );
        m_sliderSonarFrontCR->slider( FL_UP_BOX );
        m_sliderSonarFrontCR->bounds( 100, 0 );
        m_sliderSonarFrontCR->value( SonarFrontStatus::sonarFrontCenterRight() );
        m_sliderSonarFrontCR->callback( SonarFrontSliderCenterRightCB, this );
    }

    m_sliderSonarFrontR = new Fl_Slider( 380, 370, 35, 100 );
    if ( m_sliderSonarFrontR )
    {
        m_sliderSonarFrontR->label( "Right" );
        m_sliderSonarFrontR->slider( FL_UP_BOX );
        m_sliderSonarFrontR->bounds( 100, 0 );
        m_sliderSonarFrontR->value( SonarFrontStatus::sonarFrontRight() );
        m_sliderSonarFrontR->callback( SonarFrontSliderRightCB, this );
    }

    m_window->end();
}

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

void SimDisplay::showDisplay()
{
    createWindow();

    if ( m_window )
    {
        createWheelSpeedDials();
        createWheelSliders();
        createDisplays();
        createActionButtons();
        createImuControls();
        createSonarFrontControls();

        m_window->show();
    }
}

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

void SimDisplay::updateWheelSpeeds( int front, int rear )
{
    Fl::lock();

    // front

    if ( m_dialWheelSpeedFront )
    {
        m_dialWheelSpeedFront->value( 
                m_dialWheelSpeedFront->clamp( static_cast< double >( front ) ) );
    }

    if ( m_textWheelSpeedFront )
    {
        char buf[ 20 ];
        snprintf( buf, sizeof( buf ), "%d", front );
        m_textWheelSpeedFront->label( buf );
    }

    Fl::flush();

    // rear

    if ( m_dialWheelSpeedRear )
    {
        m_dialWheelSpeedRear->value( 
                m_dialWheelSpeedRear->clamp( static_cast< double >( rear ) ) );
        m_dialWheelSpeedRear->redraw();
    }

    if ( m_textWheelSpeedRear )
    {
        char buf[ 20 ];
        snprintf( buf, sizeof( buf ), "%d", rear );
        m_textWheelSpeedRear->label( buf );
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::updateWheelPositions( int front, int rear )
{
    Fl::lock();
   
   // front

    if ( m_sliderWheelPosFront )
    {
        m_sliderWheelPosFront->value( 
                m_sliderWheelPosFront->clamp( static_cast< double >( front ) ) );
    }

    if ( m_textWheelPosFront )
    {
        char buf[ 20 ];
        snprintf( buf, sizeof( buf ), "%d", front );
        m_textWheelPosFront->label( buf );
    }

    Fl::flush();

    // rear
    
    if ( m_sliderWheelPosRear )
    {
        m_sliderWheelPosRear->value( 
                m_sliderWheelPosRear->clamp( static_cast< double >( rear ) ) );
    }

    if ( m_textWheelPosRear )
    {
        char buf[ 20 ];
        snprintf( buf, sizeof( buf ), "%d", rear );
        m_textWheelPosRear->label( buf );
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::doMgrHeartbeat()
{
    Fl::lock();
   
    if ( m_boxMgrHeartbeat )
    {
        m_boxMgrHeartbeat->color( FL_GREEN );
        m_boxMgrHeartbeat->redraw();
    }

    m_timerMainHbTimeout.restart();

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::resetMgrHeartbeat()
{
    Fl::lock();
   
    if ( m_boxMgrHeartbeat )
    {
        m_boxMgrHeartbeat->color( FL_BACKGROUND_COLOR );
        m_boxMgrHeartbeat->redraw();
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::timeoutMgrHeartbeat( Poco::Timer& timer )
{
    Fl::lock();

    if ( m_boxMgrHeartbeat )
    {
        m_boxMgrHeartbeat->color( FL_RED );
        m_boxMgrHeartbeat->redraw();
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::updateManagerMsg( const char* msg, int length )
{
    Fl::lock();
   
    if ( m_textManagerMsg )
    {
        std::string message( msg, length );
        m_textManagerMsg->label( message.c_str() );
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::updateSonarFrontState( bool enabled )
{
    Fl::lock();

    if ( m_boxSonarFrontState )
    {
        if ( enabled )
        {
            m_boxSonarFrontState->label( "Front Sonar On" );
            m_boxSonarFrontState->color( FL_GREEN );
        }
        else
        {
            m_boxSonarFrontState->label( "Front Sonar Off" );
            m_boxSonarFrontState->color( FL_BACKGROUND_COLOR );
        }

        m_boxSonarFrontState->redraw();
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::updateImuValues()
{
    Fl::lock();

    if ( m_textImuValues )
    {
        char buf[ 80 ];
        snprintf( buf, sizeof( buf ), "%3.1lf  %3.1lf  %3.1lf",
                  ImuStatus::imuYaw(),
                  ImuStatus::imuPitch(),
                  ImuStatus::imuRoll() );
        m_textImuValues->label( buf );
    }

    Fl::flush();

    Fl::unlock();
}

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

void SimDisplay::updateSonarFrontValues()
{
    Fl::lock();

    uint8_t left        = SonarFrontStatus::sonarFrontLeft();
    uint8_t centerLeft  = SonarFrontStatus::sonarFrontCenterLeft();
    uint8_t centerRight = SonarFrontStatus::sonarFrontCenterRight();
    uint8_t right       = SonarFrontStatus::sonarFrontRight();

    if ( m_textSonarFront )
    {
        char buf[ 80 ];
        snprintf( buf, sizeof( buf ), "%02X  %02X  %02X  %02X",
                  left, centerLeft, centerRight, right );
        m_textSonarFront->label( buf );
    }

    Fl::flush();

    Fl::unlock();
}

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