changeset 136:6a258aafc9d7 main

Updated simulator, changes to the front-facing sonar controls.
author Bob Cook <bob@bobcookdev.com>
date Thu, 12 Jan 2012 21:28:39 -0800
parents caee904383e5
children 468faf23535b
files main/robots/odr-sim/ODRSimApp.cpp main/robots/odr-sim/Receiver.cpp main/robots/odr-sim/Receiver.h main/robots/odr-sim/SimDisplay.cpp main/robots/odr-sim/SimDisplay.h main/robots/odr-sim/SonarFrontStatus.cpp main/robots/odr-sim/SonarFrontStatus.h
diffstat 7 files changed, 271 insertions(+), 50 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-sim/ODRSimApp.cpp	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/ODRSimApp.cpp	Thu Jan 12 21:28:39 2012 -0800
@@ -126,13 +126,10 @@
 
     Poco::Thread    canMsgProcessorThread;
     CANMsgProcessor canMsgProcessor( canInterfaceName, "odrFileLogger" );
-    canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
-    canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
+    //canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
+    //canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
     canMsgProcessorThread.start( canMsgProcessor );
 
-//    std::auto_ptr<Fl_Window> mainWindow( createWindow() );
-//    mainWindow->show();
-
     SimDisplay display;
     display.showDisplay();
 
@@ -155,18 +152,3 @@
 
 // ----------------------------------------------------------------------------------------
 
-
-
-//--------------------------------------------
-#if 0
-void but_cb( Fl_Widget* o, void*  )
-{
-    Fl_Button* b=(Fl_Button*)o;
-    b->label("Good job"); //redraw not necessary
-
-    b->resize(10,150,140,30); //redraw needed
-    b->redraw();
-}
-#endif
-
-//-------------------------------------------- 
--- a/main/robots/odr-sim/Receiver.cpp	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/Receiver.cpp	Thu Jan 12 21:28:39 2012 -0800
@@ -40,6 +40,7 @@
 #include <FL/Fl_Window.H>
 
 #include "SimDisplay.h"
+#include "SonarFrontStatus.h"
 
 #include "packages/common/can/can_helpers.h"
 #include "packages/common/can/can_nodes.h"
@@ -50,7 +51,7 @@
 
 Receiver::Receiver( SimDisplay* theDisplay )
     : Poco::Runnable(),
-      m_quitEvent( false /* do not auto-reset */ ),
+      m_quitEvent( false /* not auto-reset */ ),
       m_display( theDisplay )
 {
 }
@@ -65,7 +66,7 @@
 
 void Receiver::timeToQuit()
 {
-    m_quitEvent = true;
+    m_quitEvent.set();
 }
 
 // ----------------------------------------------------------------------------------------
@@ -116,6 +117,15 @@
 
 void Receiver::recvSonarFrontStateChange( bool enabled )
 {
+    if ( enabled )
+    {
+        SonarFrontStatus::setSonarFrontEnabled();
+    }
+    else
+    {
+        SonarFrontStatus::setSonarFrontDisabled();
+    }
+
     if ( m_display )
     {
         m_display->updateSonarFrontState( enabled );
@@ -132,7 +142,7 @@
         {
             CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms
 
-            if ( msg == 0 && m_quitEvent )
+            if ( msg == 0 && m_quitEvent.tryWait( 0 ) )
             {
                 return;
             }
--- a/main/robots/odr-sim/Receiver.h	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/Receiver.h	Thu Jan 12 21:28:39 2012 -0800
@@ -32,8 +32,7 @@
 #ifndef BCDRL_ROBOTS_ODRSIM_RECEIVER_H
 #define BCDRL_ROBOTS_ODRSIM_RECEIVER_H
 
-#include <stdint.h>
-
+#include <Poco/Event.h>
 #include <Poco/Runnable.h>
 #include <Poco/RWLock.h>
 
@@ -56,7 +55,7 @@
         static Poco::RWLock sm_rwLock;
 
     private:
-        bool        m_quitEvent;
+        Poco::Event m_quitEvent;
         SimDisplay* m_display;
 
     private:
--- a/main/robots/odr-sim/SimDisplay.cpp	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/SimDisplay.cpp	Thu Jan 12 21:28:39 2012 -0800
@@ -42,6 +42,7 @@
 #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>
@@ -155,6 +156,63 @@
 
 // ----------------------------------------------------------------------------------------
 
+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 SonarFrontSliderCenterCB( Fl_Widget* widget, void* value )
+{
+    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
+    if ( slider == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    SonarFrontStatus::setSonarFrontCenter( 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 ),
@@ -174,6 +232,10 @@
       m_buttonA( 0 ),
       m_buttonB( 0 ),
       m_buttonSonarFrontKA( 0 ),
+      m_sliderSonarFrontL( 0 ),
+      m_sliderSonarFrontC( 0 ),
+      m_sliderSonarFrontR( 0 ),
+      m_textSonarFront( 0 ),
       m_timerMainHbTimeout()
 {
 }
@@ -403,14 +465,59 @@
         m_buttonB->callback( &PressButtonTwoCB );
     }
 
-    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 140, 165, 20 );
+    m_window->end();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SimDisplay::createSonarFrontControls()
+{
+    m_window->begin();
+
+    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 150, 165, 20 );
     if ( m_buttonSonarFrontKA )
     {
         m_buttonSonarFrontKA->label( "Sonar Front Alive" );
         m_buttonSonarFrontKA->selection_color( FL_GREEN );
         m_buttonSonarFrontKA->callback( &SonarFrontKeepAliveCB );
-        m_buttonSonarFrontKA->set();
-        m_buttonSonarFrontKA->do_callback();
+    }
+
+    m_textSonarFront = new Fl_Box( 250, 180, 165, 20 );
+    if ( m_textSonarFront )
+    {
+        m_textSonarFront->box( FL_THIN_DOWN_BOX );
+        m_textSonarFront->align( FL_ALIGN_CENTER );
+        m_textSonarFront->label( "0F00  0F00  0F00" );
+    }
+
+    m_sliderSonarFrontL = new Fl_Slider( 250, 210, 40, 100 );
+    if ( m_sliderSonarFrontL )
+    {
+        m_sliderSonarFrontL->label( "Left" );
+        m_sliderSonarFrontL->slider( FL_UP_BOX );
+        m_sliderSonarFrontL->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontL->value( SonarFrontStatus::sonarFrontLeft() );
+        m_sliderSonarFrontL->callback( SonarFrontSliderLeftCB, this );
+    }
+
+    m_sliderSonarFrontC = new Fl_Slider( 312, 210, 40, 100 );
+    if ( m_sliderSonarFrontC )
+    {
+        m_sliderSonarFrontC->label( "Center" );
+        m_sliderSonarFrontC->slider( FL_UP_BOX );
+        m_sliderSonarFrontC->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontC->value( SonarFrontStatus::sonarFrontCenter() );
+        m_sliderSonarFrontC->callback( SonarFrontSliderCenterCB, this );
+    }
+
+    m_sliderSonarFrontR = new Fl_Slider( 375, 210, 40, 100 );
+    if ( m_sliderSonarFrontR )
+    {
+        m_sliderSonarFrontR->label( "Right" );
+        m_sliderSonarFrontR->slider( FL_UP_BOX );
+        m_sliderSonarFrontR->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontR->value( SonarFrontStatus::sonarFrontRight() );
+        m_sliderSonarFrontR->callback( SonarFrontSliderRightCB, this );
     }
 
     m_window->end();
@@ -428,6 +535,7 @@
         createServoSliders();
         createDisplays();
         createActionButtons();
+        createSonarFrontControls();
 
         m_window->show();
     }
@@ -618,3 +726,25 @@
 }
 
 // ----------------------------------------------------------------------------------------
+
+void SimDisplay::updateSonarFrontValues()
+{
+    Fl::lock();
+
+    unsigned left   = SonarFrontStatus::sonarFrontLeft();
+    unsigned center = SonarFrontStatus::sonarFrontCenter();
+    unsigned right  = SonarFrontStatus::sonarFrontRight();
+
+    if ( m_textSonarFront )
+    {
+        char buf[ 80 ];
+        snprintf( buf, sizeof( buf ), "%04X  %04X  %04X", left, center, right );
+        m_textSonarFront->label( buf );
+    }
+
+    Fl::flush();
+
+    Fl::unlock();
+}
+
+// ----------------------------------------------------------------------------------------
--- a/main/robots/odr-sim/SimDisplay.h	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/SimDisplay.h	Thu Jan 12 21:28:39 2012 -0800
@@ -41,6 +41,7 @@
 class Fl_Hor_Slider;
 class Fl_Light_Button;
 class Fl_Line_Dial;
+class Fl_Slider;
 class Fl_Toggle_Button;
 
 // ----------------------------------------------------------------------------------------
@@ -57,6 +58,7 @@
         void doMgrHeartbeat();
         void resetMgrHeartbeat();
         void updateSonarFrontState( bool enabled );
+        void updateSonarFrontValues();
 
     private:
         Fl_Double_Window* m_window;
@@ -77,6 +79,10 @@
         Fl_Toggle_Button* m_buttonA;
         Fl_Toggle_Button* m_buttonB;
         Fl_Light_Button*  m_buttonSonarFrontKA;
+        Fl_Slider*        m_sliderSonarFrontL;
+        Fl_Slider*        m_sliderSonarFrontC;
+        Fl_Slider*        m_sliderSonarFrontR;
+        Fl_Box*           m_textSonarFront;
         Poco::Timer       m_timerMainHbTimeout;
 
     private:
@@ -85,6 +91,7 @@
         void createServoSliders();
         void createDisplays();
         void createActionButtons();
+        void createSonarFrontControls();
         void timeoutMgrHeartbeat( Poco::Timer& timer );
 };
 
--- a/main/robots/odr-sim/SonarFrontStatus.cpp	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/SonarFrontStatus.cpp	Thu Jan 12 21:28:39 2012 -0800
@@ -31,6 +31,8 @@
 
 #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"
@@ -39,8 +41,51 @@
 
 // ----------------------------------------------------------------------------------------
 
+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 = SonarFrontStatus::sonarFrontCenter();
+    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;
+unsigned     SonarFrontStatus::sm_left   = 0x0f00;
+unsigned     SonarFrontStatus::sm_center = 0x0f00;
+unsigned     SonarFrontStatus::sm_right  = 0x0f00;
 
 // ----------------------------------------------------------------------------------------
 
@@ -68,9 +113,51 @@
 
 // ----------------------------------------------------------------------------------------
 
-//void SonarFrontStatus::setSonarFrontLeft( uint16_t value )
-//{
-//}
+unsigned SonarFrontStatus::sonarFrontLeft()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_left;
+}
+
+// ----------------------------------------------------------------------------------------
+
+unsigned SonarFrontStatus::sonarFrontCenter()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_center;
+}
+
+// ----------------------------------------------------------------------------------------
+
+unsigned SonarFrontStatus::sonarFrontRight()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_right;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::setSonarFrontLeft( unsigned value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_left = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::setSonarFrontCenter( unsigned value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_center = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SonarFrontStatus::setSonarFrontRight( unsigned value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_right = value;
+}
 
 // ----------------------------------------------------------------------------------------
 
@@ -97,26 +184,24 @@
 
 void SonarFrontStatus::run()
 {
+    int loopCount = 0;
+
     for ( ;; )
     {
         try
         {
-            uint32_t msgid;
-
-            if ( SonarFrontStatus::isSonarFrontEnabled() )
+            if ( loopCount == 0 )   // every 2.5 seconds...
             {
-                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 );
+                SendSonarFrontStatusMessage();
             }
 
-            CANMessage::QueueToSend( new CANMessage( msgid ) );
+            if ( isSonarFrontEnabled() )
+            {
+                SendSonarFrontValueMessage();
+            }
+
+            loopCount++;
+            loopCount %= 5;   // 0..5
 
         }
         catch ( ... )
@@ -124,7 +209,7 @@
             // nothing to do, but don't stop the thread
         }
 
-        if ( m_quitEvent.tryWait( 2500 ) ) // 2500 ms == 2.5s
+        if ( m_quitEvent.tryWait( 500 ) ) // 500ms = 1/2 second
         {
             return;
         }
--- a/main/robots/odr-sim/SonarFrontStatus.h	Thu Jan 12 21:27:19 2012 -0800
+++ b/main/robots/odr-sim/SonarFrontStatus.h	Thu Jan 12 21:28:39 2012 -0800
@@ -41,10 +41,15 @@
 class SonarFrontStatus : public Poco::Runnable
 {
     public:
-        static bool isSonarFrontEnabled();
-        static void setSonarFrontEnabled();
-        static void setSonarFrontDisabled();
-        //static void setSonarFrontLeft( uint16_t value );
+        static bool     isSonarFrontEnabled();
+        static void     setSonarFrontEnabled();
+        static void     setSonarFrontDisabled();
+        static unsigned sonarFrontLeft();
+        static unsigned sonarFrontCenter();
+        static unsigned sonarFrontRight();
+        static void     setSonarFrontLeft( unsigned value );
+        static void     setSonarFrontCenter( unsigned value );
+        static void     setSonarFrontRight( unsigned value );
 
     public:
         SonarFrontStatus();
@@ -58,6 +63,9 @@
     private:
         static Poco::RWLock sm_rwLock;
         static bool         sm_isEnabled;
+        static unsigned     sm_left;
+        static unsigned     sm_center;
+        static unsigned     sm_right;
 
     private:
         Poco::Event m_quitEvent;