changeset 122:0448b2b7c530 main

Updates to the simulator for the latest changes to the front sonar module firmware.
author Bob Cook <bob@bobcookdev.com>
date Wed, 14 Dec 2011 11:45:33 -0800
parents e65210fe1cb0
children 25d2ba77fe8b
files main/robots/odr/MotorsAndServos.cpp main/robots/odr/ODRApp.cpp main/robots/odr/ODRApp.h main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h
diffstat 5 files changed, 300 insertions(+), 66 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/MotorsAndServos.cpp	Sat Dec 03 16:38:46 2011 -0800
+++ b/main/robots/odr/MotorsAndServos.cpp	Wed Dec 14 11:45:33 2011 -0800
@@ -63,7 +63,7 @@
 
     uint32_t servomsgid
         = can_build_message_id(
-                can_node_odr_main, can_node_odr_controller, can_dataid_servo_position );
+                can_node_odr_manager, can_node_odr_controller, can_dataid_servo_position );
 
     CANMessage::QueueToSend(
             new CANMessage( servomsgid,
@@ -81,7 +81,7 @@
 
     uint32_t motormsgid
         = can_build_message_id(
-                can_node_odr_main, can_node_odr_controller, can_dataid_motor_speed );
+                can_node_odr_manager, can_node_odr_controller, can_dataid_motor_speed );
 
     CANMessage::QueueToSend(
             new CANMessage( motormsgid,
--- a/main/robots/odr/ODRApp.cpp	Sat Dec 03 16:38:46 2011 -0800
+++ b/main/robots/odr/ODRApp.cpp	Wed Dec 14 11:45:33 2011 -0800
@@ -57,7 +57,8 @@
 // ----------------------------------------------------------------------------------------
 
 ODRApp::ODRApp()
-    : m_helpRequested( false )
+    : m_helpRequested( false ),
+      m_runloopActive( false ) 
 {
 }
 
@@ -120,18 +121,6 @@
     loadConfiguration();
 
     logger().information( "------------------------------------------------------" );
-    logger().information( "ODR startup" );
-
-    if ( config().getBool( "application.runAsDaemon", false ) )
-    {
-        logger().information( "i am a daemon!" );
-    }
-
-    if ( isInteractive() )
-    {
-        logger().information( "is interactive!" );
-    }
-
     logger().information( "ODRApp::main() started" );
 
     std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
@@ -147,35 +136,34 @@
     Scoreboard   scoreboard( logger().name() );
     scoreboardThread.start( scoreboard );
 
-    MotorsAndServos().motorSpeeds( 32, 32 );
-    MotorsAndServos().servoPositions( 12, -12 );
-    Poco::Thread::sleep( 20000 );
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
+    m_runloopActive = true;
 
-    MotorsAndServos().servoPositions( 0, 12 );
-    Poco::Thread::sleep( 5000 );
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
-
-#if 0
-    for ( uint8_t i = 0; i < 10; ++i )
+    while ( m_runloopActive )
     {
-        Poco::Thread::sleep( 5000 );
-        MotorsAndServos().servoPositions( 10, -10 );
-        Poco::Thread::sleep( 5000 );
-        MotorsAndServos().servoPositions( -8, 0 );
+        try
+        {
+            runloop();
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runloop(), Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runloop(), std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            logger().error( "Failed in runloop(), unknown exception" );
+        }
     }
-    MotorsAndServos().motorSpeeds( 0, 0 );
-    MotorsAndServos().servoPositions( 0, 0 );
-#endif
 
-//    uint8_t msgdata[8] = { 0x01, 0x02, 0x03, 0x04 };
-//    CANMessage::QueueToSend( new CANMessage( 0x333, msgdata, 4 ) );
-    //MotorsAndServos().servoPositions( 10, -10 );
-
-    //Poco::Thread::sleep( 20000 );
-    Poco::Thread::sleep( 5000 );
+    Scoreboard::sendManagerMessage( "Goodbye" );
+    Poco::Thread::sleep( 2000 );
 
     scoreboard.timeToQuit();
     scoreboardThread.join();
@@ -190,3 +178,61 @@
 
 // ----------------------------------------------------------------------------------------
 
+void ODRApp::runloop()
+{
+    for ( ;; )
+    {
+        logger().information( "waiting for the ODR controller" );
+        while ( ! Scoreboard::isControllerAlive() )
+        {
+            Poco::Thread::sleep( 250 );
+        }
+        logger().information( "the ODR controller is alive" );
+
+        Scoreboard::sendManagerMessage( "Press A" );
+
+        bool didTellLogAboutSonar = false;
+        while ( ! Scoreboard::isButtonOneDown() )
+        {
+            Poco::Thread::sleep( 250 );
+
+            if ( Scoreboard::isButtonTwoDown() )
+            {
+                m_runloopActive = false;
+                return;
+            }
+
+            if ( Scoreboard::isSonarFrontAlive() && ! didTellLogAboutSonar )
+            {
+                logger().information( "the front sonar is alive" );
+                didTellLogAboutSonar = true;
+            }
+        }
+        while ( Scoreboard::isButtonOneDown() )
+        {
+            Poco::Thread::sleep( 250 );
+        }
+
+        Scoreboard::sendManagerMessage( "Running" );
+        
+        Scoreboard::setSonarFrontEnabled();
+
+        if ( Scoreboard::isEStopActive() )
+        {
+            logger().information( "estop is active" );
+        }
+
+        MotorsAndServos().motorSpeeds( 32, 32 );
+        MotorsAndServos().servoPositions( 12, -12 );
+        Poco::Thread::sleep( 20000 );
+        MotorsAndServos().motorSpeeds( 0, 0 );
+        MotorsAndServos().servoPositions( 0, 0 );
+
+        Scoreboard::sendManagerMessage( "Done" );
+        Scoreboard::setSonarFrontDisabled();
+        Poco::Thread::sleep( 5000 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ODRApp.h	Sat Dec 03 16:38:46 2011 -0800
+++ b/main/robots/odr/ODRApp.h	Wed Dec 14 11:45:33 2011 -0800
@@ -61,7 +61,11 @@
         int  main(const std::vector<std::string>& args);
 
     private:
+        void runloop();
+
+    private:
         bool m_helpRequested;
+        bool m_runloopActive;
 };
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/Scoreboard.cpp	Sat Dec 03 16:38:46 2011 -0800
+++ b/main/robots/odr/Scoreboard.cpp	Wed Dec 14 11:45:33 2011 -0800
@@ -47,6 +47,13 @@
 
 Poco::RWLock Scoreboard::sm_rwLock;
 bool         Scoreboard::sm_isControllerAlive;
+bool         Scoreboard::sm_isEStopActive;
+bool         Scoreboard::sm_isButtonOneDown;
+bool         Scoreboard::sm_isButtonTwoDown;
+bool         Scoreboard::sm_isSonarFrontAlive;
+uint16_t     Scoreboard::sm_sonarFrontLeft;
+uint16_t     Scoreboard::sm_sonarFrontCenter;
+uint16_t     Scoreboard::sm_sonarFrontRight;
 
 // ----------------------------------------------------------------------------------------
 
@@ -58,21 +65,125 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool Scoreboard::isEStopActive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isEStopActive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isButtonOneDown()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isButtonOneDown;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isButtonTwoDown()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isButtonTwoDown;
+}
+                 
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::sendManagerMessage( const char* msg )
+{
+    uint32_t updatemsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
+
+    can_data_odrmgr_update update;
+
+    uint8_t len = 0;
+    for ( ; len < sizeof( update.msg ); ++len )
+    {
+        if ( *msg == '\0' ) break;
+        update.msg[len] = *msg++;
+    }
+
+    CANMessage::QueueToSend(
+            new CANMessage( updatemsgid, reinterpret_cast< uint8_t* >( &update ), len ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isSonarFrontAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isSonarFrontAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::setSonarFrontEnabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_enable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::setSonarFrontDisabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_disable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontLeft()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontLeft;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontCenter()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontCenter;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontRight()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontRight;
+}
+
+// ----------------------------------------------------------------------------------------
+
 Scoreboard::Scoreboard( const std::string& loggerName )
     : Poco::Runnable(),
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
-      m_timerSendMainHeartbeat(),
-      m_timerControllerTimeout()
+      m_timerSendManagerUpdate(),
+      m_timerControllerUpdate(),
+      m_timerSonarFrontUpdate()
 {
-    m_timerSendMainHeartbeat.setPeriodicInterval( 1000 );
-    m_timerSendMainHeartbeat.start(
-            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendMainHeartbeat ) );
+    m_timerSendManagerUpdate.setPeriodicInterval( 5000 );
+    m_timerSendManagerUpdate.start(
+            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
-    m_timerControllerTimeout.setPeriodicInterval( 5000 );
-    m_timerControllerTimeout.start(
+    m_timerControllerUpdate.setPeriodicInterval( 5000 );
+    m_timerControllerUpdate.start(
             Poco::TimerCallback< Scoreboard >(
-                    *this, &Scoreboard::timeoutControllerHeartbeat ) );
+                    *this, &Scoreboard::timeoutControllerUpdate ) );
+
+    m_timerSonarFrontUpdate.setPeriodicInterval( 5000 );
+    m_timerSonarFrontUpdate.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutSonarFrontUpdate ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -90,21 +201,18 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::sendMainHeartbeat( Poco::Timer& timer )
+void Scoreboard::sendManagerUpdate( Poco::Timer& timer )
 {
-    uint32_t data = can_heartbeat_data;
-
-    uint32_t heartbeatmsgid = can_build_message_id(
-                    can_node_odr_main, can_node_broadcast, can_dataid_heartbeat );
+    uint32_t updatemsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
     CANMessage::QueueToSend(
-            new CANMessage( heartbeatmsgid,
-                    reinterpret_cast< uint8_t* >( &data ), sizeof( data ) ) );
+            new CANMessage( updatemsgid, 0, 0 ) );
 }
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::timeoutControllerHeartbeat( Poco::Timer& timer )
+void Scoreboard::timeoutControllerUpdate( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_isControllerAlive = false;
@@ -112,6 +220,14 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::timeoutSonarFrontUpdate( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isSonarFrontAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::recvHeartbeat( uint8_t srcNode )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -119,14 +235,44 @@
     switch ( srcNode )
     {
         case can_node_odr_controller:
-            m_timerControllerTimeout.restart();
-            sm_isControllerAlive = true;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_isSonarFrontAlive = true;
             break;
     }
 }
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvControllerUpdate( CANMessage* msg )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    m_timerControllerUpdate.restart();
+    sm_isControllerAlive = true;
+
+    can_data_odrctl_update info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    sm_isEStopActive = info.estop;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvButtonUpdate( CANMessage* msg )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    can_data_odrctl_button info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    sm_isButtonOneDown = info.button_1;
+    sm_isButtonTwoDown = info.button_2;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -162,20 +308,32 @@
                 switch ( dataId )
                 {
                     case can_dataid_heartbeat:
+                    case can_dataid_sonar_front_state_enabled:
+                    case can_dataid_sonar_front_state_disabled:
                         recvHeartbeat( srcNode );
                         break;
+
+                    case can_dataid_odrctl_update:
+                        recvControllerUpdate( msg );
+                        break;
+
+                    case can_dataid_odrctl_button:
+                        recvButtonUpdate( msg );
+                        break;
                 }
             }
         }
         catch ( const Poco::Exception& ex )
         {
-            log.error( std::string( "Scoreboard::run() caught Poco::Exception: " )
-                    + ex.displayText() );
+            log.error(
+                Poco::Logger::format(
+                    "Scoreboard::run() caught Poco::Exception: $0", ex.displayText() ) );
         }
         catch ( const std::exception& ex )
         {
-            log.error( std::string( "Scoreboard::run() caught std::exception: " )
-                    + ex.what() );
+            log.error(
+                Poco::Logger::format(
+                    "Scoreboard::run() caught std::exception: $0", ex.what() ) );
         }
         catch ( ... )
         {
--- a/main/robots/odr/Scoreboard.h	Sat Dec 03 16:38:46 2011 -0800
+++ b/main/robots/odr/Scoreboard.h	Wed Dec 14 11:45:33 2011 -0800
@@ -39,12 +39,27 @@
 #include <Poco/RWLock.h>
 #include <Poco/Timer.h>
 
+class CANMessage;
+
 // ----------------------------------------------------------------------------------------
 
 class Scoreboard : public Poco::Runnable
 {
     public:
+        // odr-controller
         static bool isControllerAlive();
+        static bool isEStopActive();
+        static bool isButtonOneDown();
+        static bool isButtonTwoDown();
+        static void sendManagerMessage( const char* msg );
+
+        // odr-sonar-front
+        static bool     isSonarFrontAlive();
+        static void     setSonarFrontEnabled();
+        static void     setSonarFrontDisabled();
+        static uint16_t sonarFrontLeft();
+        static uint16_t sonarFrontCenter();
+        static uint16_t sonarFrontRight();
 
     public:
         Scoreboard( const std::string& loggerName );
@@ -55,17 +70,28 @@
     private:
         static Poco::RWLock sm_rwLock;
         static bool         sm_isControllerAlive;
+        static bool         sm_isEStopActive;
+        static bool         sm_isButtonOneDown;
+        static bool         sm_isButtonTwoDown;
+        static bool         sm_isSonarFrontAlive;
+        static uint16_t     sm_sonarFrontLeft;
+        static uint16_t     sm_sonarFrontCenter;
+        static uint16_t     sm_sonarFrontRight;
 
     private:
         std::string m_loggerName;
         Poco::Event m_quitEvent;
-        Poco::Timer m_timerSendMainHeartbeat;
-        Poco::Timer m_timerControllerTimeout;
+        Poco::Timer m_timerSendManagerUpdate;
+        Poco::Timer m_timerControllerUpdate;
+        Poco::Timer m_timerSonarFrontUpdate;
 
     private:
-        void sendMainHeartbeat( Poco::Timer& timer );
-        void timeoutControllerHeartbeat( Poco::Timer& timer );
+        void sendManagerUpdate( Poco::Timer& timer );
+        void timeoutControllerUpdate( Poco::Timer& timer );
+        void timeoutSonarFrontUpdate( Poco::Timer& timer );
         void recvHeartbeat( uint8_t srcNode );
+        void recvControllerUpdate( CANMessage* msg );
+        void recvButtonUpdate( CANMessage* msg );
 };
 
 // ----------------------------------------------------------------------------------------