view main/robots/odr-sim/SimDisplay.cpp @ 224:5e91f5877f1f main

Latest simulator changes.
author Bob Cook <bob@bobcookdev.com>
date Thu, 17 Apr 2014 22:03:28 -0700
parents 4ef780aebd7e
children 783f69f37c64
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-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 "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 "ControllerStatus.h"
#include "ImuStatus.h"
#include "SonarFrontStatus.h"

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

static void SendControllerUpdateCB( Fl_Widget* widget, void* value )
{
    static ControllerStatus s_csRunnable;
    static Poco::Thread     s_csThread;

    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_csRunnable.timeToQuit();
        s_csThread.join();
    }
    else
    {
        // on
        s_csThread.start( s_csRunnable );
    }
}

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

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

    ControllerStatus::setEstopActive( button->value() != 0 );
}

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

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

    ControllerStatus::setMotorCtlActive( button->value() != 0 );
}

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

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

    ControllerStatus::setButtonOne( button->value() != 0 );
}

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

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

    ControllerStatus::setButtonTwo( button->value() != 0 );
}

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

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_dialMotorSpeedFront( 0 ),
      m_textMotorSpeedFront( 0 ),
      m_dialMotorSpeedRear( 0 ),
      m_textMotorSpeedRear( 0 ),
      m_sliderServoPosFront( 0 ),
      m_textServoPosFront( 0 ),
      m_sliderServoPosRear( 0 ),
      m_textServoPosRear( 0 ),
      m_boxMgrHeartbeat( 0 ),
      m_textManagerMsg( 0 ),
      m_boxSonarFrontState( 0 ),
      m_buttonSendCtlUpdate( 0 ),
      m_buttonEstop( 0 ),
      m_buttonMotorCtl( 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::createMotorSpeedDials()
{
    m_window->begin();

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

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

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

    // rear

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

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

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

    m_window->end();
}

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

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

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

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

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

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

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

    m_textServoPosRear = new Fl_Box( 100, 200, 75, 20 );
    if ( m_textServoPosRear )
    {
        m_textServoPosRear->box( FL_THIN_DOWN_BOX );
        m_textServoPosRear->align( FL_ALIGN_CENTER );
        m_textServoPosRear->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_buttonSendCtlUpdate = new Fl_Light_Button( 250, 20, 165, 20 );
    if ( m_buttonSendCtlUpdate )
    {
        m_buttonSendCtlUpdate->label( "Do Controller Updates" );
        m_buttonSendCtlUpdate->selection_color( FL_GREEN );
        m_buttonSendCtlUpdate->callback( &SendControllerUpdateCB );
        m_buttonSendCtlUpdate->set();
        m_buttonSendCtlUpdate->do_callback();
    }

    m_buttonEstop = new Fl_Light_Button( 250, 50, 165, 20 );
    if ( m_buttonEstop )
    {
        m_buttonEstop->label( "Remote ESTOP" );
        m_buttonEstop->selection_color( FL_RED );
        m_buttonEstop->callback( &PressEstopCB );
        m_buttonEstop->set();
        m_buttonEstop->do_callback();
    }

    m_buttonMotorCtl = new Fl_Light_Button( 250, 80, 165, 20 );
    if ( m_buttonMotorCtl )
    {
        m_buttonMotorCtl->label( "Motor Controller" );
        m_buttonMotorCtl->selection_color( FL_GREEN );
        m_buttonMotorCtl->callback( &PressMotorCtlCB );
    }

    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 );
    }

    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( ImuStatus::imuYaw() );
        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( 0.0, 20.0 );
        m_sliderImuPitch->value( ImuStatus::imuPitch() );
        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( 0.0, 20.0 );
        m_sliderImuRoll->value( ImuStatus::imuRoll() );
        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( 0xff, 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( 0xff, 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( 0xff, 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( 0xff, 0 );
        m_sliderSonarFrontR->value( SonarFrontStatus::sonarFrontRight() );
        m_sliderSonarFrontR->callback( SonarFrontSliderRightCB, this );
    }

    m_window->end();
}

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

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

    if ( m_window )
    {
        createMotorSpeedDials();
        createServoSliders();
        createDisplays();
        createActionButtons();
        createImuControls();
        createSonarFrontControls();

        m_window->show();
    }
}

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

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

    // front

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

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

    Fl::flush();

    // rear

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

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

    Fl::flush();

    Fl::unlock();
}

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

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

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

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

    Fl::flush();

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

    if ( m_textServoPosRear )
    {
        char buf[ 20 ];
        snprintf( buf, sizeof( buf ), "%d", rear );
        m_textServoPosRear->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();
}

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