changeset 159:19626dab8d31 main

Pull from upstream.
author Bob Cook <bob@bobcookdev.com>
date Sat, 08 Sep 2012 13:20:14 -0700
parents 7fac5705d135 (current diff) 8fc49d9dcd43 (diff)
children a498438cc538
files
diffstat 17 files changed, 708 insertions(+), 131 deletions(-) [+]
line wrap: on
line diff
--- a/main/packages/common/can/can_messages.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/packages/common/can/can_messages.h	Sat Sep 08 13:20:14 2012 -0700
@@ -7,7 +7,7 @@
 // 
 //  Identifiers for Controller Area Network messages.
 //
-//  Copyright (c) 2011 Bob Cook
+//  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
@@ -41,6 +41,7 @@
 // 
 //      Emergency messages:     0x0001 to 0x000f
 //      General messages:       0x0010 to 0x001f
+//      IMU sensor:             0x0020 to 0x002f
 //      GPS sensor:             0x0100 to 0x010f
 //      Servo (front & rear):   0x0110 to 0x011f
 //      Motor (front & rear):   0x0120 to 0x012f
@@ -79,6 +80,20 @@
 
 // ----------------------------------------------------------------------------------------
 
+const uint16_t can_dataid_imu_roll  = 0x0020;
+const uint16_t can_dataid_imu_pitch = 0x0021;
+const uint16_t can_dataid_imu_yaw   = 0x0022;
+
+const int32_t  can_data_imu_multiplier = 10000;
+
+typedef struct
+{
+    int32_t data; // imu value * 10,000 (can_data_imu_multiplier)
+
+}   can_data_imu_data;
+
+// ----------------------------------------------------------------------------------------
+
 const uint16_t can_dataid_utc_timestamp = 0x0100;
 
 typedef struct
--- a/main/packages/linux/can/CANMessage.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/packages/linux/can/CANMessage.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -7,7 +7,7 @@
 //    
 //  CAN message object that can be queued or dequeued from multiple threads.
 //
-//  Copyright (c) 2010 Bob Cook
+//  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
@@ -51,6 +51,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+int CANMessage::SendQueueSize()
+{
+    return sm_MsgsToSend.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void CANMessage::QueueToSend( CANMessage* msg )
 {
     sm_MsgsToSend.enqueueNotification( msg );
@@ -66,6 +73,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+int CANMessage::ReceiveQueueSize()
+{
+    return sm_MsgsReceived.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void CANMessage::QueueReceived( CANMessage* msg )
 {
     sm_MsgsReceived.enqueueNotification( msg );
--- a/main/packages/linux/can/CANMessage.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/packages/linux/can/CANMessage.h	Sat Sep 08 13:20:14 2012 -0700
@@ -7,7 +7,7 @@
 //    
 //  CAN message object that can be queued or dequeued from multiple threads.
 //
-//  Copyright (c) 2011 Bob Cook
+//  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
@@ -45,8 +45,10 @@
 {
     public:
         static bool IsSendQueueEmpty();
+        static int  SendQueueSize();
         static void QueueToSend( CANMessage* msg );
         static CANMessage* WaitDequeueToSend( long timeoutMilliseconds );
+        static int  ReceiveQueueSize();
         static void QueueReceived( CANMessage* msg );
         static CANMessage* WaitDequeueReceived( long timeoutMilliseconds );
         static std::string asText( const CANMessage* msg );
--- a/main/robots/odr/AvoidBySonarTask.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -7,7 +7,7 @@
 //
 //  Virtual base class for a subsumption task object.
 //
-//  Copyright (c) 2011 Bob Cook
+//  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
@@ -37,9 +37,9 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const unsigned AVOIDANCE_ZONE = 0x0600;
-static const unsigned MAX_TURN_ZONE  = 0x0200;
-static const unsigned TRAPPED_ZONE   = 0x0100;
+static const unsigned AVOIDANCE_ZONE = 0x1000;
+static const unsigned MAX_TURN_ZONE  = 0x0800;
+static const unsigned TRAPPED_ZONE   = 0x0500;
 
 // ----------------------------------------------------------------------------------------
 
@@ -70,7 +70,8 @@
     : TaskObject( loggerName ),
       m_lastMessageTime(),
       m_frontLeft( 0 ),
-      m_frontCenter( 0 ),
+      m_frontCenterLeft( 0 ),
+      m_frontCenterRight( 0 ),
       m_frontRight( 0 )
 {
 }
@@ -90,9 +91,10 @@
         Sonar::setFrontEnabled();
     }
 
-    m_frontLeft   = Scoreboard::sonarFrontLeft();
-    m_frontCenter = Scoreboard::sonarFrontCenter();
-    m_frontRight  = Scoreboard::sonarFrontRight();
+    m_frontLeft        = Scoreboard::sonarFrontLeft();
+    m_frontCenterLeft  = Scoreboard::sonarFrontCenterLeft();
+    m_frontCenterRight = Scoreboard::sonarFrontCenterRight();
+    m_frontRight       = Scoreboard::sonarFrontRight();
 }
 
 // ----------------------------------------------------------------------------------------
@@ -100,7 +102,8 @@
 bool AvoidBySonarTask::wantsControl()
 {
     return ( m_frontLeft < AVOIDANCE_ZONE
-                || m_frontCenter < AVOIDANCE_ZONE
+                || m_frontCenterLeft < AVOIDANCE_ZONE
+                || m_frontCenterRight < AVOIDANCE_ZONE
                 || m_frontRight < AVOIDANCE_ZONE );
 }
 
@@ -109,7 +112,8 @@
 bool AvoidBySonarTask::isTrapped() const
 {
     return ( m_frontLeft < TRAPPED_ZONE
-                && m_frontCenter < TRAPPED_ZONE
+                && m_frontCenterLeft < TRAPPED_ZONE
+                && m_frontCenterRight < TRAPPED_ZONE
                 && m_frontRight < TRAPPED_ZONE );
 }
 
@@ -129,29 +133,19 @@
         return; // stuck!
     }
 
-    if ( m_frontCenter < AVOIDANCE_ZONE )
+    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 )
     {
-        int8_t turn = CalculateTurn( m_frontCenter );
-
-        if ( m_frontLeft < ( m_frontRight + 0x0010 ) ) // dead-zone of 0x0010
+        if ( minLeft < ( minRight + TRAPPED_ZONE ) )
         {
+            int8_t turn = CalculateTurn( minLeft + TRAPPED_ZONE );
             MotorsAndServos::servoPositions( -turn, turn );
         }
-        else
+        else if ( minRight < ( minLeft + TRAPPED_ZONE ) )
         {
-            MotorsAndServos::servoPositions( turn, -turn );
-        }
-    }
-    else if ( m_frontLeft < AVOIDANCE_ZONE && m_frontRight < AVOIDANCE_ZONE )
-    {
-        if ( m_frontLeft < ( m_frontRight + TRAPPED_ZONE ) )
-        {
-            int8_t turn = CalculateTurn( m_frontLeft + TRAPPED_ZONE );
-            MotorsAndServos::servoPositions( -turn, turn );
-        }
-        else if ( m_frontRight < ( m_frontLeft + TRAPPED_ZONE ) )
-        {
-            int8_t turn = CalculateTurn( m_frontRight + TRAPPED_ZONE );
+            int8_t turn = CalculateTurn( minRight + TRAPPED_ZONE );
             MotorsAndServos::servoPositions( turn, -turn );
         }
         else
@@ -159,14 +153,14 @@
             MotorsAndServos::servoPositions( 0, 0 ); // straight
         }
     }
-    else if ( m_frontLeft < AVOIDANCE_ZONE )
+    else if ( minLeft < AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( m_frontLeft );
+        int8_t turn = CalculateTurn( minLeft );
         MotorsAndServos::servoPositions( -turn, turn );
     }
-    else if ( m_frontRight < AVOIDANCE_ZONE )
+    else if ( minRight < AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( m_frontRight );
+        int8_t turn = CalculateTurn( minRight );
         MotorsAndServos::servoPositions( turn, -turn );
     }
     else
--- a/main/robots/odr/AvoidBySonarTask.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/AvoidBySonarTask.h	Sat Sep 08 13:20:14 2012 -0700
@@ -51,7 +51,8 @@
     private:
         Poco::Timestamp m_lastMessageTime;
         unsigned        m_frontLeft;
-        unsigned        m_frontCenter;
+        unsigned        m_frontCenterLeft;
+        unsigned        m_frontCenterRight;
         unsigned        m_frontRight;
 };
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuReader.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -0,0 +1,497 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuReader.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
+//
+//  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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "ImuReader.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+#include <sys/select.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <stdexcept>
+
+#include <Poco/Exception.h>
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+#include <Poco/Thread.h>
+
+#include "Scoreboard.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/common/util/misc.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+ImuReader::ImuReader( const std::string& loggerName, const std::string& serialPortPath )
+    : Poco::Runnable(),
+      m_loggerName( loggerName ),
+      m_serialPortPath( serialPortPath ),
+      m_quitEvent( false /* not auto-reset */ ),
+      m_serialPortFd( -1 /* not open */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuReader::~ImuReader()
+{
+    closeSerialPort();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::closeSerialPort()
+{
+    if ( m_serialPortFd >= 0 )
+    {
+        close( m_serialPortFd );
+        m_serialPortFd = -1;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuReader::configureSerialPort()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    closeSerialPort();
+
+    m_serialPortFd = open( m_serialPortPath.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK );
+
+    if ( m_serialPortFd < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::configureSerialPort() failed to open port \"$0\": $1",
+                m_serialPortPath,
+                std::string( strerror( errno ) ) ) );
+        return false;
+    }
+
+    // the SparkFun 9-DOF Razor IMU is configured with software from WebBot:
+    // http://webbot.org.uk/iPoint/49.page
+    //
+    // 115200 baud, 8 bits, no stop bits
+
+    termios settings;
+    bzero( &settings, sizeof( settings ) );
+
+    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
+
+    cfsetspeed( &settings, B115200 );
+
+    if ( tcsetattr( m_serialPortFd, TCSANOW, &settings ) < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::configureSerialPort() failed to configure port \"$0\": $1",
+                m_serialPortPath,
+                std::string( strerror( errno ) ) ) );
+
+        closeSerialPort();
+        return false;
+    }
+
+    log.information( "ImuReader::configureSerialPort() successfully opened \""
+                     + m_serialPortPath + "\"" );
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuReader::readFromSerialPort( char* buffer, size_t* length )
+{
+    fd_set emptyFds;
+    FD_ZERO( &emptyFds );
+
+    fd_set readableFds;
+    FD_ZERO( &readableFds );
+    FD_SET( m_serialPortFd, &readableFds );
+
+    struct timeval tv;
+    tv.tv_sec  = 0;
+    tv.tv_usec = 250 * 1000; // 250ms as microseconds
+
+    int result = select( m_serialPortFd + 1, &readableFds, &emptyFds, &emptyFds, &tv );
+
+    if ( result == 0 )
+    {
+        *length = 0;
+        return true; // timeout ok, but no bytes
+    }
+    else if ( result < 0 )
+    {
+        Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::readFromSerialPort() failed to select on port: $0",
+                std::string( strerror( errno ) ) ) );
+
+        return false;
+    }
+
+    ssize_t bytesRead = read( m_serialPortFd, buffer, *length );
+
+    if ( bytesRead < 0 )
+    {
+        if ( errno == EAGAIN || errno == EWOULDBLOCK )
+        {
+            *length = 0;
+            return true; // timeout ok, but no bytes
+        }
+
+        Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::readFromSerialPort() failed to read from port: $0",
+                std::string( strerror( errno ) ) ) );
+
+        return false;
+    }
+
+    *length = static_cast< size_t >( bytesRead );
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::resetParseState()
+{
+    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+}
+
+// ----------------------------------------------------------------------------------------
+//  the data is in the form of:
+//
+//      !ANG:roll,pitch,yaw,MAG:x,y,z<eoln>
+//
+//  we parse out the roll, pitch, and yaw values, then ignore the rest.
+//
+
+bool ImuReader::processImuData( const char* data, size_t length )
+{
+    bool completedNewValues = false;
+
+    while ( length-- )
+    {
+        char ch = *data++;
+
+        switch ( m_parseState )
+        {
+            case PARSE_STATE_LOOKING_FOR_START:
+                if ( ch == '!' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_A;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_A:
+                if ( ch == 'A' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_N;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_N:
+                if ( ch == 'N' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_G;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_G:
+                if ( ch == 'G' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_COLON;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_COLON:
+                if ( ch == ':' )
+                {
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_ROLL;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_READ_ROLL:
+                if ( ch == ',' )
+                {
+                    m_parseValueRoll = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_PITCH;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            case PARSE_STATE_READ_PITCH:
+                if ( ch == ',' )
+                {
+                    m_parseValuePitch = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_YAW;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            case PARSE_STATE_READ_YAW:
+                if ( ch == ',' )
+                {
+                    m_parseValueYaw = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                    completedNewValues = true;;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            default:
+                m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                break;
+        }
+
+        // sanity check our working buffer; too big == error
+
+        if ( m_parseToken.length() > 20 )
+        {
+            m_parseState = PARSE_STATE_LOOKING_FOR_START;
+        }
+    }
+    
+    return completedNewValues;
+}
+
+// ----------------------------------------------------------------------------------------
+//  Yaw values range from 0 to 180 then from -180 to 0 again. This means the values near
+//  zero are very close together (as the sensor turns) but so are values at 180/-180.
+//
+//  For values that aren't approaching 180, a standard average is ok. But for the values
+//  on either side of the 180/-180 boundry we compute the average delta from 180/-180.
+
+static double computeAverageYaw( const std::vector< double >& samples )
+{
+    bool valuesAreNearMax = false;
+    bool valuesAreNearNegMax = false;
+
+    for ( std::vector< double >::const_iterator itr = samples.begin();
+          itr != samples.end();
+          ++itr )
+    {
+        if ( *itr > (  180.0 - 10.0 ) ) valuesAreNearMax = true;
+        if ( *itr < ( -180.0 + 10.0 ) ) valuesAreNearNegMax = true;
+    }
+
+    if ( valuesAreNearMax && valuesAreNearNegMax )
+    {
+        // wrap-around case computes the delta from 180/-180
+
+        double deltaValuesSum = 0.0;
+
+        for ( std::vector< double >::const_iterator itr = samples.begin();
+              itr != samples.end();
+              ++itr )
+        {
+            if ( *itr < 0.0 )
+            {
+                deltaValuesSum += ( -180.0 - *itr );
+            }
+            else
+            {
+                deltaValuesSum += ( 180.0 - *itr );
+            }
+        }
+
+        if ( deltaValuesSum < 0.0 )
+        {
+            return -180.0 - ( deltaValuesSum / samples.size() );
+        }
+        else
+        {
+            return 180.0 - ( deltaValuesSum / samples.size() );
+        }
+    }
+
+    // just compute a regular average
+
+    double valuesSum = 0.0;
+
+    for ( std::vector< double >::const_iterator itr = samples.begin();
+          itr != samples.end();
+          ++itr )
+    {
+        valuesSum += *itr;
+    }
+
+    return valuesSum / samples.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::run()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    for ( ;; )
+    {
+        log.information( std::string( "ImuReader::run() starting" ) );
+
+        try
+        {
+            if ( ! configureSerialPort() )
+            {
+                Poco::Thread::sleep( 10000 ); // 10 sec
+                continue;
+            }
+
+            resetParseState();
+
+            // keep the last 10 values to average together (approx 0.5 seconds elapsed)
+
+            std::vector< double > valuesYaw;
+            valuesYaw.reserve( 10 );
+
+            // loop while we are still getting data
+
+            for ( ;; )
+            {
+                if ( m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "ImuReader::run() stopping" ) );
+                    return;
+                }
+
+                char   buffer[ 64 ];
+                size_t length = array_sizeof( buffer );
+
+                if ( ! readFromSerialPort( buffer, &length ) )
+                {
+                    break; // error, drop out and reinit the port
+                }
+
+                if ( processImuData( buffer, length ) )
+                {
+                    // new values available, save them
+
+                    valuesYaw.push_back( m_parseValueYaw );
+
+                    // if we've reached 10 values, compute the average and update the
+                    // scoreboard; then clear the vector making room for new values
+
+                    if ( valuesYaw.size() == 10 )
+                    {
+                        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 ) ) );
+
+                        // clear the vector, to accumulate more samples
+
+                        valuesYaw.clear();
+                    }
+                }
+            }
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "ImuReader::run() caught Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "ImuReader::run() caught std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            log.error( "ImuReader::run() caught unknown exception" );
+        }
+
+        // sleep for 1/2 second after exception processing, but don't exit
+        Poco::Thread::sleep( 500 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuReader.h	Sat Sep 08 13:20:14 2012 -0700
@@ -0,0 +1,88 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuReader.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
+//
+//  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_IMUREADER_H
+#define BCDRL_ROBOTS_ODR_IMUREADER_H
+
+#include <stdint.h>
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+#include <Poco/Timer.h>
+
+// ----------------------------------------------------------------------------------------
+
+class ImuReader : public Poco::Runnable
+{
+    public:
+        ImuReader( const std::string& loggerName, const std::string& serialPortPath );
+        virtual ~ImuReader();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        typedef enum
+        {
+            PARSE_STATE_LOOKING_FOR_START,
+            PARSE_STATE_START_SYMBOL_A,
+            PARSE_STATE_START_SYMBOL_N,
+            PARSE_STATE_START_SYMBOL_G,
+            PARSE_STATE_START_SYMBOL_COLON,
+            PARSE_STATE_READ_ROLL,
+            PARSE_STATE_READ_PITCH,
+            PARSE_STATE_READ_YAW
+
+        }   imu_parse_state_t;
+
+    private:
+        std::string       m_loggerName;
+        std::string       m_serialPortPath;
+        Poco::Event       m_quitEvent;
+        int               m_serialPortFd;
+        imu_parse_state_t m_parseState;
+        std::string       m_parseToken;
+        double            m_parseValueRoll;
+        double            m_parseValuePitch;
+        double            m_parseValueYaw;
+
+    private:
+        void closeSerialPort();
+        bool configureSerialPort();
+        bool readFromSerialPort( char* buffer, size_t* length );
+        void resetParseState();
+        bool processImuData( const char* data, size_t length );
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_IMUREADER_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/MotorsAndServos.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/MotorsAndServos.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -111,3 +111,17 @@
 
 // ----------------------------------------------------------------------------------------
 
+int8_t MotorsAndServos::maxSpeed()
+{
+    return 128;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t MotorsAndServos::minSpeed()
+{
+    return 32;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/MotorsAndServos.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/MotorsAndServos.h	Sat Sep 08 13:20:14 2012 -0700
@@ -41,6 +41,8 @@
     public:
         static void servoPositions( int8_t front, int8_t rear );
         static void motorSpeeds( int8_t front, int8_t rear );
+        static int8_t maxSpeed();
+        static int8_t minSpeed();
 
     private:
         static bool   sm_neverSentServoCmd;
--- a/main/robots/odr/ODRApp.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/ODRApp.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -47,6 +47,7 @@
 
 #include "AvoidBySonarTask.h"
 #include "CruisingTask.h"
+#include "ImuReader.h"
 #include "MotorsAndServos.h"
 #include "SafetyTask.h"
 #include "Scoreboard.h"
@@ -143,6 +144,21 @@
     Scoreboard   scoreboard( logger().name() );
     scoreboardThread.start( scoreboard );
 
+    std::string imuSerialPortPath = config().getString( "imu.serialPortPath", "" );
+
+    Poco::Thread imuReaderThread;
+    ImuReader    imuReader( logger().name(), imuSerialPortPath );
+
+    if ( ! imuSerialPortPath.empty() )
+    {
+        logger().information( "using IMU serial port \"" + imuSerialPortPath + "\"" );
+        imuReaderThread.start( imuReader );
+    }
+    else
+    {
+        logger().information( "no IMU serial port configured; disabled" );
+    }
+
     m_tasks.clear();
 
     m_tasks.push_back(
@@ -164,33 +180,6 @@
     m_runloopActive = true;
     while ( m_runloopActive ) Poco::Thread::sleep( 5000 );
 
-#if 0
-
-    while ( m_runloopActive )
-    {
-        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" );
-        }
-    }
-#endif
-
     Scoreboard::sendManagerMessage( "Goodbye" );
     Poco::Thread::sleep( 5000 );
 
@@ -207,63 +196,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-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" );
-        Sonar::setFrontEnabled();
-
-        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" );
-        Sonar::setFrontDisabled();
-        Poco::Thread::sleep( 5000 );
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
 void ODRApp::runTasks( Poco::Timer& timer )
 {
     // refresh all the tasks
--- a/main/robots/odr/ODRApp.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/ODRApp.h	Sat Sep 08 13:20:14 2012 -0700
@@ -68,7 +68,6 @@
         typedef std::vector< Poco::SharedPtr< TaskObject > > TaskVectorType;
 
     private:
-        void runloop();
         void runTasks( Poco::Timer& timer );
 
     private:
--- a/main/robots/odr/Scoreboard.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -53,7 +53,8 @@
 bool         Scoreboard::sm_isSonarFrontAlive;
 bool         Scoreboard::sm_isSonarFrontEnabled;
 uint16_t     Scoreboard::sm_sonarFrontLeft;
-uint16_t     Scoreboard::sm_sonarFrontCenter;
+uint16_t     Scoreboard::sm_sonarFrontCenterLeft;
+uint16_t     Scoreboard::sm_sonarFrontCenterRight;
 uint16_t     Scoreboard::sm_sonarFrontRight;
 
 // ----------------------------------------------------------------------------------------
@@ -134,10 +135,18 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontCenter()
+uint16_t Scoreboard::sonarFrontCenterLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_sonarFrontCenter;
+    return sm_sonarFrontCenterLeft;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontCenterRight()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontCenterRight;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -274,9 +283,10 @@
     can_data_sonar_front info;
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
-    sm_sonarFrontLeft   = info.left;
-    sm_sonarFrontCenter = info.center;
-    sm_sonarFrontRight  = info.right;
+    sm_sonarFrontLeft        = info.left;
+    sm_sonarFrontCenterLeft  = info.center_left;
+    sm_sonarFrontCenterRight = info.center_right;
+    sm_sonarFrontRight       = info.right;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -336,6 +346,7 @@
                         break;
 
                     case can_dataid_sonar_front:
+                        recvSonarFrontStatus( true /* enabled */ );
                         recvSonarFrontValues( msg );
                         break;
                 }
--- a/main/robots/odr/Scoreboard.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/Scoreboard.h	Sat Sep 08 13:20:14 2012 -0700
@@ -57,7 +57,8 @@
         static bool     isSonarFrontAlive();
         static bool     isSonarFrontEnabled();
         static uint16_t sonarFrontLeft();
-        static uint16_t sonarFrontCenter();
+        static uint16_t sonarFrontCenterLeft();
+        static uint16_t sonarFrontCenterRight();
         static uint16_t sonarFrontRight();
 
     public:
@@ -75,7 +76,8 @@
         static bool         sm_isSonarFrontAlive;
         static bool         sm_isSonarFrontEnabled;
         static uint16_t     sm_sonarFrontLeft;
-        static uint16_t     sm_sonarFrontCenter;
+        static uint16_t     sm_sonarFrontCenterLeft;
+        static uint16_t     sm_sonarFrontCenterRight;
         static uint16_t     sm_sonarFrontRight;
 
     private:
--- a/main/robots/odr/TaskObject.cpp	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/TaskObject.cpp	Sat Sep 08 13:20:14 2012 -0700
@@ -33,7 +33,8 @@
 
 // ----------------------------------------------------------------------------------------
 
-const Poco::Timestamp::TimeDiff TaskObject::TwoSeconds = Poco::Timestamp::resolution() * 2;
+const Poco::Timestamp::TimeDiff TaskObject::TwoSeconds  = Poco::Timestamp::resolution()* 2;
+const Poco::Timestamp::TimeDiff TaskObject::FiveSeconds = Poco::Timestamp::resolution()* 5;
 
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/odr/TaskObject.h	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/TaskObject.h	Sat Sep 08 13:20:14 2012 -0700
@@ -46,6 +46,7 @@
 
     public:
         static const Poco::Timestamp::TimeDiff TwoSeconds;
+        static const Poco::Timestamp::TimeDiff FiveSeconds;
 
     public:
         TaskObject( const std::string& loggerName );
--- a/main/robots/odr/jamfile	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/jamfile	Sat Sep 08 13:20:14 2012 -0700
@@ -36,7 +36,8 @@
 
 COMMON_SOURCES = 
     main.cpp
-    ODRApp.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
+    ODRApp.cpp
+    ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
     AvoidBySonarTask.cpp CruisingTask.cpp SafetyTask.cpp StartupTask.cpp
     packages.common.can.pkg
--- a/main/robots/odr/odr_ubuntu.xml	Sat Sep 08 13:19:43 2012 -0700
+++ b/main/robots/odr/odr_ubuntu.xml	Sat Sep 08 13:20:14 2012 -0700
@@ -32,5 +32,8 @@
         <logIncoming>no</logIncoming>
         <logOutgoing>no</logOutgoing>
     </can>
+    <imu>
+        <serialPortPath>/dev/ttyUSB0</serialPortPath>
+    </imu>
 </config>