changeset 182:070201175cc6 main

The sonar (ultrasonic) sensors now send uint8_t in (approximately) inch measurement values.
author Bob Cook <bob@bobcookdev.com>
date Sat, 09 Feb 2013 14:29:59 -0800
parents 7bfb98e62138
children 8fb361214b57
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/AvoidBySonarTask.h main/robots/odr/ImuReader.cpp main/robots/odr/NavigateTask.cpp main/robots/odr/NavigateTask.h main/robots/odr/ODRApp.cpp main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h main/robots/odr/jamfile
diffstat 9 files changed, 575 insertions(+), 92 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/AvoidBySonarTask.cpp	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Sat Feb 09 14:29:59 2013 -0800
@@ -7,7 +7,7 @@
 //
 //  Virtual base class for a subsumption task object.
 //
-//  Copyright (c) 2012 Bob Cook
+//  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
@@ -31,26 +31,33 @@
 
 #include "AvoidBySonarTask.h"
 
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
 #include "MotorsAndServos.h"
 #include "Scoreboard.h"
 #include "Sonar.h"
 
 // ----------------------------------------------------------------------------------------
 
-static const unsigned AVOIDANCE_ZONE = 0x1000;
-static const unsigned MAX_TURN_ZONE  = 0x0800;
-static const unsigned TRAPPED_ZONE   = 0x0500;
+static const unsigned CENTER_AVOIDANCE_ZONE = 0x2000 / 300;
+static const unsigned CENTER_MAX_TURN_ZONE  = 0x1000 / 300;
+
+static const unsigned SIDE_AVOIDANCE_ZONE  = 0x1800 / 300;
+static const unsigned SIDE_MAX_TURN_ZONE   = 0x1000 / 300;
+
+static const unsigned TRAPPED_ZONE = 0x0700 / 300;
 
 // ----------------------------------------------------------------------------------------
 
-static int8_t CalculateTurn( unsigned sensorValue )
+static int8_t CalculateTurnFromCenterSensor( unsigned sensorValue )
 {
-    if ( sensorValue >= AVOIDANCE_ZONE )
+    if ( sensorValue >= CENTER_AVOIDANCE_ZONE )
     {
         return 0; // no turn
     }
 
-    if ( sensorValue <= MAX_TURN_ZONE )
+    if ( sensorValue <= CENTER_MAX_TURN_ZONE )
     {
         return 12; // max turn
     }
@@ -58,8 +65,41 @@
     // the wheels have a range of -12 to 12, but we are only calculating the positive
     // angle - the caller will figure out the rest
 
-    const unsigned turnPerSensorInc = ( AVOIDANCE_ZONE - MAX_TURN_ZONE ) / 12;
-    unsigned rawTurn = ( AVOIDANCE_ZONE - sensorValue ) / turnPerSensorInc;
+    //const unsigned turnPerSensorInc = ( CENTER_AVOIDANCE_ZONE - CENTER_MAX_TURN_ZONE ) / 12;
+    unsigned rawTurn = ( CENTER_AVOIDANCE_ZONE - sensorValue );// / turnPerSensorInc;
+
+    if ( rawTurn > 12 )
+    {
+        return 12;
+    }
+
+    return static_cast< int8_t >( rawTurn );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static int8_t CalculateTurnFromSideSensor( unsigned sensorValue )
+{
+    if ( sensorValue >= SIDE_AVOIDANCE_ZONE )
+    {
+        return 0; // no turn
+    }
+
+    if ( sensorValue <= SIDE_MAX_TURN_ZONE )
+    {
+        return 12; // max turn
+    }
+
+    // the wheels have a range of -12 to 12, but we are only calculating the positive
+    // angle - the caller will figure out the rest
+
+    //const unsigned turnPerSensorInc = ( SIDE_AVOIDANCE_ZONE - SIDE_MAX_TURN_ZONE ) / 12;
+    unsigned rawTurn = ( SIDE_AVOIDANCE_ZONE - sensorValue ); // / turnPerSensorInc;
+
+    if ( rawTurn > 12 )
+    {
+        return 12;
+    }
 
     return static_cast< int8_t >( rawTurn );
 }
@@ -68,11 +108,11 @@
 
 AvoidBySonarTask::AvoidBySonarTask( const std::string& loggerName )
     : TaskObject( loggerName ),
-      m_lastMessageTime(),
       m_frontLeft( 0 ),
       m_frontCenterLeft( 0 ),
       m_frontCenterRight( 0 ),
-      m_frontRight( 0 )
+      m_frontRight( 0 ),
+      m_isAvoiding( false )
 {
 }
 
@@ -101,31 +141,59 @@
 
 bool AvoidBySonarTask::wantsControl()
 {
-    return ( m_frontLeft < AVOIDANCE_ZONE
-                || m_frontCenterLeft < AVOIDANCE_ZONE
-                || m_frontCenterRight < AVOIDANCE_ZONE
-                || m_frontRight < AVOIDANCE_ZONE );
+    if ( m_frontLeft < SIDE_AVOIDANCE_ZONE
+                || m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
+                || m_frontCenterRight < CENTER_AVOIDANCE_ZONE
+                || m_frontRight < SIDE_AVOIDANCE_ZONE )
+    {
+        if ( ! m_isAvoiding )
+        {
+            m_isAvoiding = true;
+
+            log().information(
+                Poco::Logger::format(
+                    "AvoidBySonarTask: now avoiding: $0 $1 $2 $3",
+                    Poco::NumberFormatter::formatHex( m_frontLeft ),
+                    Poco::NumberFormatter::formatHex( m_frontCenterLeft ),
+                    Poco::NumberFormatter::formatHex( m_frontCenterRight ),
+                    Poco::NumberFormatter::formatHex( m_frontRight ) ) );
+        }
+    }
+    else
+    {
+        if ( m_isAvoiding )
+        {
+            m_isAvoiding = false;
+
+            log().information(
+                Poco::Logger::format(
+                    "AvoidBySonarTask: no longer avoiding: $0 $1 $2 $3",
+                    Poco::NumberFormatter::formatHex( m_frontLeft ),
+                    Poco::NumberFormatter::formatHex( m_frontCenterLeft ),
+                    Poco::NumberFormatter::formatHex( m_frontCenterRight ),
+                    Poco::NumberFormatter::formatHex( m_frontRight ) ) );
+        }
+    }
+
+    return m_isAvoiding;
+//    return ( m_frontLeft < SIDE_AVOIDANCE_ZONE
+//                || m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
+//                || m_frontCenterRight < CENTER_AVOIDANCE_ZONE
+//                || m_frontRight < SIDE_AVOIDANCE_ZONE );
 }
 
 // ----------------------------------------------------------------------------------------
 
 bool AvoidBySonarTask::isTrapped() const
 {
-    return ( m_frontLeft < TRAPPED_ZONE
-                && m_frontCenterLeft < TRAPPED_ZONE
-                && m_frontCenterRight < TRAPPED_ZONE
-                && m_frontRight < TRAPPED_ZONE );
+    return ( m_frontCenterLeft < TRAPPED_ZONE && m_frontCenterRight < TRAPPED_ZONE );
 }
 
 // ----------------------------------------------------------------------------------------
 
 void AvoidBySonarTask::takeControl()
 {
-    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
-    {
-        Scoreboard::sendManagerMessage( "Avoiding" );
-        m_lastMessageTime.update();
-    }
+    Scoreboard::sendManagerMessage( "Avoiding" );
 
     if ( isTrapped() )
     {
@@ -133,34 +201,38 @@
         return; // stuck!
     }
 
-    uint16_t minLeft = ( m_frontCenterLeft < m_frontLeft ? m_frontCenterLeft : m_frontLeft );
-    uint16_t minRight = ( m_frontCenterRight < m_frontRight ? m_frontCenterRight : m_frontRight );
-
-    if ( minLeft < AVOIDANCE_ZONE && minRight < AVOIDANCE_ZONE )
+    if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
+                                         && m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
     {
-        if ( minLeft < ( minRight + TRAPPED_ZONE ) )
+        if ( m_frontLeft < m_frontRight )
         {
-            int8_t turn = CalculateTurn( minLeft + TRAPPED_ZONE );
+            int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
             MotorsAndServos::servoPositions( -turn, turn );
         }
-        else if ( minRight < ( minLeft + TRAPPED_ZONE ) )
-        {
-            int8_t turn = CalculateTurn( minRight + TRAPPED_ZONE );
-            MotorsAndServos::servoPositions( turn, -turn );
-        }
         else
         {
-            MotorsAndServos::servoPositions( 0, 0 ); // straight
+            int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
+            MotorsAndServos::servoPositions( turn, -turn );
         }
     }
-    else if ( minLeft < AVOIDANCE_ZONE )
+    else if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( minLeft );
+        int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterLeft );
         MotorsAndServos::servoPositions( -turn, turn );
     }
-    else if ( minRight < AVOIDANCE_ZONE )
+    else if ( m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
+    {
+        int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterRight );
+        MotorsAndServos::servoPositions( turn, -turn );
+    }
+    else if ( m_frontLeft < SIDE_AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( minRight );
+        int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
+        MotorsAndServos::servoPositions( -turn, turn );
+    }
+    else if ( m_frontRight < SIDE_AVOIDANCE_ZONE )
+    {
+        int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
         MotorsAndServos::servoPositions( turn, -turn );
     }
     else
--- a/main/robots/odr/AvoidBySonarTask.h	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/AvoidBySonarTask.h	Sat Feb 09 14:29:59 2013 -0800
@@ -49,11 +49,11 @@
         bool isTrapped() const;
 
     private:
-        Poco::Timestamp m_lastMessageTime;
-        unsigned        m_frontLeft;
-        unsigned        m_frontCenterLeft;
-        unsigned        m_frontCenterRight;
-        unsigned        m_frontRight;
+        unsigned m_frontLeft;
+        unsigned m_frontCenterLeft;
+        unsigned m_frontCenterRight;
+        unsigned m_frontRight;
+        bool     m_isAvoiding;
 };
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/ImuReader.cpp	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/ImuReader.cpp	Sat Feb 09 14:29:59 2013 -0800
@@ -122,8 +122,20 @@
     termios settings;
     bzero( &settings, sizeof( settings ) );
 
-    settings.c_iflag = IGNBRK;
-    settings.c_cflag = CREAD | CLOCAL;
+    if ( tcgetattr( m_serialPortFd, &settings ) < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::configureSerialPort() cannot get config on port \"$0\": $1",
+                m_serialPortPath,
+                std::string( strerror( errno ) ) ) );
+
+        closeSerialPort();
+        return false;
+    }
+
+    settings.c_iflag |= IGNBRK;
+    settings.c_cflag |= CREAD | CLOCAL;
     settings.c_cc[ VMIN ]  = 0; // read doesn't block
     settings.c_cc[ VTIME ] = 1; // always return within 0.1 seconds
 
@@ -313,7 +325,7 @@
                     m_parseValueYaw = Poco::NumberParser::parseFloat( m_parseToken );
                     m_parseToken.clear();
                     m_parseState = PARSE_STATE_LOOKING_FOR_START;
-                    completedNewValues = true;;
+                    completedNewValues = true;
                 }
                 else
                 {
@@ -413,6 +425,12 @@
 
         try
         {
+            // to keep ourselves sane, start by assuming the IMU is offline
+
+            Scoreboard::imuIsOffline();
+
+            // now try to find it on the serial port
+
             if ( ! configureSerialPort() )
             {
                 Poco::Thread::sleep( 10000 ); // 10 sec
@@ -426,6 +444,10 @@
             std::vector< double > valuesYaw;
             valuesYaw.reserve( 10 );
 
+            // timeout after 10 consecutive runs of not getting data
+
+            int timeoutNoData = 0;
+
             // loop while we are still getting data
 
             for ( ;; )
@@ -444,6 +466,19 @@
                     break; // error, drop out and reinit the port
                 }
 
+                if ( length == 0 )
+                {
+                    if ( ++timeoutNoData == 10 )
+                    {
+                        Scoreboard::imuIsOffline();
+                    }
+                    continue;
+                }
+
+                // got some bytes, try to parse them
+
+                timeoutNoData = 0;
+
                 if ( processImuData( buffer, length ) )
                 {
                     // new values available, save them
@@ -457,12 +492,9 @@
                     {
                         double averageYaw = computeAverageYaw( valuesYaw );
 
-                        log.information(
-                            Poco::Logger::format(
-                                "ImuReader average roll: $0 / pitch: $1 / yaw: $2",
-                                Poco::NumberFormatter::format( m_parseValueRoll ),
-                                Poco::NumberFormatter::format( m_parseValuePitch ),
-                                Poco::NumberFormatter::format( averageYaw ) ) );
+                        Scoreboard::imuUpdate( m_parseValueRoll,
+                                               m_parseValuePitch,
+                                               averageYaw );
 
                         // clear the vector, to accumulate more samples
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/NavigateTask.cpp	Sat Feb 09 14:29:59 2013 -0800
@@ -0,0 +1,130 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/NavigateTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to drive straight forward. Lowest priority task.
+//
+//  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 "NavigateTask.h"
+
+#include <cmath>
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+static double ComputeHeadingDiff( double target, double current )
+{
+    if ( target >= 0.0 && current >= 0.0 )
+    {
+        return target - current;
+    }
+    else if ( target < 0.0 && current < 0.0 )
+    {
+        return target - current;
+    }
+    else
+    {
+        double diffFromZero = std::abs( target ) + std::abs( current );
+        double diffFrom180  = 360.0 - diffFromZero;
+
+        if ( diffFromZero < diffFrom180 )
+        {
+            if ( current < 0.0 ) diffFromZero *= -1.0;
+            return diffFromZero;
+        }
+        else
+        {
+            if ( current < 0.0 ) diffFrom180 *= -1.0;
+            return diffFrom180;
+        }
+    }
+
+    return 0.0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+NavigateTask::NavigateTask( const std::string& loggerName )
+    : TaskObject( loggerName )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+NavigateTask::~NavigateTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void NavigateTask::update()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool NavigateTask::wantsControl()
+{
+    double headingDiff = ComputeHeadingDiff( Scoreboard::navTargetHeading(),
+                                             Scoreboard::imuYaw() );
+
+    return ( std::abs( headingDiff ) > 0.0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void NavigateTask::takeControl()
+{
+    Scoreboard::sendManagerMessage( "Navigate" );
+
+    double headingDiff = ComputeHeadingDiff( Scoreboard::navTargetHeading(),
+                                             Scoreboard::imuYaw() );
+
+    int8_t servoPosition;
+
+    if ( headingDiff <= 12.0 && headingDiff >= -12.0 )
+    {
+        servoPosition = static_cast< int8_t >( headingDiff );
+    }
+    else if ( headingDiff < 0.0 )
+    {
+        servoPosition = -12;
+    }
+    else
+    {
+        servoPosition = 12;
+    }
+
+    MotorsAndServos::servoPositions( -servoPosition, servoPosition );
+    MotorsAndServos::motorSpeeds( 32, 32 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/NavigateTask.h	Sat Feb 09 14:29:59 2013 -0800
@@ -0,0 +1,53 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/NavigateTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate towards a particular heading.
+//
+//  Copyright (c) 2012 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_NAVIGATETASK_H
+#define BCDRL_ROBOTS_ODR_NAVIGATETASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class NavigateTask : public TaskObject
+{
+    public:
+        NavigateTask( const std::string& loggerName );
+        virtual ~NavigateTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_NAVIGATETASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ODRApp.cpp	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/ODRApp.cpp	Sat Feb 09 14:29:59 2013 -0800
@@ -49,6 +49,7 @@
 #include "CruisingTask.h"
 #include "ImuReader.h"
 #include "MotorsAndServos.h"
+#include "NavigateTask.h"
 #include "SafetyTask.h"
 #include "Scoreboard.h"
 #include "Sonar.h"
@@ -170,6 +171,9 @@
     m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
 
+//    m_tasks.push_back(
+//            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
+
     m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new CruisingTask( logger().name() ) ) );
 
--- a/main/robots/odr/Scoreboard.cpp	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/Scoreboard.cpp	Sat Feb 09 14:29:59 2013 -0800
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 // 
-//  Global status derived from incoming CAN messages, available from any thread.
+//  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2010-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
@@ -35,6 +35,7 @@
 
 #include <Poco/Exception.h>
 #include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
 #include <Poco/Thread.h>
 
 #include "packages/common/can/can_helpers.h"
@@ -45,17 +46,25 @@
 
 // ----------------------------------------------------------------------------------------
 
-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;
-bool         Scoreboard::sm_isSonarFrontEnabled;
-uint16_t     Scoreboard::sm_sonarFrontLeft;
-uint16_t     Scoreboard::sm_sonarFrontCenterLeft;
-uint16_t     Scoreboard::sm_sonarFrontCenterRight;
-uint16_t     Scoreboard::sm_sonarFrontRight;
+Poco::RWLock    Scoreboard::sm_rwLock;
+bool            Scoreboard::sm_isControllerAlive;
+bool            Scoreboard::sm_isEStopActive;
+bool            Scoreboard::sm_isButtonOneDown;
+bool            Scoreboard::sm_isButtonTwoDown;
+std::string     Scoreboard::sm_msgLastText;
+Poco::Timestamp Scoreboard::sm_msgLastUpdate;
+bool            Scoreboard::sm_isSonarFrontAlive;
+bool            Scoreboard::sm_isSonarFrontEnabled;
+uint8_t         Scoreboard::sm_sonarFrontLeft;
+uint8_t         Scoreboard::sm_sonarFrontCenterLeft;
+uint8_t         Scoreboard::sm_sonarFrontCenterRight;
+uint8_t         Scoreboard::sm_sonarFrontRight;
+bool            Scoreboard::sm_isImuAlive;
+Poco::Timestamp Scoreboard::sm_imuLastUpdate;
+double          Scoreboard::sm_imuRoll;
+double          Scoreboard::sm_imuPitch;
+double          Scoreboard::sm_imuYaw;
+double          Scoreboard::sm_navTargetHeading;
 
 // ----------------------------------------------------------------------------------------
 
@@ -93,6 +102,23 @@
 
 void Scoreboard::sendManagerMessage( const char* msg )
 {
+    // don't repeat the same message more than once per five seconds
+
+    static const Poco::Timestamp::TimeDiff FiveSeconds = Poco::Timestamp::resolution() * 5;
+
+    if ( sm_msgLastText == msg )
+    {
+        if ( sm_msgLastUpdate.elapsed() <= FiveSeconds )
+        {
+            return;
+        }
+    }
+
+    // either a new message, or time to send it again
+
+    sm_msgLastText = msg;
+    sm_msgLastUpdate.update();
+
     uint32_t updatemsgid = can_build_message_id(
                     can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
@@ -127,7 +153,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontLeft()
+uint8_t Scoreboard::sonarFrontLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_sonarFrontLeft;
@@ -135,7 +161,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontCenterLeft()
+uint8_t Scoreboard::sonarFrontCenterLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_sonarFrontCenterLeft;
@@ -143,7 +169,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontCenterRight()
+uint8_t Scoreboard::sonarFrontCenterRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_sonarFrontCenterRight;
@@ -151,7 +177,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontRight()
+uint8_t Scoreboard::sonarFrontRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_sonarFrontRight;
@@ -159,13 +185,84 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool Scoreboard::isImuAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isImuAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::imuRoll()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuRoll;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::imuPitch()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuPitch;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::imuYaw()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuYaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::imuUpdate( double roll, double pitch, double yaw )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isImuAlive = true;
+    sm_imuLastUpdate.update();
+
+    sm_imuRoll  = roll;
+    sm_imuPitch = pitch;
+    sm_imuYaw   = yaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::imuIsOffline()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isImuAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::navTargetHeading()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_navTargetHeading;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::navSetTargetHeading( double heading )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_navTargetHeading = heading;
+}
+
+// ----------------------------------------------------------------------------------------
+
 Scoreboard::Scoreboard( const std::string& loggerName )
     : Poco::Runnable(),
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
       m_timerSendManagerUpdate(),
       m_timerControllerUpdate(),
-      m_timerSonarFrontUpdate()
+      m_timerSonarFrontUpdate(),
+      m_timerSendImuDataUpdate()
 {
     m_timerSendManagerUpdate.setPeriodicInterval( 2500 );
     m_timerSendManagerUpdate.start(
@@ -180,6 +277,10 @@
     m_timerSonarFrontUpdate.start(
             Poco::TimerCallback< Scoreboard >(
                     *this, &Scoreboard::timeoutSonarFrontUpdate ) );
+
+    m_timerSendImuDataUpdate.setPeriodicInterval( 750 );
+    m_timerSendImuDataUpdate.start(
+            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendImuDataUpdate ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -203,6 +304,22 @@
                     can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
     CANMessage::QueueToSend( new CANMessage( updatemsgid ) );
+
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    unsigned send_q_len = CANMessage::SendQueueSize();
+    unsigned recv_q_len = CANMessage::ReceiveQueueSize();
+
+    if ( send_q_len > 5 || recv_q_len > 5 )
+    {
+        log.information(
+            Poco::Logger::format(
+                "Scoreboard: CAN queue length: send $0 / recv $1",
+                Poco::NumberFormatter::format( send_q_len ),
+                Poco::NumberFormatter::format( recv_q_len ) ) );
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -223,6 +340,58 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::sendImuDataUpdate( Poco::Timer& timer )
+{
+    // check if the IMU is actually offline; more than 2 seconds silence means yes
+
+    static const Poco::Timestamp::TimeDiff TwoSeconds = Poco::Timestamp::resolution() * 2;
+
+    if ( sm_imuLastUpdate.elapsed() > TwoSeconds )
+    {
+        sm_isImuAlive = false;
+        return;
+    }
+
+    // YAW
+
+    uint32_t imudatamsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_yaw );
+
+    can_data_imu_data data;
+    data.data = static_cast< int32_t >( Scoreboard::imuYaw() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+
+    // PITCH
+
+    imudatamsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_pitch );
+
+    data.data = static_cast< int32_t >( Scoreboard::imuPitch() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+
+    // ROLL
+
+    imudatamsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_roll );
+
+    data.data = static_cast< int32_t >( Scoreboard::imuRoll() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::recvHeartbeat( uint8_t srcNode )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
--- a/main/robots/odr/Scoreboard.h	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/Scoreboard.h	Sat Feb 09 14:29:59 2013 -0800
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 // 
-//  Global status derived from incoming CAN messages, available from any thread.
+//  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2010-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
@@ -38,6 +38,7 @@
 #include <Poco/Runnable.h>
 #include <Poco/RWLock.h>
 #include <Poco/Timer.h>
+#include <Poco/Timestamp.h>
 
 class CANMessage;
 
@@ -54,12 +55,24 @@
         static void sendManagerMessage( const char* msg );
 
         // odr-sonar-front
-        static bool     isSonarFrontAlive();
-        static bool     isSonarFrontEnabled();
-        static uint16_t sonarFrontLeft();
-        static uint16_t sonarFrontCenterLeft();
-        static uint16_t sonarFrontCenterRight();
-        static uint16_t sonarFrontRight();
+        static bool    isSonarFrontAlive();
+        static bool    isSonarFrontEnabled();
+        static uint8_t sonarFrontLeft();
+        static uint8_t sonarFrontCenterLeft();
+        static uint8_t sonarFrontCenterRight();
+        static uint8_t sonarFrontRight();
+
+        // imu
+        static bool   isImuAlive();
+        static double imuRoll();
+        static double imuPitch();
+        static double imuYaw();
+        static void   imuUpdate( double roll, double pitch, double yaw );
+        static void   imuIsOffline();
+
+        // navigation
+        static double navTargetHeading();
+        static void   navSetTargetHeading( double heading );
 
     public:
         Scoreboard( const std::string& loggerName );
@@ -68,17 +81,25 @@
         void timeToQuit();
 
     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 bool         sm_isSonarFrontEnabled;
-        static uint16_t     sm_sonarFrontLeft;
-        static uint16_t     sm_sonarFrontCenterLeft;
-        static uint16_t     sm_sonarFrontCenterRight;
-        static uint16_t     sm_sonarFrontRight;
+        static Poco::RWLock    sm_rwLock;
+        static bool            sm_isControllerAlive;
+        static bool            sm_isEStopActive;
+        static bool            sm_isButtonOneDown;
+        static bool            sm_isButtonTwoDown;
+        static std::string     sm_msgLastText;
+        static Poco::Timestamp sm_msgLastUpdate;
+        static bool            sm_isSonarFrontAlive;
+        static bool            sm_isSonarFrontEnabled;
+        static uint8_t         sm_sonarFrontLeft;
+        static uint8_t         sm_sonarFrontCenterLeft;
+        static uint8_t         sm_sonarFrontCenterRight;
+        static uint8_t         sm_sonarFrontRight;
+        static bool            sm_isImuAlive;
+        static Poco::Timestamp sm_imuLastUpdate;
+        static double          sm_imuRoll;
+        static double          sm_imuPitch;
+        static double          sm_imuYaw;
+        static double          sm_navTargetHeading;
 
     private:
         std::string m_loggerName;
@@ -86,11 +107,13 @@
         Poco::Timer m_timerSendManagerUpdate;
         Poco::Timer m_timerControllerUpdate;
         Poco::Timer m_timerSonarFrontUpdate;
+        Poco::Timer m_timerSendImuDataUpdate;
 
     private:
         void sendManagerUpdate( Poco::Timer& timer );
         void timeoutControllerUpdate( Poco::Timer& timer );
         void timeoutSonarFrontUpdate( Poco::Timer& timer );
+        void sendImuDataUpdate( Poco::Timer& timer );
         void recvHeartbeat( uint8_t srcNode );
         void recvControllerUpdate( CANMessage* msg );
         void recvButtonUpdate( CANMessage* msg );
--- a/main/robots/odr/jamfile	Wed Feb 06 21:46:34 2013 -0800
+++ b/main/robots/odr/jamfile	Sat Feb 09 14:29:59 2013 -0800
@@ -39,7 +39,7 @@
     ODRApp.cpp
     ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
-    AvoidBySonarTask.cpp CruisingTask.cpp SafetyTask.cpp StartupTask.cpp
+    AvoidBySonarTask.cpp CruisingTask.cpp NavigateTask.cpp SafetyTask.cpp StartupTask.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;