changeset 236:493f994c999a main

Merge from upstream
author Bob Cook <bob@bobcookdev.com>
date Sun, 13 Jul 2014 09:15:53 -0700
parents 8af48f7c2f34 (current diff) 5657cf2325c9 (diff)
children 43f43ebe35df
files main/robots/odr-sim/ControllerStatus.cpp main/robots/odr-sim/ControllerStatus.h
diffstat 45 files changed, 3275 insertions(+), 809 deletions(-) [+]
line wrap: on
line diff
--- a/main/packages/common/can/can_messages.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/packages/common/can/can_messages.h	Sun Jul 13 09:15:53 2014 -0700
@@ -34,6 +34,8 @@
 
 #include <stdint.h>
 
+#pragma pack(push,1)
+
 // ----------------------------------------------------------------------------------------
 //  Overall scheme (lower is higher priority):
 // 
@@ -205,6 +207,10 @@
 } can_data_sonar_front;
 
 // ----------------------------------------------------------------------------------------
+
+#pragma pack(pop)
+
+// ----------------------------------------------------------------------------------------
 #endif  // #if !defined( BCDRL_COMMON_CAN_CAN_MESSAGES_H )
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/cantool/CanToolApp.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/cantool/CanToolApp.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Application object implementation for demonstrating the CANSocket class.    
 //
-//  Copyright (c) 2011-2013 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -72,9 +72,10 @@
     m_commandDispatchTable[ "estop" ] = CommandEStop;
     m_commandDispatchTable[ "gps" ] = CommandGps;
     m_commandDispatchTable[ "listen" ] = CommandListen;
+    m_commandDispatchTable[ "sonarfront" ] = CommandSonarFront;
     m_commandDispatchTable[ "speed" ] = CommandSpeed;
+    m_commandDispatchTable[ "start" ] = CommandStartPgm;
     m_commandDispatchTable[ "steer" ] = CommandSteer;
-    m_commandDispatchTable[ "sonarfront" ] = CommandSonarFront;
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/cantool/CmdGps.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/cantool/CmdGps.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -60,7 +60,7 @@
 {
     uint32_t msgid = can_build_message_id( can_node_sensor_gps,
                                            can_node_broadcast,
-                                           can_dataid_latitude );
+                                           can_dataid_gps_latitude );
 
     can_data_gps_data msg;
 
@@ -104,7 +104,7 @@
 {
     uint32_t msgid = can_build_message_id( can_node_sensor_gps,
                                            can_node_broadcast,
-                                           can_dataid_longitude );
+                                           can_dataid_gps_longitude );
 
     can_data_gps_data msg;
 
--- a/main/robots/cantool/CmdListen.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/cantool/CmdListen.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Application object implementation for demonstrating the CANSocket class.    
 //
-//  Copyright (c) 2012-2013 Bob Cook
+//  Copyright (c) 2012-2014 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
@@ -66,6 +66,9 @@
         case can_node_odr_controller:
             return "odr-controller";
 
+        case can_node_odr_display:
+            return "odr-display";
+
         case can_node_odr_motion:
             return "odr-motion";
 
@@ -88,10 +91,11 @@
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
     int    deg = info.degrees;
-    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+    double min = static_cast< double >( info.min_thousandths )
+               / can_data_gps_min_multiplier;
 
     std::cout << "gps latitude:  " << Poco::NumberFormatter::format( deg )
-        << " " << Poco::NumberFormatter::format( min )
+        << " " << Poco::NumberFormatter::format( min, 4 )
         << std::endl;
 }
 
@@ -103,10 +107,11 @@
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
     int    deg = info.degrees;
-    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+    double min = static_cast< double >( info.min_thousandths )
+               / can_data_gps_min_multiplier;
 
     std::cout << "gps longitude: " << Poco::NumberFormatter::format( deg )
-        << " " << Poco::NumberFormatter::format( min )
+        << " " << Poco::NumberFormatter::format( min, 4 )
         << std::endl;
 }
 
@@ -123,6 +128,33 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void MsgStartProgram( const CANMessage* msg )
+{
+    can_data_pgm_selection info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    switch ( info.pgm_id )
+    {
+        case odr_pgm_wander:
+            std::cout << "start program wander" << std::endl;
+            break;
+
+        case odr_pgm_straight_line:
+            std::cout << "start program staight line" << std::endl;
+            break;
+
+        case odr_pgm_small_box:
+            std::cout << "start program small box" << std::endl;
+            break;
+
+        default:
+            std::cout << "start program (unknown)" << std::endl;
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void MsgSonarFrontUpdate( const CANMessage* msg )
 {
     can_data_sonar_front info;
@@ -181,18 +213,24 @@
                 std::cout << "heartbeat from " << NodeName( srcNode ) << std::endl;
                 break;
 
-            case can_dataid_latitude:
+            case can_dataid_gps_latitude:
                 MsgGpsLatitude( msg );
+                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                 break;
 
-            case can_dataid_longitude:
+            case can_dataid_gps_longitude:
                 MsgGpsLongitude( msg );
+                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                 break;
 
             case can_dataid_gps_fix:
                 MsgGpsFix( msg );
                 break;
 
+            case can_dataid_odr_start_pgm:
+                MsgStartProgram( msg );
+                break;
+
             case can_dataid_sonar_front:
                 MsgSonarFrontUpdate( msg );
                 break;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdStartPgm.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,106 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdStartPgm.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "start" implementation.
+//
+//  Copyright (c) 2014 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 <iostream>
+
+#include <Poco/Exception.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+
+#include "Commands.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+static void PrintHelp()
+{
+    std::cerr << "error: start <program>" << std::endl;
+    std::cerr << "       where \"program\" may be one of:" << std::endl;
+    std::cerr << "              wander" << std::endl;
+    std::cerr << "              line" << std::endl;
+    std::cerr << "              smbox" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CommandStartPgm( const std::vector< std::string >& args )
+{
+    if ( args.size() != 2 )
+    {
+        PrintHelp();
+        return;
+    }
+
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_display,
+                                           can_dataid_odr_start_pgm );
+    can_data_pgm_selection msg;
+
+    try
+    {
+        if ( args[ 1 ] == "wander" )
+        {
+            msg.pgm_id = odr_pgm_wander;
+        }
+        else if ( args[ 1 ] == "line" )
+        {
+            msg.pgm_id = odr_pgm_straight_line;
+        }
+        else if ( args[ 1 ] == "smbox" )
+        {
+            msg.pgm_id = odr_pgm_small_box;
+        }
+        else
+        {
+            throw Poco::SyntaxException( "unrecognized program name", 1 );
+        }
+
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent start cmd " << args[ 1 ] << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/Commands.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/cantool/Commands.h	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Command function prototypes.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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,5 +45,7 @@
 
 void CommandGps( const std::vector< std::string >& args );
 
+void CommandStartPgm( const std::vector< std::string >& args );
+
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/cantool/jamfile	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/cantool/jamfile	Sun Jul 13 09:15:53 2014 -0700
@@ -38,6 +38,7 @@
     main.cpp
     CanToolApp.cpp
     CmdEStop.cpp CmdGps.cpp CmdListen.cpp CmdSpeed.cpp CmdSonarFront.cpp CmdSteer.cpp
+    CmdStartPgm.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;
--- a/main/robots/odr-sim/ControllerStatus.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,167 +0,0 @@
-// ----------------------------------------------------------------------------------------
-//
-//  robots/odr-sim/ControllerStatus.cpp
-//    
-//  Bob Cook Development, Robotics Library
-//  http://www.bobcookdev.com/rl/
-// 
-//  Thread that periodically sends the controller "status" message.
-//
-//  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 "ControllerStatus.h"
-
-#include "packages/common/can/can_helpers.h"
-#include "packages/common/can/can_messages.h"
-#include "packages/common/can/can_nodes.h"
-
-#include "packages/linux/can/CANMessage.h"
-
-// ----------------------------------------------------------------------------------------
-
-Poco::RWLock ControllerStatus::sm_rwLock;
-bool         ControllerStatus::sm_isEstopActive;
-bool         ControllerStatus::sm_isMotorCtlActive;
-bool         ControllerStatus::sm_isButtonOneDown;
-bool         ControllerStatus::sm_isButtonTwoDown;
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::sendButtonStatusMessage()
-{
-    uint32_t msgid = can_build_message_id( can_node_odr_controller,
-                                           can_node_broadcast,
-                                           can_dataid_odrctl_button );
-
-    can_data_odrctl_button info;
-
-    info.button_1 = sm_isButtonOneDown;
-    info.button_2 = sm_isButtonTwoDown;
-
-    CANMessage::QueueToSend(
-        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &info ), sizeof( info ) ) );
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::setEstopActive( bool active )
-{
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-    sm_isEstopActive = active;
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::setMotorCtlActive( bool active )
-{
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-    sm_isMotorCtlActive = active;
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::setButtonOne( bool down )
-{
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-
-    if ( down != sm_isButtonOneDown )
-    {
-        sm_isButtonOneDown = down;
-        sendButtonStatusMessage();
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::setButtonTwo( bool down )
-{
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-
-    if ( down != sm_isButtonTwoDown )
-    {
-        sm_isButtonTwoDown = down;
-        sendButtonStatusMessage();
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
-ControllerStatus::ControllerStatus()
-    : Poco::Runnable(),
-      m_quitEvent( true /* auto-reset */ )
-{
-}
-
-// ----------------------------------------------------------------------------------------
-
-ControllerStatus::~ControllerStatus()
-{
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::timeToQuit()
-{
-    m_quitEvent.set();
-}
-
-// ----------------------------------------------------------------------------------------
-
-void ControllerStatus::run()
-{
-    for ( ;; )
-    {
-        try
-        {
-            uint32_t msgid = can_build_message_id( can_node_odr_controller,
-                                                   can_node_broadcast,
-                                                   can_dataid_odrctl_update );
-
-            can_data_odrctl_update info;
-
-            {
-                Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-
-                info.estop    = sm_isEstopActive;
-                info.motorctl = sm_isMotorCtlActive;
-            }
-
-            CANMessage::QueueToSend(
-                new CANMessage( msgid,
-                                reinterpret_cast< uint8_t* >( &info ), 
-                                sizeof( info ) ) );
-        }
-        catch ( ... )
-        {
-            // nothing to do, but don't stop the thread
-        }
-
-        if ( m_quitEvent.tryWait( 2500 ) ) // 2500 ms == 2.5s
-        {
-            return;
-        }
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
--- a/main/robots/odr-sim/ControllerStatus.h	Sat Jul 12 13:20:14 2014 -0700
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,72 +0,0 @@
-// ----------------------------------------------------------------------------------------
-//
-//  robots/odr-sim/ControllerStatus.h
-//    
-//  Bob Cook Development, Robotics Library
-//  http://www.bobcookdev.com/rl/
-// 
-//  Thread that periodically checks status and issues "heartbeat" messages.
-//
-//  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.
-//
-// ----------------------------------------------------------------------------------------
-
-#ifndef BCDRL_ROBOTS_ODRSIM_CONTROLLERSTATUS_H
-#define BCDRL_ROBOTS_ODRSIM_CONTROLLERSTATUS_H
-
-#include <Poco/Event.h>
-#include <Poco/Runnable.h>
-#include <Poco/RWLock.h>
-
-// ----------------------------------------------------------------------------------------
-
-class ControllerStatus : public Poco::Runnable
-{
-    public:
-        static void setEstopActive( bool active );
-        static void setMotorCtlActive( bool active );
-        static void setButtonOne( bool down );
-        static void setButtonTwo( bool down );
-
-    public:
-        ControllerStatus();
-        virtual ~ControllerStatus();
-        virtual void run();
-        void timeToQuit();
-
-    private:
-        static void sendButtonStatusMessage();
-
-    private:
-        static Poco::RWLock sm_rwLock;
-        static bool         sm_isEstopActive;
-        static bool         sm_isMotorCtlActive;
-        static bool         sm_isButtonOneDown;
-        static bool         sm_isButtonTwoDown;
-
-    private:
-        Poco::Event m_quitEvent;
-};
-
-// ----------------------------------------------------------------------------------------
-#endif // #ifndef BCDRL_ROBOTS_ODRSIM_CONTROLLERSTATUS_H
-// ----------------------------------------------------------------------------------------
-
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/DisplayStatus.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,95 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/DisplayStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "ODR display" node.
+//
+//  Copyright (c) 2014 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 "DisplayStatus.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock DisplayStatus::sm_rwLock;
+
+// ----------------------------------------------------------------------------------------
+
+DisplayStatus::DisplayStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+DisplayStatus::~DisplayStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void DisplayStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void DisplayStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+            Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+            uint32_t msgid = can_build_message_id( can_node_odr_display,
+                                                   can_node_broadcast,
+                                                   can_dataid_heartbeat );
+
+            CANMessage::QueueToSend( new CANMessage( msgid ) );
+
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 2500 /* milliseconds */ ) )
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/DisplayStatus.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,62 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/DisplayStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "ODR display" node.
+//
+//  Copyright (c) 2014 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_ODRSIM_DISPLAYSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_DISPLAYSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class DisplayStatus : public Poco::Runnable
+{
+    public:
+        // static void set...( bool value );
+
+    public:
+        DisplayStatus();
+        virtual ~DisplayStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_DISPLAYSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/GpsStatus.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,95 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/GpsStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "ODR display" node.
+//
+//  Copyright (c) 2014 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 "GpsStatus.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock GpsStatus::sm_rwLock;
+
+// ----------------------------------------------------------------------------------------
+
+GpsStatus::GpsStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+GpsStatus::~GpsStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void GpsStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void GpsStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+            Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+            uint32_t msgid = can_build_message_id( can_node_sensor_gps,
+                                                   can_node_broadcast,
+                                                   can_dataid_heartbeat );
+
+            CANMessage::QueueToSend( new CANMessage( msgid ) );
+
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 1250 /* milliseconds */ ) )
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/GpsStatus.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,62 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/GpsStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "GPS sensor" node.
+//
+//  Copyright (c) 2014 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_ODRSIM_GPSSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_GPSSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class GpsStatus : public Poco::Runnable
+{
+    public:
+        // static void set...( bool value );
+
+    public:
+        GpsStatus();
+        virtual ~GpsStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_GPSSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/ImuStatus.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,184 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/ImuStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the IMU details.
+//
+//  Copyright (c) 2013-2014 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 "ImuStatus.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"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+static void SendImuUpdate()
+{
+    // 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 >( ImuStatus::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 >( ImuStatus::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 >( ImuStatus::imuRoll() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock ImuStatus::sm_rwLock;
+double       ImuStatus::sm_yaw   = 0.0;
+double       ImuStatus::sm_pitch = 0.0;
+double       ImuStatus::sm_roll  = 0.0;
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuYaw()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_yaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuPitch()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_pitch;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuRoll()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_roll;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuYaw( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_yaw = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuPitch( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_pitch = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuRoll( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_roll = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuStatus::ImuStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuStatus::~ImuStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+            SendImuUpdate();
+
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 750 ) ) // 500ms = 1/2 second
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/ImuStatus.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,77 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/ImuStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the IMU details.
+//
+//  Copyright (c) 2013-2014 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_ODRSIM_IMUSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_IMUSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class ImuStatus : public Poco::Runnable
+{
+    public:
+        static bool   isImuEnabled();
+        static void   setImuEnabled();
+        static void   setImuDisabled();
+        static double imuYaw();
+        static double imuPitch();
+        static double imuRoll();
+        static void   setImuYaw( double value );
+        static void   setImuPitch( double value );
+        static void   setImuRoll( double value );
+
+    public:
+        ImuStatus();
+        virtual ~ImuStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static void sendImuData();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+        static bool         sm_isEnabled;
+        static double       sm_yaw;
+        static double       sm_pitch;
+        static double       sm_roll;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_IMUSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/MotionStatus.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,95 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/MotionStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "ODR motion controller" node.
+//
+//  Copyright (c) 2014 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 "MotionStatus.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock MotionStatus::sm_rwLock;
+
+// ----------------------------------------------------------------------------------------
+
+MotionStatus::MotionStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+MotionStatus::~MotionStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void MotionStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void MotionStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+            Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+            uint32_t msgid = can_build_message_id( can_node_odr_motion,
+                                                   can_node_broadcast,
+                                                   can_dataid_heartbeat );
+
+            CANMessage::QueueToSend( new CANMessage( msgid ) );
+
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 1250 /* milliseconds */ ) )
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/MotionStatus.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,62 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/MotionStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that emulates the "ODR motion controller" node.
+//
+//  Copyright (c) 2014 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_ODRSIM_MOTIONSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_MOTIONSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class MotionStatus : public Poco::Runnable
+{
+    public:
+        // static void set...( bool value );
+
+    public:
+        MotionStatus();
+        virtual ~MotionStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_MOTIONSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sim/Receiver.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/Receiver.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Update status from incoming CAN messages.
 //
-//  Copyright (c) 2011 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
@@ -89,27 +89,27 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Receiver::recvMotorSpeed( CANMessage* msg )
+void Receiver::recvWheelSpeed( CANMessage* msg )
 {
-    can_data_motor_speed data;
+    can_data_wheel_speed data;
     msg->msgData( reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 
     if ( m_display )
     {
-        m_display->updateMotorSpeeds( data.motor_front, data.motor_rear );
+        m_display->updateWheelSpeeds( data.rpm_front, data.rpm_rear );
     }
 }
 
 // ----------------------------------------------------------------------------------------
 
-void Receiver::recvServoPosition( CANMessage* msg )
+void Receiver::recvWheelPosition( CANMessage* msg )
 {
-    can_data_servo_position data;
+    can_data_wheel_position data;
     msg->msgData( reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 
     if ( m_display )
     {
-        m_display->updateServoPositions( data.servo_front, data.servo_rear );
+        m_display->updateWheelPositions( data.angle_front, data.angle_rear );
     }
 }
 
@@ -165,12 +165,12 @@
                     recvMgrUpdate( msg );
                     break;
 
-                case can_dataid_motor_speed:
-                    recvMotorSpeed( msg );
+                case can_dataid_wheel_speed:
+                    recvWheelSpeed( msg );
                     break;
 
-                case can_dataid_servo_position:
-                    recvServoPosition( msg );
+                case can_dataid_wheel_position:
+                    recvWheelPosition( msg );
                     break;
 
                 case can_dataid_sonar_front_enable:
--- a/main/robots/odr-sim/Receiver.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/Receiver.h	Sun Jul 13 09:15:53 2014 -0700
@@ -60,8 +60,8 @@
 
     private:
         void recvMgrUpdate( CANMessage* msg );
-        void recvServoPosition( CANMessage* msg );
-        void recvMotorSpeed( CANMessage* msg );
+        void recvWheelPosition( CANMessage* msg );
+        void recvWheelSpeed( CANMessage* msg );
         void recvSonarFrontStateChange( bool enabled );
 };
 
--- a/main/robots/odr-sim/SimDisplay.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/SimDisplay.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Window display for the ODR platform simulator.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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,6 +37,7 @@
 #include <FL/Fl.H>
 #include <FL/Fl_Box.H>
 #include <FL/Fl_Button.H>
+#include <FL/Fl_Dial.H>
 #include <FL/Fl_Double_Window.H>
 #include <FL/Fl_Hor_Slider.H>
 #include <FL/Fl_Light_Button.H>
@@ -47,15 +48,70 @@
 
 #include <Poco/Thread.h>
 
-#include "ControllerStatus.h"
+#include "DisplayStatus.h"
+#include "GpsStatus.h"
+#include "ImuStatus.h"
+#include "MotionStatus.h"
 #include "SonarFrontStatus.h"
 
 // ----------------------------------------------------------------------------------------
 
-static void SendControllerUpdateCB( Fl_Widget* widget, void* value )
+static void DoDisplayStatusCB( Fl_Widget* widget, void* value )
+{
+    static DisplayStatus s_dsRunnable;
+    static Poco::Thread  s_dsThread;
+
+    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
+    if ( button == 0 )
+    {
+        return; // oops, not what we thought?!
+    }
+
+    if ( button->value() == 0 )
+    {
+        // off
+        s_dsRunnable.timeToQuit();
+        s_dsThread.join();
+    }
+    else
+    {
+        // on
+        s_dsThread.start( s_dsRunnable );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void DoGpsStatusCB( Fl_Widget* widget, void* value )
 {
-    static ControllerStatus s_csRunnable;
-    static Poco::Thread     s_csThread;
+    static GpsStatus    s_gsRunnable;
+    static Poco::Thread s_gsThread;
+
+    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
+    if ( button == 0 )
+    {
+        return; // oops, not what we thought?!
+    }
+
+    if ( button->value() == 0 )
+    {
+        // off
+        s_gsRunnable.timeToQuit();
+        s_gsThread.join();
+    }
+    else
+    {
+        // on
+        s_gsThread.start( s_gsRunnable );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void DoMotionStatusCB( Fl_Widget* widget, void* value )
+{
+    static MotionStatus s_msRunnable;
+    static Poco::Thread s_msThread;
 
     Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
     if ( button == 0 )
@@ -66,66 +122,97 @@
     if ( button->value() == 0 )
     {
         // off
-        s_csRunnable.timeToQuit();
-        s_csThread.join();
+        s_msRunnable.timeToQuit();
+        s_msThread.join();
     }
     else
     {
         // on
-        s_csThread.start( s_csRunnable );
+        s_msThread.start( s_msRunnable );
     }
 }
 
 // ----------------------------------------------------------------------------------------
 
-static void PressEstopCB( Fl_Widget* widget, void* value )
+static void ImuKeepAliveCB( Fl_Widget* widget, void* value )
 {
+    static ImuStatus    s_imuStatusRunnable;
+    static Poco::Thread s_imuStatusThread;
+
     Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
     if ( button == 0 )
     {
         return; // opps, not what we thought?!
     }
 
-    ControllerStatus::setEstopActive( button->value() != 0 );
+    if ( button->value() == 0 )
+    {
+        // off
+        s_imuStatusRunnable.timeToQuit();
+        s_imuStatusThread.join();
+    }
+    else
+    {
+        // on
+        s_imuStatusThread.start( s_imuStatusRunnable );
+    }
 }
 
 // ----------------------------------------------------------------------------------------
 
-static void PressMotorCtlCB( Fl_Widget* widget, void* value )
+static void ImuHeadingDialCB( Fl_Widget* widget, void* value )
 {
-    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
-    if ( button == 0 )
+    Fl_Dial* dial = dynamic_cast< Fl_Dial* >( widget );
+    if ( dial == 0 )
     {
         return; // opps, not what we thought?!
     }
 
-    ControllerStatus::setMotorCtlActive( button->value() != 0 );
+    ImuStatus::setImuYaw( dial->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
 }
 
 // ----------------------------------------------------------------------------------------
 
-static void PressButtonOneCB( Fl_Widget* widget, void* value )
+static void ImuPitchSliderCB( Fl_Widget* widget, void* value )
 {
-    Fl_Toggle_Button* button = dynamic_cast< Fl_Toggle_Button* >( widget );
-    if ( button == 0 )
+    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
+    if ( slider == 0 )
     {
         return; // opps, not what we thought?!
     }
 
-    ControllerStatus::setButtonOne( button->value() != 0 );
+    ImuStatus::setImuPitch( slider->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
 }
 
 // ----------------------------------------------------------------------------------------
 
-static void PressButtonTwoCB( Fl_Widget* widget, void* value )
+static void ImuRollSliderCB( Fl_Widget* widget, void* value )
 {
-    Fl_Toggle_Button* button = dynamic_cast< Fl_Toggle_Button* >( widget );
-    if ( button == 0 )
+    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
+    if ( slider == 0 )
     {
         return; // opps, not what we thought?!
     }
 
-    ControllerStatus::setButtonTwo( button->value() != 0 );
+    ImuStatus::setImuRoll( slider->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -234,20 +321,20 @@
 
 SimDisplay::SimDisplay()
     : m_window( 0 ),
-      m_dialMotorSpeedFront( 0 ),
-      m_textMotorSpeedFront( 0 ),
-      m_dialMotorSpeedRear( 0 ),
-      m_textMotorSpeedRear( 0 ),
-      m_sliderServoPosFront( 0 ),
-      m_textServoPosFront( 0 ),
-      m_sliderServoPosRear( 0 ),
-      m_textServoPosRear( 0 ),
+      m_dialWheelSpeedFront( 0 ),
+      m_textWheelSpeedFront( 0 ),
+      m_dialWheelSpeedRear( 0 ),
+      m_textWheelSpeedRear( 0 ),
+      m_sliderWheelPosFront( 0 ),
+      m_textWheelPosFront( 0 ),
+      m_sliderWheelPosRear( 0 ),
+      m_textWheelPosRear( 0 ),
       m_boxMgrHeartbeat( 0 ),
       m_textManagerMsg( 0 ),
       m_boxSonarFrontState( 0 ),
-      m_buttonSendCtlUpdate( 0 ),
-      m_buttonEstop( 0 ),
-      m_buttonMotorCtl( 0 ),
+      m_buttonDisplayAlive( 0 ),
+      m_buttonGpsAlive( 0 ),
+      m_buttonMotionAlive( 0 ),
       m_buttonA( 0 ),
       m_buttonB( 0 ),
       m_buttonSonarFrontKA( 0 ),
@@ -280,60 +367,60 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SimDisplay::createMotorSpeedDials()
+void SimDisplay::createWheelSpeedDials()
 {
     m_window->begin();
 
     // front
     
-    Fl_Box* labelMotorSpeedFront = new Fl_Box( 10, 10, 75, 20 );
-    if ( labelMotorSpeedFront )
+    Fl_Box* labelWheelSpeedFront = new Fl_Box( 10, 10, 75, 20 );
+    if ( labelWheelSpeedFront )
     {
-        labelMotorSpeedFront->box( FL_NO_BOX );
-        labelMotorSpeedFront->align( FL_ALIGN_CENTER );
-        labelMotorSpeedFront->label( "Front Motor" );
+        labelWheelSpeedFront->box( FL_NO_BOX );
+        labelWheelSpeedFront->align( FL_ALIGN_CENTER );
+        labelWheelSpeedFront->label( "Front Speed" );
     }
 
-    m_dialMotorSpeedFront = new Fl_Line_Dial( 10, 35, 75, 75 );
-    if ( m_dialMotorSpeedFront )
+    m_dialWheelSpeedFront = new Fl_Line_Dial( 10, 35, 75, 75 );
+    if ( m_dialWheelSpeedFront )
     {
-        m_dialMotorSpeedFront->box( FL_ROUND_UP_BOX );
-        m_dialMotorSpeedFront->bounds( -128.0, 128.0 );
-        m_dialMotorSpeedFront->set_output();
+        m_dialWheelSpeedFront->box( FL_ROUND_UP_BOX );
+        m_dialWheelSpeedFront->bounds( -128.0, 128.0 );
+        m_dialWheelSpeedFront->set_output();
     }
 
-    m_textMotorSpeedFront = new Fl_Box( 10, 115, 75, 20 );
-    if ( m_textMotorSpeedFront )
+    m_textWheelSpeedFront = new Fl_Box( 10, 115, 75, 20 );
+    if ( m_textWheelSpeedFront )
     {
-        m_textMotorSpeedFront->box( FL_THIN_DOWN_BOX );
-        m_textMotorSpeedFront->align( FL_ALIGN_CENTER );
-        m_textMotorSpeedFront->label( "0" );
+        m_textWheelSpeedFront->box( FL_THIN_DOWN_BOX );
+        m_textWheelSpeedFront->align( FL_ALIGN_CENTER );
+        m_textWheelSpeedFront->label( "0" );
     }
 
     // rear
 
-    Fl_Box* labelMotorSpeedRear = new Fl_Box( 100, 10, 75, 20 );
-    if ( labelMotorSpeedRear )
+    Fl_Box* labelWheelSpeedRear = new Fl_Box( 100, 10, 75, 20 );
+    if ( labelWheelSpeedRear )
     {
-        labelMotorSpeedRear->box( FL_NO_BOX );
-        labelMotorSpeedRear->align( FL_ALIGN_CENTER );
-        labelMotorSpeedRear->label( "Rear Motor" );
+        labelWheelSpeedRear->box( FL_NO_BOX );
+        labelWheelSpeedRear->align( FL_ALIGN_CENTER );
+        labelWheelSpeedRear->label( "Rear Wheels" );
     }
 
-    m_dialMotorSpeedRear = new Fl_Line_Dial( 100, 35, 75, 75 );
-    if ( m_dialMotorSpeedRear )
+    m_dialWheelSpeedRear = new Fl_Line_Dial( 100, 35, 75, 75 );
+    if ( m_dialWheelSpeedRear )
     {
-        m_dialMotorSpeedRear->box( FL_ROUND_UP_BOX );
-        m_dialMotorSpeedRear->bounds( -128.0, 128.0 );
-        m_dialMotorSpeedRear->set_output();
+        m_dialWheelSpeedRear->box( FL_ROUND_UP_BOX );
+        m_dialWheelSpeedRear->bounds( -128.0, 128.0 );
+        m_dialWheelSpeedRear->set_output();
     }
 
-    m_textMotorSpeedRear = new Fl_Box( 100, 115, 75, 20 );
-    if ( m_textMotorSpeedRear )
+    m_textWheelSpeedRear = new Fl_Box( 100, 115, 75, 20 );
+    if ( m_textWheelSpeedRear )
     {
-        m_textMotorSpeedRear->box( FL_THIN_DOWN_BOX );
-        m_textMotorSpeedRear->align( FL_ALIGN_CENTER );
-        m_textMotorSpeedRear->label( "0" );
+        m_textWheelSpeedRear->box( FL_THIN_DOWN_BOX );
+        m_textWheelSpeedRear->align( FL_ALIGN_CENTER );
+        m_textWheelSpeedRear->label( "0" );
     }
 
     m_window->end();
@@ -341,60 +428,60 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SimDisplay::createServoSliders()
+void SimDisplay::createWheelSliders()
 {
     m_window->begin();
 
     // front
     
-    Fl_Box* labelServoPosFront = new Fl_Box( 10, 150, 75, 20 );
-    if ( labelServoPosFront )
+    Fl_Box* labelWheelPosFront = new Fl_Box( 10, 150, 75, 20 );
+    if ( labelWheelPosFront )
     {
-        labelServoPosFront->box( FL_NO_BOX );
-        labelServoPosFront->align( FL_ALIGN_CENTER );
-        labelServoPosFront->label( "Front Servo" );
+        labelWheelPosFront->box( FL_NO_BOX );
+        labelWheelPosFront->align( FL_ALIGN_CENTER );
+        labelWheelPosFront->label( "Front Position" );
     }
 
-    m_sliderServoPosFront = new Fl_Hor_Slider( 10, 175, 75, 20 );
-    if ( m_sliderServoPosFront )
+    m_sliderWheelPosFront = new Fl_Hor_Slider( 10, 175, 75, 20 );
+    if ( m_sliderWheelPosFront )
     {
-        m_sliderServoPosFront->slider( FL_UP_BOX );
-        m_sliderServoPosFront->bounds( -12.0, 12.0 );
-        m_sliderServoPosFront->set_output();
+        m_sliderWheelPosFront->slider( FL_UP_BOX );
+        m_sliderWheelPosFront->bounds( -12.0, 12.0 );
+        m_sliderWheelPosFront->set_output();
     }
 
-    m_textServoPosFront = new Fl_Box( 10, 200, 75, 20 );
-    if ( m_textServoPosFront )
+    m_textWheelPosFront = new Fl_Box( 10, 200, 75, 20 );
+    if ( m_textWheelPosFront )
     {
-        m_textServoPosFront->box( FL_THIN_DOWN_BOX );
-        m_textServoPosFront->align( FL_ALIGN_CENTER );
-        m_textServoPosFront->label( "0" );
+        m_textWheelPosFront->box( FL_THIN_DOWN_BOX );
+        m_textWheelPosFront->align( FL_ALIGN_CENTER );
+        m_textWheelPosFront->label( "0" );
     }
 
     // rear
    
-    Fl_Box* labelServoPosRear = new Fl_Box( 100, 150, 75, 20 );
-    if ( labelServoPosRear )
+    Fl_Box* labelWheelPosRear = new Fl_Box( 100, 150, 75, 20 );
+    if ( labelWheelPosRear )
     {
-        labelServoPosRear->box( FL_NO_BOX );
-        labelServoPosRear->align( FL_ALIGN_CENTER );
-        labelServoPosRear->label( "Rear Servo" );
+        labelWheelPosRear->box( FL_NO_BOX );
+        labelWheelPosRear->align( FL_ALIGN_CENTER );
+        labelWheelPosRear->label( "Rear Wheel" );
     }
 
-    m_sliderServoPosRear = new Fl_Hor_Slider( 100, 175, 75, 20 );
-    if ( m_sliderServoPosRear )
+    m_sliderWheelPosRear = new Fl_Hor_Slider( 100, 175, 75, 20 );
+    if ( m_sliderWheelPosRear )
     {
-        m_sliderServoPosRear->slider( FL_UP_BOX );
-        m_sliderServoPosRear->bounds( -12.0, 12.0 );
-        m_sliderServoPosRear->set_output();
+        m_sliderWheelPosRear->slider( FL_UP_BOX );
+        m_sliderWheelPosRear->bounds( -12.0, 12.0 );
+        m_sliderWheelPosRear->set_output();
     }
 
-    m_textServoPosRear = new Fl_Box( 100, 200, 75, 20 );
-    if ( m_textServoPosRear )
+    m_textWheelPosRear = new Fl_Box( 100, 200, 75, 20 );
+    if ( m_textWheelPosRear )
     {
-        m_textServoPosRear->box( FL_THIN_DOWN_BOX );
-        m_textServoPosRear->align( FL_ALIGN_CENTER );
-        m_textServoPosRear->label( "0" );
+        m_textWheelPosRear->box( FL_THIN_DOWN_BOX );
+        m_textWheelPosRear->align( FL_ALIGN_CENTER );
+        m_textWheelPosRear->label( "0" );
     }
 
     m_window->end();
@@ -443,34 +530,31 @@
 {
     m_window->begin();
 
-    m_buttonSendCtlUpdate = new Fl_Light_Button( 250, 20, 165, 20 );
-    if ( m_buttonSendCtlUpdate )
+    m_buttonDisplayAlive = new Fl_Light_Button( 250, 20, 165, 20 );
+    if ( m_buttonDisplayAlive )
     {
-        m_buttonSendCtlUpdate->label( "Do Controller Updates" );
-        m_buttonSendCtlUpdate->selection_color( FL_GREEN );
-        m_buttonSendCtlUpdate->callback( &SendControllerUpdateCB );
-        m_buttonSendCtlUpdate->set();
-        m_buttonSendCtlUpdate->do_callback();
+        m_buttonDisplayAlive->label( "Display Alive" );
+        m_buttonDisplayAlive->selection_color( FL_GREEN );
+        m_buttonDisplayAlive->callback( &DoDisplayStatusCB );
     }
 
-    m_buttonEstop = new Fl_Light_Button( 250, 50, 165, 20 );
-    if ( m_buttonEstop )
+    m_buttonGpsAlive = new Fl_Light_Button( 250, 50, 165, 20 );
+    if ( m_buttonGpsAlive )
     {
-        m_buttonEstop->label( "Remote ESTOP" );
-        m_buttonEstop->selection_color( FL_RED );
-        m_buttonEstop->callback( &PressEstopCB );
-        m_buttonEstop->set();
-        m_buttonEstop->do_callback();
+        m_buttonGpsAlive->label( "GPS Alive" );
+        m_buttonGpsAlive->selection_color( FL_GREEN );
+        m_buttonGpsAlive->callback( &DoGpsStatusCB );
     }
 
-    m_buttonMotorCtl = new Fl_Light_Button( 250, 80, 165, 20 );
-    if ( m_buttonMotorCtl )
+    m_buttonMotionAlive = new Fl_Light_Button( 250, 80, 165, 20 );
+    if ( m_buttonMotionAlive )
     {
-        m_buttonMotorCtl->label( "Motor Controller" );
-        m_buttonMotorCtl->selection_color( FL_GREEN );
-        m_buttonMotorCtl->callback( &PressMotorCtlCB );
+        m_buttonMotionAlive->label( "Motion Control Alive" );
+        m_buttonMotionAlive->selection_color( FL_GREEN );
+        m_buttonMotionAlive->callback( &DoMotionStatusCB );
     }
 
+#if 0
     m_buttonA = new Fl_Toggle_Button( 250, 110, 80, 20 );
     if ( m_buttonA )
     {
@@ -484,6 +568,61 @@
         m_buttonB->label( "B" );
         m_buttonB->callback( &PressButtonTwoCB );
     }
+#endif
+
+    m_window->end();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SimDisplay::createImuControls()
+{
+    m_window->begin();
+
+    m_buttonImuKA = new Fl_Light_Button( 250, 150, 165, 20 );
+    if ( m_buttonImuKA )
+    {
+        m_buttonImuKA->label( "IMU Alive" );
+        m_buttonImuKA->selection_color( FL_GREEN );
+        m_buttonImuKA->callback( &ImuKeepAliveCB );
+    }
+
+    m_textImuValues = new Fl_Box( 250, 180, 165, 20 );
+    if ( m_textImuValues )
+    {
+        m_textImuValues->box( FL_THIN_DOWN_BOX );
+        m_textImuValues->align( FL_ALIGN_CENTER );
+        m_textImuValues->label( "0.0  0.0  0.0" );
+    }
+
+    m_dialImuHeading = new Fl_Dial( 250, 210, 75, 75 );
+    if ( m_dialImuHeading )
+    {
+        m_dialImuHeading->box( FL_ROUND_UP_BOX );
+        m_dialImuHeading->bounds( 0.0, 360.0 );
+        m_dialImuHeading->value( 180.0 );
+        m_dialImuHeading->callback( ImuHeadingDialCB, this );
+    }
+
+    m_sliderImuPitch = new Fl_Slider( 335, 210, 35, 60 );
+    if ( m_sliderImuPitch )
+    {
+        m_sliderImuPitch->label( "Pitch" );
+        m_sliderImuPitch->slider( FL_UP_BOX );
+        m_sliderImuPitch->bounds( 20.0, -20.0 );
+        m_sliderImuPitch->value( 0.0 );
+        m_sliderImuPitch->callback( ImuPitchSliderCB, this );
+    }
+
+    m_sliderImuRoll = new Fl_Slider( 380, 210, 35, 60 );
+    if ( m_sliderImuRoll )
+    {
+        m_sliderImuRoll->label( "Roll" );
+        m_sliderImuRoll->slider( FL_UP_BOX );
+        m_sliderImuRoll->bounds( 20.0, -20.0 );
+        m_sliderImuRoll->value( 0.0 );
+        m_sliderImuRoll->callback( ImuRollSliderCB, this );
+    }
 
     m_window->end();
 }
@@ -494,7 +633,7 @@
 {
     m_window->begin();
 
-    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 150, 165, 20 );
+    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 310, 165, 20 );
     if ( m_buttonSonarFrontKA )
     {
         m_buttonSonarFrontKA->label( "Sonar Front Alive" );
@@ -502,7 +641,7 @@
         m_buttonSonarFrontKA->callback( &SonarFrontKeepAliveCB );
     }
 
-    m_textSonarFront = new Fl_Box( 250, 180, 165, 20 );
+    m_textSonarFront = new Fl_Box( 250, 340, 165, 20 );
     if ( m_textSonarFront )
     {
         m_textSonarFront->box( FL_THIN_DOWN_BOX );
@@ -510,42 +649,42 @@
         m_textSonarFront->label( "0F00 0F00 0F00 0F00" );
     }
 
-    m_sliderSonarFrontL = new Fl_Slider( 250, 210, 35, 100 );
+    m_sliderSonarFrontL = new Fl_Slider( 250, 370, 35, 100 );
     if ( m_sliderSonarFrontL )
     {
         m_sliderSonarFrontL->label( "Left" );
         m_sliderSonarFrontL->slider( FL_UP_BOX );
-        m_sliderSonarFrontL->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontL->bounds( 100, 0 );
         m_sliderSonarFrontL->value( SonarFrontStatus::sonarFrontLeft() );
         m_sliderSonarFrontL->callback( SonarFrontSliderLeftCB, this );
     }
 
-    m_sliderSonarFrontCL = new Fl_Slider( 293, 210, 35, 100 );
+    m_sliderSonarFrontCL = new Fl_Slider( 293, 370, 35, 100 );
     if ( m_sliderSonarFrontCL )
     {
         m_sliderSonarFrontCL->label( "C Left" );
         m_sliderSonarFrontCL->slider( FL_UP_BOX );
-        m_sliderSonarFrontCL->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontCL->bounds( 100, 0 );
         m_sliderSonarFrontCL->value( SonarFrontStatus::sonarFrontCenterLeft() );
         m_sliderSonarFrontCL->callback( SonarFrontSliderCenterLeftCB, this );
     }
 
-    m_sliderSonarFrontCR = new Fl_Slider( 336, 210, 35, 100 );
+    m_sliderSonarFrontCR = new Fl_Slider( 336, 370, 35, 100 );
     if ( m_sliderSonarFrontCR )
     {
         m_sliderSonarFrontCR->label( "C Right" );
         m_sliderSonarFrontCR->slider( FL_UP_BOX );
-        m_sliderSonarFrontCR->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontCR->bounds( 100, 0 );
         m_sliderSonarFrontCR->value( SonarFrontStatus::sonarFrontCenterRight() );
         m_sliderSonarFrontCR->callback( SonarFrontSliderCenterRightCB, this );
     }
 
-    m_sliderSonarFrontR = new Fl_Slider( 380, 210, 35, 100 );
+    m_sliderSonarFrontR = new Fl_Slider( 380, 370, 35, 100 );
     if ( m_sliderSonarFrontR )
     {
         m_sliderSonarFrontR->label( "Right" );
         m_sliderSonarFrontR->slider( FL_UP_BOX );
-        m_sliderSonarFrontR->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontR->bounds( 100, 0 );
         m_sliderSonarFrontR->value( SonarFrontStatus::sonarFrontRight() );
         m_sliderSonarFrontR->callback( SonarFrontSliderRightCB, this );
     }
@@ -561,10 +700,11 @@
 
     if ( m_window )
     {
-        createMotorSpeedDials();
-        createServoSliders();
+        createWheelSpeedDials();
+        createWheelSliders();
         createDisplays();
         createActionButtons();
+        createImuControls();
         createSonarFrontControls();
 
         m_window->show();
@@ -573,41 +713,41 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SimDisplay::updateMotorSpeeds( int front, int rear )
+void SimDisplay::updateWheelSpeeds( int front, int rear )
 {
     Fl::lock();
 
     // front
 
-    if ( m_dialMotorSpeedFront )
+    if ( m_dialWheelSpeedFront )
     {
-        m_dialMotorSpeedFront->value( 
-                m_dialMotorSpeedFront->clamp( static_cast< double >( front ) ) );
+        m_dialWheelSpeedFront->value( 
+                m_dialWheelSpeedFront->clamp( static_cast< double >( front ) ) );
     }
 
-    if ( m_textMotorSpeedFront )
+    if ( m_textWheelSpeedFront )
     {
         char buf[ 20 ];
         snprintf( buf, sizeof( buf ), "%d", front );
-        m_textMotorSpeedFront->label( buf );
+        m_textWheelSpeedFront->label( buf );
     }
 
     Fl::flush();
 
     // rear
 
-    if ( m_dialMotorSpeedRear )
+    if ( m_dialWheelSpeedRear )
     {
-        m_dialMotorSpeedRear->value( 
-                m_dialMotorSpeedRear->clamp( static_cast< double >( rear ) ) );
-        m_dialMotorSpeedRear->redraw();
+        m_dialWheelSpeedRear->value( 
+                m_dialWheelSpeedRear->clamp( static_cast< double >( rear ) ) );
+        m_dialWheelSpeedRear->redraw();
     }
 
-    if ( m_textMotorSpeedRear )
+    if ( m_textWheelSpeedRear )
     {
         char buf[ 20 ];
         snprintf( buf, sizeof( buf ), "%d", rear );
-        m_textMotorSpeedRear->label( buf );
+        m_textWheelSpeedRear->label( buf );
     }
 
     Fl::flush();
@@ -617,40 +757,40 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SimDisplay::updateServoPositions( int front, int rear )
+void SimDisplay::updateWheelPositions( int front, int rear )
 {
     Fl::lock();
    
    // front
 
-    if ( m_sliderServoPosFront )
+    if ( m_sliderWheelPosFront )
     {
-        m_sliderServoPosFront->value( 
-                m_sliderServoPosFront->clamp( static_cast< double >( front ) ) );
+        m_sliderWheelPosFront->value( 
+                m_sliderWheelPosFront->clamp( static_cast< double >( front ) ) );
     }
 
-    if ( m_textServoPosFront )
+    if ( m_textWheelPosFront )
     {
         char buf[ 20 ];
         snprintf( buf, sizeof( buf ), "%d", front );
-        m_textServoPosFront->label( buf );
+        m_textWheelPosFront->label( buf );
     }
 
     Fl::flush();
 
     // rear
     
-    if ( m_sliderServoPosRear )
+    if ( m_sliderWheelPosRear )
     {
-        m_sliderServoPosRear->value( 
-                m_sliderServoPosRear->clamp( static_cast< double >( rear ) ) );
+        m_sliderWheelPosRear->value( 
+                m_sliderWheelPosRear->clamp( static_cast< double >( rear ) ) );
     }
 
-    if ( m_textServoPosRear )
+    if ( m_textWheelPosRear )
     {
         char buf[ 20 ];
         snprintf( buf, sizeof( buf ), "%d", rear );
-        m_textServoPosRear->label( buf );
+        m_textWheelPosRear->label( buf );
     }
 
     Fl::flush();
@@ -757,19 +897,40 @@
 
 // ----------------------------------------------------------------------------------------
 
+void SimDisplay::updateImuValues()
+{
+    Fl::lock();
+
+    if ( m_textImuValues )
+    {
+        char buf[ 80 ];
+        snprintf( buf, sizeof( buf ), "%3.1lf  %3.1lf  %3.1lf",
+                  ImuStatus::imuYaw(),
+                  ImuStatus::imuPitch(),
+                  ImuStatus::imuRoll() );
+        m_textImuValues->label( buf );
+    }
+
+    Fl::flush();
+
+    Fl::unlock();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void SimDisplay::updateSonarFrontValues()
 {
     Fl::lock();
 
-    unsigned left        = SonarFrontStatus::sonarFrontLeft();
-    unsigned centerLeft  = SonarFrontStatus::sonarFrontCenterLeft();
-    unsigned centerRight = SonarFrontStatus::sonarFrontCenterRight();
-    unsigned right       = SonarFrontStatus::sonarFrontRight();
+    uint8_t left        = SonarFrontStatus::sonarFrontLeft();
+    uint8_t centerLeft  = SonarFrontStatus::sonarFrontCenterLeft();
+    uint8_t centerRight = SonarFrontStatus::sonarFrontCenterRight();
+    uint8_t right       = SonarFrontStatus::sonarFrontRight();
 
     if ( m_textSonarFront )
     {
         char buf[ 80 ];
-        snprintf( buf, sizeof( buf ), "%04X %04X %04X %04X",
+        snprintf( buf, sizeof( buf ), "%02X  %02X  %02X  %02X",
                   left, centerLeft, centerRight, right );
         m_textSonarFront->label( buf );
     }
--- a/main/robots/odr-sim/SimDisplay.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/SimDisplay.h	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //    
 //  Window display for the ODR platform simulator.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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,6 +37,7 @@
 #include <Poco/Timer.h>
 
 class Fl_Box;
+class Fl_Dial;
 class Fl_Double_Window;
 class Fl_Hor_Slider;
 class Fl_Light_Button;
@@ -52,32 +53,38 @@
         SimDisplay();
         virtual ~SimDisplay();
         void showDisplay();
-        void updateMotorSpeeds( int front, int rear );
-        void updateServoPositions( int front, int rear );
+        void updateWheelSpeeds( int front, int rear );
+        void updateWheelPositions( int front, int rear );
         void updateManagerMsg( const char* msg, int length );
         void doMgrHeartbeat();
         void resetMgrHeartbeat();
+        void updateImuValues();
         void updateSonarFrontState( bool enabled );
         void updateSonarFrontValues();
 
     private:
         Fl_Double_Window* m_window;
-        Fl_Line_Dial*     m_dialMotorSpeedFront;
-        Fl_Box*           m_textMotorSpeedFront;
-        Fl_Line_Dial*     m_dialMotorSpeedRear;
-        Fl_Box*           m_textMotorSpeedRear;
-        Fl_Hor_Slider*    m_sliderServoPosFront;
-        Fl_Box*           m_textServoPosFront;
-        Fl_Hor_Slider*    m_sliderServoPosRear;
-        Fl_Box*           m_textServoPosRear;
+        Fl_Line_Dial*     m_dialWheelSpeedFront;
+        Fl_Box*           m_textWheelSpeedFront;
+        Fl_Line_Dial*     m_dialWheelSpeedRear;
+        Fl_Box*           m_textWheelSpeedRear;
+        Fl_Hor_Slider*    m_sliderWheelPosFront;
+        Fl_Box*           m_textWheelPosFront;
+        Fl_Hor_Slider*    m_sliderWheelPosRear;
+        Fl_Box*           m_textWheelPosRear;
         Fl_Box*           m_boxMgrHeartbeat;
         Fl_Box*           m_textManagerMsg;
         Fl_Box*           m_boxSonarFrontState;
-        Fl_Light_Button*  m_buttonSendCtlUpdate;
-        Fl_Light_Button*  m_buttonEstop;
-        Fl_Light_Button*  m_buttonMotorCtl;
+        Fl_Light_Button*  m_buttonDisplayAlive;
+        Fl_Light_Button*  m_buttonGpsAlive;
+        Fl_Light_Button*  m_buttonMotionAlive;
         Fl_Toggle_Button* m_buttonA;
         Fl_Toggle_Button* m_buttonB;
+        Fl_Light_Button*  m_buttonImuKA;
+        Fl_Box*           m_textImuValues;
+        Fl_Dial*          m_dialImuHeading;
+        Fl_Slider*        m_sliderImuPitch;
+        Fl_Slider*        m_sliderImuRoll;
         Fl_Light_Button*  m_buttonSonarFrontKA;
         Fl_Slider*        m_sliderSonarFrontL;
         Fl_Slider*        m_sliderSonarFrontCL;
@@ -88,10 +95,11 @@
 
     private:
         void createWindow();
-        void createMotorSpeedDials();
-        void createServoSliders();
+        void createWheelSpeedDials();
+        void createWheelSliders();
         void createDisplays();
         void createActionButtons();
+        void createImuControls();
         void createSonarFrontControls();
         void timeoutMgrHeartbeat( Poco::Timer& timer );
 };
--- a/main/robots/odr-sim/SonarFrontStatus.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/SonarFrontStatus.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -84,10 +84,10 @@
 
 Poco::RWLock SonarFrontStatus::sm_rwLock;
 bool         SonarFrontStatus::sm_isEnabled   = false;
-unsigned     SonarFrontStatus::sm_left        = 0x0f00;
-unsigned     SonarFrontStatus::sm_centerLeft  = 0x0f00;
-unsigned     SonarFrontStatus::sm_centerRight = 0x0f00;
-unsigned     SonarFrontStatus::sm_right       = 0x0f00;
+uint8_t      SonarFrontStatus::sm_left        = 60;
+uint8_t      SonarFrontStatus::sm_centerLeft  = 60;
+uint8_t      SonarFrontStatus::sm_centerRight = 60;
+uint8_t      SonarFrontStatus::sm_right       = 60;
 
 // ----------------------------------------------------------------------------------------
 
@@ -115,7 +115,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontLeft()
+uint8_t SonarFrontStatus::sonarFrontLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_left;
@@ -123,7 +123,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontCenterLeft()
+uint8_t SonarFrontStatus::sonarFrontCenterLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_centerLeft;
@@ -131,7 +131,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontCenterRight()
+uint8_t SonarFrontStatus::sonarFrontCenterRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_centerRight;
@@ -139,7 +139,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontRight()
+uint8_t SonarFrontStatus::sonarFrontRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_right;
@@ -147,7 +147,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontLeft( unsigned value )
+void SonarFrontStatus::setSonarFrontLeft( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_left = value;
@@ -155,7 +155,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontCenterLeft( unsigned value )
+void SonarFrontStatus::setSonarFrontCenterLeft( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_centerLeft = value;
@@ -163,7 +163,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontCenterRight( unsigned value )
+void SonarFrontStatus::setSonarFrontCenterRight( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_centerRight = value;
@@ -171,7 +171,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontRight( unsigned value )
+void SonarFrontStatus::setSonarFrontRight( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_right = value;
--- a/main/robots/odr-sim/SonarFrontStatus.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/SonarFrontStatus.h	Sun Jul 13 09:15:53 2014 -0700
@@ -32,6 +32,8 @@
 #ifndef BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
 #define BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
 
+#include <stdint.h>
+
 #include <Poco/Event.h>
 #include <Poco/Runnable.h>
 #include <Poco/RWLock.h>
@@ -44,14 +46,14 @@
         static bool     isSonarFrontEnabled();
         static void     setSonarFrontEnabled();
         static void     setSonarFrontDisabled();
-        static unsigned sonarFrontLeft();
-        static unsigned sonarFrontCenterLeft();
-        static unsigned sonarFrontCenterRight();
-        static unsigned sonarFrontRight();
-        static void     setSonarFrontLeft( unsigned value );
-        static void     setSonarFrontCenterLeft( unsigned value );
-        static void     setSonarFrontCenterRight( unsigned value );
-        static void     setSonarFrontRight( unsigned value );
+        static uint8_t  sonarFrontLeft();
+        static uint8_t  sonarFrontCenterLeft();
+        static uint8_t  sonarFrontCenterRight();
+        static uint8_t  sonarFrontRight();
+        static void     setSonarFrontLeft( uint8_t value );
+        static void     setSonarFrontCenterLeft( uint8_t value );
+        static void     setSonarFrontCenterRight( uint8_t value );
+        static void     setSonarFrontRight( uint8_t value );
 
     public:
         SonarFrontStatus();
@@ -65,10 +67,10 @@
     private:
         static Poco::RWLock sm_rwLock;
         static bool         sm_isEnabled;
-        static unsigned     sm_left;
-        static unsigned     sm_centerLeft;
-        static unsigned     sm_centerRight;
-        static unsigned     sm_right;
+        static uint8_t      sm_left;
+        static uint8_t      sm_centerLeft;
+        static uint8_t      sm_centerRight;
+        static uint8_t      sm_right;
 
     private:
         Poco::Event m_quitEvent;
--- a/main/robots/odr-sim/jamfile	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr-sim/jamfile	Sun Jul 13 09:15:53 2014 -0700
@@ -36,7 +36,10 @@
 ubuntu_executable odr-sim
     : main.cpp
       ODRSimApp.cpp
-      ControllerStatus.cpp 
+      DisplayStatus.cpp
+      GpsStatus.cpp
+      ImuStatus.cpp
+      MotionStatus.cpp
       Receiver.cpp 
       SimDisplay.cpp 
       SonarFrontStatus.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/odrsim.log	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,341 @@
+I 12/30/11-15:41:54.638594 ------------------------------------------------------
+I 12/30/11-15:41:54.638893 ODR H/W Simulator startup
+I 12/30/11-15:41:54.638909 ODRSimApp::main() started
+I 12/30/11-15:41:54.639032 using CANbus interface name "can0"
+I 12/30/11-15:41:54.740234 CAN frame send: [02:00:0012] 09                   .
+I 12/30/11-15:42:11.398476 CAN frame send: [02:00:0012] 09                   .
+I 12/30/11-15:42:13.911409 CAN frame send: [02:00:0012] 09                   .
+I 12/30/11-15:42:16.431669 CAN frame send: [02:00:0012] 09                   .
+I 12/30/11-15:42:18.947417 CAN frame send: [02:00:0012] 09                   .
+I 12/30/11-15:42:21.466028 CAN frame send: [02:00:0012] 08                   .
+I 12/30/11-15:42:23.983330 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:25.720028 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:26.426648 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:28.223911 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:28.930324 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:30.727640 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:31.432743 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:33.233196 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:33.938857 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:35.737285 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:36.443286 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:38.240750 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:38.944917 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:40.743815 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:41.448870 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:43.248206 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:43.954355 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:45.752622 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:46.458559 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:48.258283 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:48.965882 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:50.762645 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:51.468553 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:53.266126 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:53.973406 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:55.770770 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:56.476359 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:42:58.274529 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:42:58.978542 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:43:00.776061 CAN frame send: [03:00:0131]                      
+I 12/30/11-15:43:01.479613 CAN frame send: [02:00:0012] 0A                   .
+I 12/30/11-15:43:02.485439 ODR H/W Simulator shutdown
+I 12/30/11-15:44:09.586276 ------------------------------------------------------
+I 12/30/11-15:44:09.586532 ODR H/W Simulator startup
+I 12/30/11-15:44:09.586548 ODRSimApp::main() started
+I 12/30/11-15:44:09.586655 using CANbus interface name "can0"
+I 12/30/11-15:46:06.045971 ODR H/W Simulator shutdown
+I 12/30/11-15:50:52.906067 ------------------------------------------------------
+I 12/30/11-15:50:52.906316 ODR H/W Simulator startup
+I 12/30/11-15:50:52.906331 ODRSimApp::main() started
+I 12/30/11-15:50:52.906434 using CANbus interface name "can0"
+I 12/30/11-15:53:14.681129 ODR H/W Simulator shutdown
+I 01/13/12-05:29:48.828683 ------------------------------------------------------
+I 01/13/12-05:29:48.838202 ODR H/W Simulator startup
+I 01/13/12-05:29:48.838220 ODRSimApp::main() started
+I 01/13/12-05:29:48.838326 using CANbus interface name "can0"
+I 01/13/12-05:30:27.861651 ODR H/W Simulator shutdown
+I 02/11/12-20:00:09.841334 ------------------------------------------------------
+I 02/11/12-20:00:10.044188 ODR H/W Simulator startup
+I 02/11/12-20:00:10.044232 ODRSimApp::main() started
+I 02/11/12-20:00:10.044382 using CANbus interface name "can0"
+I 02/11/12-20:01:44.293176 ODR H/W Simulator shutdown
+I 02/11/12-22:09:02.353065 ------------------------------------------------------
+I 02/11/12-22:09:02.353311 ODR H/W Simulator startup
+I 02/11/12-22:09:02.353327 ODRSimApp::main() started
+I 02/11/12-22:09:02.353429 using CANbus interface name "can0"
+I 02/11/12-22:11:38.319707 ODR H/W Simulator shutdown
+I 02/12/12-18:43:38.707217 ------------------------------------------------------
+I 02/12/12-18:43:38.707452 ODR H/W Simulator startup
+I 02/12/12-18:43:38.707467 ODRSimApp::main() started
+I 02/12/12-18:43:38.707568 using CANbus interface name "can0"
+I 02/12/12-18:46:10.109877 ODR H/W Simulator shutdown
+I 06/09/12-19:53:40.084032 ------------------------------------------------------
+I 06/09/12-19:53:40.118266 ODR H/W Simulator startup
+I 06/09/12-19:53:40.118307 ODRSimApp::main() started
+I 06/09/12-19:53:40.118446 using CANbus interface name "can0"
+I 06/09/12-20:01:09.281913 ODR H/W Simulator shutdown
+I 06/09/12-20:16:00.804035 ------------------------------------------------------
+I 06/09/12-20:16:00.804261 ODR H/W Simulator startup
+I 06/09/12-20:16:00.804277 ODRSimApp::main() started
+I 06/09/12-20:16:00.804378 using CANbus interface name "can0"
+I 07/08/12-00:04:43.311729 ------------------------------------------------------
+I 07/08/12-00:04:43.333980 ODR H/W Simulator startup
+I 07/08/12-00:04:43.334026 ODRSimApp::main() started
+I 07/08/12-00:04:43.334173 using CANbus interface name "can0"
+I 07/08/12-00:05:00.460464 ODR H/W Simulator shutdown
+I 07/08/12-00:09:37.061681 ------------------------------------------------------
+I 07/08/12-00:09:37.061906 ODR H/W Simulator startup
+I 07/08/12-00:09:37.061922 ODRSimApp::main() started
+I 07/08/12-00:09:37.062025 using CANbus interface name "can0"
+I 07/08/12-00:09:50.248281 ODR H/W Simulator shutdown
+I 07/08/12-00:10:51.106037 ------------------------------------------------------
+I 07/08/12-00:10:51.106729 ODR H/W Simulator startup
+I 07/08/12-00:10:51.106745 ODRSimApp::main() started
+I 07/08/12-00:10:51.106849 using CANbus interface name "can0"
+I 07/08/12-00:11:03.599376 ODR H/W Simulator shutdown
+I 02/09/13-20:54:32.547108 ------------------------------------------------------
+I 02/09/13-20:54:32.552550 ODR H/W Simulator startup
+I 02/09/13-20:54:32.552582 ODRSimApp::main() started
+I 02/09/13-20:54:32.552698 using CANbus interface name "can0"
+I 02/09/13-20:54:56.482210 ODR H/W Simulator shutdown
+I 08/10/13-21:23:54.877694 ------------------------------------------------------
+I 08/10/13-21:23:54.877985 ODR H/W Simulator startup
+I 08/10/13-21:23:54.877999 ODRSimApp::main() started
+I 08/10/13-21:23:54.878076 using CANbus interface name "can0"
+I 08/10/13-21:25:15.574660 ODR H/W Simulator shutdown
+I 08/10/13-21:32:15.271464 ------------------------------------------------------
+I 08/10/13-21:32:15.271537 ODR H/W Simulator startup
+I 08/10/13-21:32:15.271546 ODRSimApp::main() started
+I 08/10/13-21:32:15.271615 using CANbus interface name "can0"
+I 08/10/13-21:32:22.317674 ODR H/W Simulator shutdown
+I 08/10/13-21:33:55.119721 ------------------------------------------------------
+I 08/10/13-21:33:55.119801 ODR H/W Simulator startup
+I 08/10/13-21:33:55.119810 ODRSimApp::main() started
+I 08/10/13-21:33:55.119876 using CANbus interface name "can0"
+I 08/10/13-21:34:07.921040 ODR H/W Simulator shutdown
+I 08/10/13-21:35:12.551851 ------------------------------------------------------
+I 08/10/13-21:35:12.551922 ODR H/W Simulator startup
+I 08/10/13-21:35:12.551931 ODRSimApp::main() started
+I 08/10/13-21:35:12.551994 using CANbus interface name "can0"
+I 08/10/13-21:35:29.196782 ODR H/W Simulator shutdown
+I 08/10/13-21:36:14.001590 ------------------------------------------------------
+I 08/10/13-21:36:14.001661 ODR H/W Simulator startup
+I 08/10/13-21:36:14.001671 ODRSimApp::main() started
+I 08/10/13-21:36:14.001747 using CANbus interface name "can0"
+I 08/10/13-21:36:38.997458 ODR H/W Simulator shutdown
+I 08/10/13-21:38:56.914045 ------------------------------------------------------
+I 08/10/13-21:38:56.914115 ODR H/W Simulator startup
+I 08/10/13-21:38:56.914124 ODRSimApp::main() started
+I 08/10/13-21:38:56.914190 using CANbus interface name "can0"
+I 08/10/13-21:39:00.031050 ODR H/W Simulator shutdown
+I 08/10/13-21:39:32.102198 ------------------------------------------------------
+I 08/10/13-21:39:32.102277 ODR H/W Simulator startup
+I 08/10/13-21:39:32.102285 ODRSimApp::main() started
+I 08/10/13-21:39:32.102352 using CANbus interface name "can0"
+I 08/10/13-21:39:36.025963 ODR H/W Simulator shutdown
+I 08/10/13-21:40:52.166575 ------------------------------------------------------
+I 08/10/13-21:40:52.166650 ODR H/W Simulator startup
+I 08/10/13-21:40:52.166659 ODRSimApp::main() started
+I 08/10/13-21:40:52.166727 using CANbus interface name "can0"
+I 08/10/13-21:41:06.948733 ODR H/W Simulator shutdown
+I 08/10/13-21:43:05.306256 ------------------------------------------------------
+I 08/10/13-21:43:05.306336 ODR H/W Simulator startup
+I 08/10/13-21:43:05.306346 ODRSimApp::main() started
+I 08/10/13-21:43:05.306446 using CANbus interface name "can0"
+I 08/10/13-21:43:49.243835 ODR H/W Simulator shutdown
+I 08/10/13-21:44:03.223969 ------------------------------------------------------
+I 08/10/13-21:44:03.224050 ODR H/W Simulator startup
+I 08/10/13-21:44:03.224060 ODRSimApp::main() started
+I 08/10/13-21:44:03.224131 using CANbus interface name "can0"
+I 08/10/13-21:44:17.020165 ODR H/W Simulator shutdown
+I 08/10/13-22:05:58.303742 ------------------------------------------------------
+I 08/10/13-22:05:58.303834 ODR H/W Simulator startup
+I 08/10/13-22:05:58.303876 ODRSimApp::main() started
+I 08/10/13-22:05:58.303973 using CANbus interface name "can0"
+I 08/10/13-22:06:29.378061 ODR H/W Simulator shutdown
+I 08/10/13-22:08:04.788293 ------------------------------------------------------
+I 08/10/13-22:08:04.788383 ODR H/W Simulator startup
+I 08/10/13-22:08:04.788395 ODRSimApp::main() started
+I 08/10/13-22:08:04.788483 using CANbus interface name "can0"
+I 08/10/13-22:08:17.998494 ODR H/W Simulator shutdown
+I 08/10/13-22:09:40.075972 ------------------------------------------------------
+I 08/10/13-22:09:40.076065 ODR H/W Simulator startup
+I 08/10/13-22:09:40.076078 ODRSimApp::main() started
+I 08/10/13-22:09:40.076165 using CANbus interface name "can0"
+I 08/10/13-22:09:44.196287 ODR H/W Simulator shutdown
+I 08/10/13-22:11:30.568274 ------------------------------------------------------
+I 08/10/13-22:11:30.568382 ODR H/W Simulator startup
+I 08/10/13-22:11:30.568392 ODRSimApp::main() started
+I 08/10/13-22:11:30.568462 using CANbus interface name "can0"
+I 08/10/13-22:11:36.005252 ODR H/W Simulator shutdown
+I 08/10/13-22:12:49.786019 ------------------------------------------------------
+I 08/10/13-22:12:49.786092 ODR H/W Simulator startup
+I 08/10/13-22:12:49.786102 ODRSimApp::main() started
+I 08/10/13-22:12:49.786169 using CANbus interface name "can0"
+I 08/10/13-22:13:15.207680 ODR H/W Simulator shutdown
+I 08/10/13-22:17:31.113035 ------------------------------------------------------
+I 08/10/13-22:17:31.113115 ODR H/W Simulator startup
+I 08/10/13-22:17:31.113126 ODRSimApp::main() started
+I 08/10/13-22:17:31.113200 using CANbus interface name "can0"
+I 08/10/13-22:17:44.318427 ODR H/W Simulator shutdown
+I 08/10/13-22:21:52.798099 ------------------------------------------------------
+I 08/10/13-22:21:52.798198 ODR H/W Simulator startup
+I 08/10/13-22:21:52.798211 ODRSimApp::main() started
+I 08/10/13-22:21:52.798303 using CANbus interface name "can0"
+I 08/10/13-22:22:03.473455 ODR H/W Simulator shutdown
+I 08/10/13-22:22:48.510786 ------------------------------------------------------
+I 08/10/13-22:22:48.510859 ODR H/W Simulator startup
+I 08/10/13-22:22:48.510868 ODRSimApp::main() started
+I 08/10/13-22:22:48.510935 using CANbus interface name "can0"
+I 08/10/13-22:22:59.191150 ODR H/W Simulator shutdown
+I 08/10/13-22:27:23.774583 ------------------------------------------------------
+I 08/10/13-22:27:23.774666 ODR H/W Simulator startup
+I 08/10/13-22:27:23.774675 ODRSimApp::main() started
+I 08/10/13-22:27:23.774743 using CANbus interface name "can0"
+I 08/10/13-22:27:34.057964 ODR H/W Simulator shutdown
+I 08/10/13-22:30:07.736631 ------------------------------------------------------
+I 08/10/13-22:30:07.736721 ODR H/W Simulator startup
+I 08/10/13-22:30:07.736733 ODRSimApp::main() started
+I 08/10/13-22:30:07.736821 using CANbus interface name "can0"
+I 08/10/13-22:30:19.627229 ODR H/W Simulator shutdown
+I 08/10/13-22:32:00.890957 ------------------------------------------------------
+I 08/10/13-22:32:00.891272 ODR H/W Simulator startup
+I 08/10/13-22:32:00.891283 ODRSimApp::main() started
+I 08/10/13-22:32:00.891351 using CANbus interface name "can0"
+I 08/10/13-22:32:21.353636 ODR H/W Simulator shutdown
+I 08/10/13-22:35:23.221157 ------------------------------------------------------
+I 08/10/13-22:35:23.221248 ODR H/W Simulator startup
+I 08/10/13-22:35:23.221260 ODRSimApp::main() started
+I 08/10/13-22:35:23.221348 using CANbus interface name "can0"
+I 08/10/13-22:35:42.975009 ODR H/W Simulator shutdown
+I 08/10/13-22:37:16.698070 ------------------------------------------------------
+I 08/10/13-22:37:16.698161 ODR H/W Simulator startup
+I 08/10/13-22:37:16.698173 ODRSimApp::main() started
+I 08/10/13-22:37:16.698260 using CANbus interface name "can0"
+I 08/10/13-22:37:31.712473 ODR H/W Simulator shutdown
+I 08/10/13-22:38:20.493683 ------------------------------------------------------
+I 08/10/13-22:38:20.493757 ODR H/W Simulator startup
+I 08/10/13-22:38:20.493767 ODRSimApp::main() started
+I 08/10/13-22:38:20.493834 using CANbus interface name "can0"
+I 08/10/13-22:38:26.931993 ODR H/W Simulator shutdown
+I 08/10/13-22:38:49.599385 ------------------------------------------------------
+I 08/10/13-22:38:49.599457 ODR H/W Simulator startup
+I 08/10/13-22:38:49.599466 ODRSimApp::main() started
+I 08/10/13-22:38:49.599532 using CANbus interface name "can0"
+I 08/10/13-22:39:14.905809 ODR H/W Simulator shutdown
+I 08/10/13-22:40:30.139337 ------------------------------------------------------
+I 08/10/13-22:40:30.139408 ODR H/W Simulator startup
+I 08/10/13-22:40:30.139418 ODRSimApp::main() started
+I 08/10/13-22:40:30.139504 using CANbus interface name "can0"
+I 08/10/13-22:40:45.960987 ODR H/W Simulator shutdown
+I 08/10/13-22:41:21.006798 ------------------------------------------------------
+I 08/10/13-22:41:21.006887 ODR H/W Simulator startup
+I 08/10/13-22:41:21.006899 ODRSimApp::main() started
+I 08/10/13-22:41:21.007353 using CANbus interface name "can0"
+I 08/10/13-22:41:30.382858 ODR H/W Simulator shutdown
+I 08/10/13-22:42:09.184492 ------------------------------------------------------
+I 08/10/13-22:42:09.184581 ODR H/W Simulator startup
+I 08/10/13-22:42:09.184593 ODRSimApp::main() started
+I 08/10/13-22:42:09.184680 using CANbus interface name "can0"
+I 08/10/13-22:45:23.453537 ODR H/W Simulator shutdown
+I 08/10/13-22:45:44.661356 ------------------------------------------------------
+I 08/10/13-22:45:44.661428 ODR H/W Simulator startup
+I 08/10/13-22:45:44.661437 ODRSimApp::main() started
+I 08/10/13-22:45:44.661504 using CANbus interface name "can0"
+I 08/10/13-22:46:14.046225 ODR H/W Simulator shutdown
+I 08/10/13-23:06:03.703308 ------------------------------------------------------
+I 08/10/13-23:06:03.703389 ODR H/W Simulator startup
+I 08/10/13-23:06:03.703399 ODRSimApp::main() started
+I 08/10/13-23:06:03.703468 using CANbus interface name "can0"
+I 08/10/13-23:06:58.048740 ODR H/W Simulator shutdown
+I 08/10/13-23:10:50.459136 ------------------------------------------------------
+I 08/10/13-23:10:50.459213 ODR H/W Simulator startup
+I 08/10/13-23:10:50.459222 ODRSimApp::main() started
+I 08/10/13-23:10:50.459292 using CANbus interface name "can0"
+I 08/10/13-23:11:37.678220 ODR H/W Simulator shutdown
+I 08/11/13-18:21:33.889143 ------------------------------------------------------
+I 08/11/13-18:21:33.889227 ODR H/W Simulator startup
+I 08/11/13-18:21:33.889236 ODRSimApp::main() started
+I 08/11/13-18:21:33.889318 using CANbus interface name "can0"
+I 08/16/13-02:00:50.781111 ODR H/W Simulator shutdown
+I 08/17/13-18:07:44.662561 ------------------------------------------------------
+I 08/17/13-18:07:44.663440 ODR H/W Simulator startup
+I 08/17/13-18:07:44.663471 ODRSimApp::main() started
+I 08/17/13-18:07:44.663612 using CANbus interface name "can0"
+I 08/17/13-18:09:40.583010 ODR H/W Simulator shutdown
+I 07/12/14-18:25:54.972583 ------------------------------------------------------
+I 07/12/14-18:25:54.973245 ODR H/W Simulator startup
+I 07/12/14-18:25:54.973267 ODRSimApp::main() started
+I 07/12/14-18:25:54.973372 using CANbus interface name "can0"
+I 07/12/14-18:27:11.818084 ODR H/W Simulator shutdown
+I 07/12/14-19:00:23.939175 ------------------------------------------------------
+I 07/12/14-19:00:23.939292 ODR H/W Simulator startup
+I 07/12/14-19:00:23.939306 ODRSimApp::main() started
+I 07/12/14-19:00:23.939399 using CANbus interface name "can0"
+I 07/12/14-19:02:08.176230 ODR H/W Simulator shutdown
+I 07/12/14-19:21:40.436115 ------------------------------------------------------
+I 07/12/14-19:21:40.436234 ODR H/W Simulator startup
+I 07/12/14-19:21:40.436248 ODRSimApp::main() started
+I 07/12/14-19:21:40.436342 using CANbus interface name "can0"
+I 07/12/14-19:22:05.701147 ODR H/W Simulator shutdown
+I 07/12/14-19:40:23.244222 ------------------------------------------------------
+I 07/12/14-19:40:23.244334 ODR H/W Simulator startup
+I 07/12/14-19:40:23.244347 ODRSimApp::main() started
+I 07/12/14-19:40:23.244438 using CANbus interface name "can0"
+I 07/12/14-19:44:43.266963 ODR H/W Simulator shutdown
+I 07/12/14-19:47:51.473531 ------------------------------------------------------
+I 07/12/14-19:47:51.473649 ODR H/W Simulator startup
+I 07/12/14-19:47:51.473663 ODRSimApp::main() started
+I 07/12/14-19:47:51.473753 using CANbus interface name "can0"
+I 07/12/14-19:48:00.352214 ODR H/W Simulator shutdown
+I 07/12/14-20:17:52.302488 ------------------------------------------------------
+I 07/12/14-20:17:52.302848 ODR H/W Simulator startup
+I 07/12/14-20:17:52.302866 ODRSimApp::main() started
+I 07/12/14-20:17:52.302964 using CANbus interface name "can0"
+I 07/12/14-20:18:54.617871 ODR H/W Simulator shutdown
+I 07/12/14-20:24:05.654917 ------------------------------------------------------
+I 07/12/14-20:24:05.655027 ODR H/W Simulator startup
+I 07/12/14-20:24:05.655041 ODRSimApp::main() started
+I 07/12/14-20:24:05.655136 using CANbus interface name "can0"
+I 07/12/14-20:24:52.235568 ODR H/W Simulator shutdown
+I 07/12/14-22:04:28.953071 ------------------------------------------------------
+I 07/12/14-22:04:28.953192 ODR H/W Simulator startup
+I 07/12/14-22:04:28.953206 ODRSimApp::main() started
+I 07/12/14-22:04:28.953298 using CANbus interface name "can0"
+I 07/12/14-22:08:10.500004 ODR H/W Simulator shutdown
+I 07/12/14-22:18:54.694580 ------------------------------------------------------
+I 07/12/14-22:18:54.694698 ODR H/W Simulator startup
+I 07/12/14-22:18:54.694712 ODRSimApp::main() started
+I 07/12/14-22:18:54.694803 using CANbus interface name "can0"
+I 07/12/14-22:25:29.472422 ODR H/W Simulator shutdown
+I 07/12/14-22:25:54.573827 ------------------------------------------------------
+I 07/12/14-22:25:54.573932 ODR H/W Simulator startup
+I 07/12/14-22:25:54.573945 ODRSimApp::main() started
+I 07/12/14-22:25:54.574036 using CANbus interface name "can0"
+I 07/12/14-22:29:47.876123 ODR H/W Simulator shutdown
+I 07/13/14-01:04:28.359774 ------------------------------------------------------
+I 07/13/14-01:04:28.359885 ODR H/W Simulator startup
+I 07/13/14-01:04:28.359899 ODRSimApp::main() started
+I 07/13/14-01:04:28.359991 using CANbus interface name "can0"
+I 07/13/14-01:05:03.108401 ODR H/W Simulator shutdown
+I 07/13/14-01:08:11.853011 ------------------------------------------------------
+I 07/13/14-01:08:11.853286 ODR H/W Simulator startup
+I 07/13/14-01:08:11.853303 ODRSimApp::main() started
+I 07/13/14-01:08:11.853549 using CANbus interface name "can0"
+I 07/13/14-01:20:18.090331 ODR H/W Simulator shutdown
+I 07/13/14-01:25:10.431615 ------------------------------------------------------
+I 07/13/14-01:25:10.431829 ODR H/W Simulator startup
+I 07/13/14-01:25:10.431857 ODRSimApp::main() started
+I 07/13/14-01:25:10.432053 using CANbus interface name "can0"
+I 07/13/14-01:26:01.485262 ODR H/W Simulator shutdown
+I 07/13/14-01:26:24.930296 ------------------------------------------------------
+I 07/13/14-01:26:24.930404 ODR H/W Simulator startup
+I 07/13/14-01:26:24.930418 ODRSimApp::main() started
+I 07/13/14-01:26:24.930510 using CANbus interface name "can0"
+I 07/13/14-01:27:00.229683 ODR H/W Simulator shutdown
+I 07/13/14-01:31:20.908956 ------------------------------------------------------
+I 07/13/14-01:31:20.909050 ODR H/W Simulator startup
+I 07/13/14-01:31:20.909060 ODRSimApp::main() started
+I 07/13/14-01:31:20.909127 using CANbus interface name "can0"
+I 07/13/14-01:36:17.269122 ODR H/W Simulator shutdown
--- a/main/robots/odr/AvoidBySonarTask.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -201,6 +201,8 @@
         return; // stuck!
     }
 
+    int8_t adjustedSpeed = Scoreboard::navMaximumSpeed();
+
     if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
                                          && m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
     {
@@ -208,39 +210,46 @@
         {
             int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
             MotorsAndServos::servoPositions( -turn, turn );
+            adjustedSpeed -= ( 2 * turn );
         }
         else
         {
             int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
             MotorsAndServos::servoPositions( turn, -turn );
+            adjustedSpeed -= ( 2 * turn );
         }
     }
     else if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterLeft );
         MotorsAndServos::servoPositions( -turn, turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterRight );
         MotorsAndServos::servoPositions( turn, -turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontLeft < SIDE_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
         MotorsAndServos::servoPositions( -turn, turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontRight < SIDE_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
         MotorsAndServos::servoPositions( turn, -turn );
+        adjustedSpeed -= turn;
     }
     else
     {
         MotorsAndServos::servoPositions( 0, 0 ); // unexpected...
     }
 
-    MotorsAndServos::motorSpeeds( 15, 15 );
+    if ( adjustedSpeed < 5 ) { adjustedSpeed = 5; }
+    MotorsAndServos::motorSpeeds( adjustedSpeed, adjustedSpeed );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/CruisingTask.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/CruisingTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -73,7 +73,9 @@
     }
 
     MotorsAndServos::servoPositions( 0, 0 );
-    MotorsAndServos::motorSpeeds( 15, 15 );
+
+    int8_t speed = Scoreboard::navMaximumSpeed();
+    MotorsAndServos::motorSpeeds( speed, speed );
 }
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuApp.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,248 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuApp.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Application object for the IMU sensor bridge.
+//
+//  Copyright (c) 2014 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 <iostream>
+#include <memory>
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+#include <Poco/Logger.h>
+#include <Poco/Thread.h>
+#include <Poco/Util/Application.h>
+#include <Poco/Util/HelpFormatter.h>
+#include <Poco/Util/LoggingSubsystem.h>
+#include <Poco/Util/Option.h>
+#include <Poco/Util/OptionSet.h>
+
+#include "ImuApp.h"
+
+#include "ImuReader.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+void ImuApp::sendImuDataUpdate( Poco::Timer& timer )
+{
+    // if the IMU is not online then we don't send any messages
+
+    if ( ! ImuReader::isImuAlive() )
+    {
+        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 >( ImuReader::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 >( ImuReader::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 >( ImuReader::imuRoll() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuApp::ImuApp()
+    : m_helpRequested( false ),
+      m_runloopActive( false ),
+      m_timerSendImuDataUpdate()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuApp::~ImuApp()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuApp::initialize( Poco::Util::Application& self )
+{
+    loadConfiguration();
+    Poco::Util::ServerApplication::initialize( self );
+
+    addSubsystem( new Poco::Util::LoggingSubsystem() );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuApp::uninitialize()
+{
+    Poco::Util::ServerApplication::uninitialize();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuApp::defineOptions( Poco::Util::OptionSet& options )
+{
+    Poco::Util::ServerApplication::defineOptions( options );
+
+    options.addOption(
+        Poco::Util::Option( "help", "h", "display argument help information" )
+            .required( false )
+            .repeatable( false )
+            .callback( Poco::Util::OptionCallback<ImuApp>( this, &ImuApp::handleHelp ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuApp::handleHelp( const std::string& name, const std::string& value )
+{
+    Poco::Util::HelpFormatter helpFormatter( options() );
+    helpFormatter.setCommand( commandName() );
+    helpFormatter.format( std::cout );
+    stopOptionsProcessing();
+    m_helpRequested = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int ImuApp::main( const std::vector<std::string>& args )
+{
+    if ( m_helpRequested )
+    {
+        return Poco::Util::Application::EXIT_OK;
+    }
+
+    loadConfiguration();
+
+    logger().information( "------------------------------------------------------" );
+    logger().information( "ImuApp::main() started" );
+
+    std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
+    logger().information( "using CANbus interface name \"" + canInterfaceName + "\"" );
+
+    Poco::Thread    canMsgProcessorThread;
+    CANMsgProcessor canMsgProcessor( canInterfaceName, logger().name() );
+    canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
+    canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
+    canMsgProcessorThread.start( canMsgProcessor );
+
+    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_timerSendImuDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
+    m_timerSendImuDataUpdate.start(
+            Poco::TimerCallback< ImuApp >( *this, &ImuApp::sendImuDataUpdate ) );
+
+    m_runloopActive = true;
+    while ( m_runloopActive ) Poco::Thread::sleep( 5000 /* milliseconds */ );
+
+    imuReader.timeToQuit();
+    imuReaderThread.join();
+
+    canMsgProcessor.timeToQuit();
+    canMsgProcessorThread.join();
+
+    logger().information( "ImuApp::main() stopping" );
+
+    return Poco::Util::Application::EXIT_OK;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int main( int argc, char** argv )
+{
+    int result = -1;
+
+    try
+    {
+        ImuApp app;
+        result = app.run( argc, argv );
+    }
+    catch ( const Poco::Exception& ex )
+    {
+        std::cerr << "Failed to start application, Poco::Exception: "
+            << ex.displayText() << std::endl;
+    }
+    catch ( const std::exception& ex )
+    {
+        std::cerr << "Failed to start application, std::exception: "
+            << ex.what() << std::endl;
+    }
+    catch ( ... )
+    {
+        std::cerr << "Failed to start application, unknown exception" << std::endl;
+    }
+
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuApp.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,76 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuApp.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//    
+//  Application object for the IMU sensor bridge.
+//
+//  Copyright (c) 2014 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_IMUAPP_H
+#define BCDRL_ROBOTS_ODR_IMUAPP_H
+
+#include <Poco/Timer.h>
+#include <Poco/Util/ServerApplication.h>
+
+namespace Poco
+{
+    namespace Util
+    {
+        class OptionSet;
+    }
+}
+
+#include <string>
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+class ImuApp : public Poco::Util::ServerApplication
+{
+    public:
+        ImuApp();
+        virtual ~ImuApp();
+
+    protected:
+        void initialize( Poco::Util::Application& self );
+        void uninitialize();
+        void defineOptions( Poco::Util::OptionSet& options );
+        void handleHelp( const std::string& name, const std::string& value );
+        int  main( const std::vector< std::string >& args );
+
+    private:
+        void sendImuDataUpdate( Poco::Timer& timer );
+
+    private:
+        bool        m_helpRequested;
+        bool        m_runloopActive;
+        Poco::Timer m_timerSendImuDataUpdate;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_IMUAPP_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ImuReader.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/ImuReader.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
 //
-//  Copyright (c) 2012 Bob Cook
+//  Copyright (c) 2012-2014 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
@@ -40,7 +40,11 @@
 #include <termios.h>
 #include <unistd.h>
 
+#include <algorithm>
+#include <cmath>
 #include <stdexcept>
+#include <utility>
+#include <vector>
 
 #include <Poco/Exception.h>
 #include <Poco/Logger.h>
@@ -48,8 +52,6 @@
 #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"
@@ -60,6 +62,84 @@
 
 // ----------------------------------------------------------------------------------------
 
+Poco::RWLock    ImuReader::sm_rwLock;
+bool            ImuReader::sm_isImuAlive;
+Poco::Timestamp ImuReader::sm_imuLastUpdate;
+double          ImuReader::sm_imuRoll;
+double          ImuReader::sm_imuPitch;
+double          ImuReader::sm_imuYaw;
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::imuIsOffline()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isImuAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuReader::isImuAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( ! sm_isImuAlive )
+    {
+        return false;
+    }
+
+    // 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 sm_isImuAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuReader::imuRoll()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuRoll;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuReader::imuPitch()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuPitch;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuReader::imuYaw()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_imuYaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::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;
+}
+
+// ----------------------------------------------------------------------------------------
+
 ImuReader::ImuReader( const std::string& loggerName, const std::string& serialPortPath )
     : Poco::Runnable(),
       m_loggerName( loggerName ),
@@ -350,6 +430,176 @@
 }
 
 // ----------------------------------------------------------------------------------------
+//  Compute the calibrated yaw value from the uncalibrated (raw) input. This method uses
+//  data derived experimentally on a rotating table.
+//
+//  This data is in the range of 0 to 180, -180 to 0.
+//
+
+static int yaw_uncalibrated_values[ 36 ] = {
+      38,   46,   54,   61,   68,   76,   83,   90,   98,  105,
+     113,  121,  128,  137,  145,  154,  163,  172, -177, -166,
+    -155, -142, -129, -114,  -98,  -83,  -67,  -53,  -39,  -27,
+     -15,   -5,    4,   13,   22,   30
+};
+
+static double yaw_uncalibrated_offset = 18.0;
+
+typedef std::pair< double, double > yaw_cal_table_item_t;
+typedef std::vector< yaw_cal_table_item_t > yaw_cal_table_t;
+
+static yaw_cal_table_t yaw_cal_table;
+
+// ----------------------------------------------------------------------------------------
+//  Initialize the yaw calibration table using the translation data. This results in a
+//  table mapping between uncalibrated values and the corresponding "actual" value.
+//
+//  The table contains values in the range of 0 to 360, sorted by the uncalibrated values.
+
+static bool yaw_cal_table_item_sorter( yaw_cal_table_item_t a, yaw_cal_table_item_t b )
+{
+    return a.first < b.first;
+}
+
+static void initYawCalibrationTable()
+{
+    yaw_cal_table.clear();
+
+    for ( size_t i = 0; i < array_sizeof( yaw_uncalibrated_values ); ++i )
+    {
+        double v = static_cast< double >( yaw_uncalibrated_values[ i ] );
+        double a = yaw_uncalibrated_offset + ( static_cast< double >( i ) * 10.0 );
+
+        // adjust the uncalibrated data from the range of [-180,180] to [0,360]
+        v += 180.0;
+
+        yaw_cal_table.push_back( std::make_pair( v, a ) );
+    }
+
+    std::sort( yaw_cal_table.begin(), yaw_cal_table.end(), yaw_cal_table_item_sorter );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double getUncalibratedYawValueAtIndex( int index )
+{
+    while ( index < 0 )
+    {
+        index += yaw_cal_table.size();
+    }
+
+    index %= yaw_cal_table.size();
+
+    return yaw_cal_table[ index ].first;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double getCalibratedYawValueAtIndex( int index )
+{
+    while ( index < 0 )
+    {
+        index += yaw_cal_table.size();
+    }
+
+    index %= yaw_cal_table.size();
+
+    return yaw_cal_table[ index ].second;
+}
+
+// ----------------------------------------------------------------------------------------
+//  Translate the uncalibrated yaw value to a calibrated value. This is done by iterating
+//  through the calibration table to find the two values on either side of the input
+//  value then interpolating (linerally) between the two bounding points.
+//
+//  The only tricky consideration is where the two bounding points are on either side of
+//  the 0 degree (or 180 degree) boundry. To make this easier (only a single point to
+//  be concerned about) we do all the calculations in the range of [0, 360] then check
+//  for the "wrap-around" at the zero point.
+
+static double calibrateYawValue( double uncalibratedYaw )
+{
+    //--    Adjust the uncalibrated data from the range of [-180,180] to [0,360].
+
+    double inputUncalValue = uncalibratedYaw + 180.0;
+
+    //--    Find the index of the two bounding points around the uncalibrated value.
+
+    int indexUncalHi = 0;
+
+    for ( ; indexUncalHi < 36; ++indexUncalHi )
+    {
+        if ( getUncalibratedYawValueAtIndex( indexUncalHi ) > inputUncalValue ) break;
+    }
+
+    int indexUncalLo = indexUncalHi - 1;
+
+    double uncalHi = getUncalibratedYawValueAtIndex( indexUncalHi );
+    double uncalLo = getUncalibratedYawValueAtIndex( indexUncalLo );
+
+    //--    The case of two points on opposite sides of the origin (zero point) is slightly
+    //      more complicated.
+
+    if ( uncalHi - uncalLo > 180.0 || uncalLo - uncalHi > 180.0 )
+    {
+        //--    Wrap-around the zero point; shift to negative coordinates as needed and
+        //      recompute the fraction.
+
+        if ( uncalLo > 180.0 )
+        {
+            uncalLo -= 360.0;
+        }
+        else
+        {
+            uncalHi -= 360.0;
+        }
+
+        if ( inputUncalValue > 180.0 )
+        {
+            inputUncalValue -= 360.0;
+        }
+    }
+
+    double uncalFract = std::abs( inputUncalValue - uncalLo ) / std::abs( uncalLo - uncalHi );
+
+    //--    Fetch the calibrated values for interpolation.
+
+    double calLo = getCalibratedYawValueAtIndex( indexUncalLo );
+    double calHi = getCalibratedYawValueAtIndex( indexUncalHi );
+
+    //--    The case of two points on opposite sides of the origin (zero point) is slightly
+    //      more complicated.
+
+    if ( calHi - calLo > 180.0 || calLo - calHi > 180.0 )
+    {
+        //--    Wrap-around the zero point; shift to negative coordinates as needed.
+
+        if ( calLo > 180.0 )
+        {
+            calLo -= 360.0;
+        }
+        else
+        {
+            calHi -= 360.0;
+        }
+    }
+
+    if ( calLo > calHi )
+    {
+        std::swap( calLo, calHi );
+    }
+
+    double result = ( ( calHi - calLo ) * uncalFract ) + calLo;
+    result = ::fmod( result, 360.0 );
+
+    //--    Adjust the calibrated result from the range of [0, 360] to [-180,180].
+
+    result -= 180.0;
+
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
 //  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.
 //
@@ -423,11 +673,15 @@
     {
         log.information( std::string( "ImuReader::run() starting" ) );
 
+        // initialize the calibration table
+
+        initYawCalibrationTable();
+
         try
         {
             // to keep ourselves sane, start by assuming the IMU is offline
 
-            Scoreboard::imuIsOffline();
+            ImuReader::imuIsOffline();
 
             // now try to find it on the serial port
 
@@ -470,7 +724,7 @@
                 {
                     if ( ++timeoutNoData == 10 )
                     {
-                        Scoreboard::imuIsOffline();
+                        ImuReader::imuIsOffline();
                     }
                     continue;
                 }
@@ -483,7 +737,7 @@
                 {
                     // new values available, save them
 
-                    valuesYaw.push_back( m_parseValueYaw );
+                    valuesYaw.push_back( calibrateYawValue( m_parseValueYaw ) );
 
                     // if we've reached 10 values, compute the average and update the
                     // scoreboard; then clear the vector making room for new values
@@ -492,9 +746,9 @@
                     {
                         double averageYaw = computeAverageYaw( valuesYaw );
 
-                        Scoreboard::imuUpdate( m_parseValueRoll,
-                                               m_parseValuePitch,
-                                               averageYaw );
+                        ImuReader::imuUpdate( m_parseValueRoll,
+                                              m_parseValuePitch,
+                                              averageYaw );
 
                         // clear the vector, to accumulate more samples
 
--- a/main/robots/odr/ImuReader.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/ImuReader.h	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
 //
-//  Copyright (c) 2012 Bob Cook
+//  Copyright (c) 2012-2014 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,12 +38,21 @@
 #include <Poco/Runnable.h>
 #include <Poco/RWLock.h>
 #include <Poco/Timer.h>
+#include <Poco/Timestamp.h>
 
 // ----------------------------------------------------------------------------------------
 
 class ImuReader : public Poco::Runnable
 {
     public:
+        static void   imuIsOffline();
+        static bool   isImuAlive();
+        static double imuRoll();
+        static double imuPitch();
+        static double imuYaw();
+        static void   imuUpdate( double roll, double pitch, double yaw );
+
+    public:
         ImuReader( const std::string& loggerName, const std::string& serialPortPath );
         virtual ~ImuReader();
         virtual void run();
@@ -64,6 +73,14 @@
         }   imu_parse_state_t;
 
     private:
+        static Poco::RWLock    sm_rwLock;
+        static bool            sm_isImuAlive;
+        static double          sm_imuRoll;
+        static double          sm_imuPitch;
+        static double          sm_imuYaw;
+        static Poco::Timestamp sm_imuLastUpdate;
+
+    private:
         std::string       m_loggerName;
         std::string       m_serialPortPath;
         Poco::Event       m_quitEvent;
--- a/main/robots/odr/NavigateTask.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -125,15 +125,19 @@
         servoPosition = 12;
     }
 
+#if 0
     log().information(
         Poco::Logger::format(
             "NavigateTask: heading: $0 diff: $1 steer: $2",
             Poco::NumberFormatter::format( Scoreboard::imuYaw() ),
             Poco::NumberFormatter::format( headingDiff ),
             Poco::NumberFormatter::format( servoPosition ) ) );
+#endif
 
     MotorsAndServos::servoPositions( -servoPosition, servoPosition );
-    MotorsAndServos::motorSpeeds( 15, 15 );
+
+    int8_t speed = Scoreboard::navMaximumSpeed();
+    MotorsAndServos::motorSpeeds( speed, speed );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/ODRApp.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/ODRApp.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Application object implementation for the outdoor robot project.
 //
-//  Copyright (c) 2011-2013 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -47,14 +47,15 @@
 
 #include "AvoidBySonarTask.h"
 #include "CruisingTask.h"
-#include "ImuReader.h"
 #include "MotorsAndServos.h"
 #include "NavigateTask.h"
 #include "SafetyTask.h"
 #include "Scoreboard.h"
 #include "Sonar.h"
+#include "SquareCourseTask.h"
 #include "StartupTask.h"
 #include "TaskObject.h"
+#include "TrackWaypointsTask.h"
 
 #include "packages/common/can/can_helpers.h"
 
@@ -145,31 +146,24 @@
     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" );
-    }
+    Scoreboard::navSetMaximumSpeed( 15 );
 
     m_tasks.clear();
 
     m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new SafetyTask( logger().name() ) ) );
+
+    m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new StartupTask( logger().name() ) ) );
 
     m_tasks.push_back(
-            Poco::SharedPtr< TaskObject >( new SafetyTask( logger().name() ) ) );
+            Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
 
-    m_tasks.push_back(
-            Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
+//    m_tasks.push_back(
+//            Poco::SharedPtr< TaskObject >( new TrackWaypointsTask( logger().name() ) ) );
+
+//    m_tasks.push_back(
+//            Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );
 
 //    m_tasks.push_back(
 //            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
--- a/main/robots/odr/SafetyTask.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/SafetyTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Subsumption task to enact safety measures under certain unsafe conditions.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -39,8 +39,11 @@
 SafetyTask::SafetyTask( const std::string& loggerName )
     : TaskObject( loggerName ),
       m_lastMessageTime(),
-      m_lastEstopStatus( Scoreboard::isEStopActive() ),
-      m_lastControllerStatus( Scoreboard::isControllerAlive() ),
+      m_lastEmergencyStatus( Scoreboard::isEmergencyActive() ),
+      m_lastDisplayStatus( Scoreboard::isDisplayAlive() ),
+      m_lastGpsStatus( Scoreboard::isGpsAlive() ),
+      m_lastImuStatus( Scoreboard::isImuAlive() ),
+      m_lastMotionStatus( Scoreboard::isMotionAlive() ),
       m_lastSonarFrontStatus( Scoreboard::isSonarFrontAlive() )
 {
 }
@@ -62,22 +65,76 @@
 
 bool SafetyTask::wantsControl()
 {
-    // odr-controller
+    // overall emergency situation
     
-    if ( ! Scoreboard::isControllerAlive() )
+    if ( Scoreboard::isEmergencyActive() )
     {
-        if ( m_lastControllerStatus )
+        if ( ! m_lastEmergencyStatus )
         {
-            log().information( "SafetyTask: controller is not alive" );
-            m_lastControllerStatus = false;
+            log().information( "SafetyTask: emergency situation active" );
+            m_lastEmergencyStatus = true;
+        }
+
+        return true;
+    }
+    else if ( m_lastEmergencyStatus )
+    {
+        log().information( "SafetyTask: emergency situation cleared" );
+        m_lastEmergencyStatus = false;
+    }
+
+    // odr-display
+    
+    if ( ! Scoreboard::isDisplayAlive() )
+    {
+        if ( m_lastDisplayStatus )
+        {
+            log().information( "SafetyTask: odr-display is not alive" );
+            m_lastDisplayStatus = false;
         }
 
         return true;
     }
-    else if ( ! m_lastControllerStatus )
+    else if ( ! m_lastDisplayStatus )
+    {
+        log().information( "SafetyTask: odr-display is alive" );
+        m_lastDisplayStatus = true;
+    }
+
+    // odr-gps
+    
+    if ( ! Scoreboard::isGpsAlive() )
+    {
+        if ( m_lastGpsStatus )
+        {
+            log().information( "SafetyTask: sensor-gps is not alive" );
+            m_lastGpsStatus = false;
+        }
+
+        return true;
+    }
+    else if ( ! m_lastGpsStatus )
     {
-        log().information( "SafetyTask: controller is alive" );
-        m_lastControllerStatus = true;
+        log().information( "SafetyTask: sensor-gps is alive" );
+        m_lastGpsStatus = true;
+    }
+
+    // odr-motion
+    
+    if ( ! Scoreboard::isMotionAlive() )
+    {
+        if ( m_lastMotionStatus )
+        {
+            log().information( "SafetyTask: odr-motion is not alive" );
+            m_lastMotionStatus = false;
+        }
+
+        return true;
+    }
+    else if ( ! m_lastMotionStatus )
+    {
+        log().information( "SafetyTask: odr-motion is alive" );
+        m_lastMotionStatus = true;
     }
 
     // odr-sonar-front
@@ -98,22 +155,22 @@
         m_lastSonarFrontStatus = true;
     }
 
-    // emergency stop
+    // imu
     
-    if ( Scoreboard::isEStopActive() )
+    if ( ! Scoreboard::isImuAlive() )
     {
-        if ( ! m_lastEstopStatus )
+        if ( m_lastImuStatus )
         {
-            log().information( "SafetyTask: estop is active" );
-            m_lastEstopStatus = true;
+            log().information( "SafetyTask: sensor-imu is not alive" );
+            m_lastImuStatus = false;
         }
 
-//        return true;
+        return true;
     }
-    else if ( m_lastEstopStatus )
+    else if ( ! m_lastImuStatus )
     {
-        log().information( "SafetyTask: estop is not active" );
-        m_lastEstopStatus = false;
+        log().information( "SafetyTask: sensor-imu is alive" );
+        m_lastImuStatus = true;
     }
 
     // otherwise no issues
--- a/main/robots/odr/SafetyTask.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/SafetyTask.h	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Subsumption task to enact safety measures under certain unsafe conditions.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -47,8 +47,11 @@
 
     private:
         Poco::Timestamp m_lastMessageTime;
-        bool            m_lastEstopStatus;
-        bool            m_lastControllerStatus;
+        bool            m_lastEmergencyStatus;
+        bool            m_lastDisplayStatus;
+        bool            m_lastGpsStatus;
+        bool            m_lastImuStatus;
+        bool            m_lastMotionStatus;
         bool            m_lastSonarFrontStatus;
 };
 
--- a/main/robots/odr/Scoreboard.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2010-2013 Bob Cook
+//  Copyright (c) 2010-2014 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
@@ -46,61 +46,103 @@
 
 // ----------------------------------------------------------------------------------------
 
-Poco::RWLock    Scoreboard::sm_rwLock;
-bool            Scoreboard::sm_isEmergencyActive;
-bool            Scoreboard::sm_MotionEmergency;
-bool            Scoreboard::sm_isMotionAlive;
-bool            Scoreboard::sm_isControllerAlive;
-bool            Scoreboard::sm_isEStopActive;
-bool            Scoreboard::sm_isButtonOneDown;
-bool            Scoreboard::sm_isButtonTwoDown;
+// scoreboard lock
+Poco::RWLock Scoreboard::sm_rwLock;
+
+// overall emergency state
+bool Scoreboard::sm_isEmergencyActive;
+
+// message broadcast
 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 = 100.0;
+
+// odr-display
+bool Scoreboard::sm_DisplayEmergency;
+bool Scoreboard::sm_isDisplayAlive;
+
+// odr-gps
+bool            Scoreboard::sm_GpsEmergency;
+bool            Scoreboard::sm_isGpsAlive;
+bool            Scoreboard::sm_gpsHasFix;
+std::list< double > Scoreboard::sm_gpsLatitudes;
+double                Scoreboard::sm_gpsLatitudeAvg;
+std::list< double > Scoreboard::sm_gpsLongitudes;
+double                Scoreboard::sm_gpsLongitudeAvg;
+
+// odr-motion
+bool Scoreboard::sm_MotionEmergency;
+bool Scoreboard::sm_isMotionAlive;
+
+// odr-sonar-front
+bool    Scoreboard::sm_SonarFrontEmergency;
+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;
+
+// imu
+bool   Scoreboard::sm_isImuAlive;
+double Scoreboard::sm_imuRoll;
+double Scoreboard::sm_imuPitch;
+double Scoreboard::sm_imuYaw;
+
+// program
+bool    Scoreboard::sm_hasActiveProgram;
+uint8_t Scoreboard::sm_activeProgram;
+
+// navigation
+int8_t Scoreboard::sm_navMaximumSpeed;
+double Scoreboard::sm_navTargetHeading;
 
 // ----------------------------------------------------------------------------------------
 
-bool Scoreboard::isControllerAlive()
+bool Scoreboard::isEmergencyActive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isControllerAlive;
+    
+    sm_isEmergencyActive = sm_DisplayEmergency    | ! sm_isDisplayAlive
+                         | sm_GpsEmergency        | ! sm_isGpsAlive
+                         | sm_MotionEmergency     | ! sm_isMotionAlive
+                         | sm_SonarFrontEmergency | ! sm_isSonarFrontAlive
+                         ;
+
+    return sm_isEmergencyActive;
 }
 
 // ----------------------------------------------------------------------------------------
 
-bool Scoreboard::isEStopActive()
+bool Scoreboard::isDisplayAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isEStopActive;
+    return sm_isDisplayAlive;
 }
 
 // ----------------------------------------------------------------------------------------
 
-bool Scoreboard::isButtonOneDown()
+bool Scoreboard::isGpsAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isButtonOneDown;
+    return sm_isGpsAlive;
 }
 
 // ----------------------------------------------------------------------------------------
 
-bool Scoreboard::isButtonTwoDown()
+bool Scoreboard::isMotionAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isButtonTwoDown;
+    return sm_isMotionAlive;
 }
-                 
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isSonarFrontAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isSonarFrontAlive;
+}
+
 // ----------------------------------------------------------------------------------------
 
 void Scoreboard::sendManagerMessage( const char* msg )
@@ -140,14 +182,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-bool Scoreboard::isSonarFrontAlive()
-{
-    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isSonarFrontAlive;
-}
-
-// ----------------------------------------------------------------------------------------
-
 bool Scoreboard::isSonarFrontEnabled()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
@@ -188,6 +222,30 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool Scoreboard::gpsHasFix()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsHasFix;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::gpsLatitude()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsLatitudeAvg;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::gpsLongitude()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsLongitudeAvg;
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool Scoreboard::isImuAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
@@ -220,24 +278,56 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::imuUpdate( double roll, double pitch, double yaw )
+bool Scoreboard::isProgramRunning()
 {
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_hasActiveProgram;
+}
+
+// ----------------------------------------------------------------------------------------
 
-    sm_isImuAlive = true;
-    sm_imuLastUpdate.update();
-
-    sm_imuRoll  = roll;
-    sm_imuPitch = pitch;
-    sm_imuYaw   = yaw;
+uint8_t Scoreboard::activeProgram()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    if ( ! sm_hasActiveProgram )
+    {
+        return 0;
+    }
+    return sm_activeProgram;
 }
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::imuIsOffline()
+void Scoreboard::setActiveProgram( uint8_t program )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_hasActiveProgram = true;
+    sm_activeProgram    = program;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::stopActiveProgram()
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-    sm_isImuAlive = false;
+    sm_hasActiveProgram = false;
+    sm_activeProgram    = 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t Scoreboard::navMaximumSpeed()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_navMaximumSpeed;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::navSetMaximumSpeed( int8_t max )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_navMaximumSpeed = max;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -262,34 +352,46 @@
     : Poco::Runnable(),
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
+      m_timerLogStatus(),
       m_timerSendManagerUpdate(),
-      m_timerMotionUpdate(),
-      m_timerControllerUpdate(),
-      m_timerSonarFrontUpdate(),
-      m_timerSendImuDataUpdate()
+      m_timerDisplayTimeout(),
+      m_timerGpsTimeout(),
+      m_timerImuTimeout(),
+      m_timerMotionTimeout(),
+      m_timerSonarFrontTimeout()
 {
+    m_timerLogStatus.setPeriodicInterval( 5000 );
+    m_timerLogStatus.start(
+            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::logSystemStatus ) );
+
     m_timerSendManagerUpdate.setPeriodicInterval( 2500 );
     m_timerSendManagerUpdate.start(
             Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
-    m_timerMotionUpdate.setPeriodicInterval( 5000 );
-    m_timerMotionUpdate.start(
+    m_timerDisplayTimeout.setPeriodicInterval( 5000 );
+    m_timerDisplayTimeout.start(
             Poco::TimerCallback< Scoreboard >(
-                    *this, &Scoreboard::timeoutMotionUpdate ) );
+                    *this, &Scoreboard::timeoutDisplayNode ) );
 
-    m_timerControllerUpdate.setPeriodicInterval( 5000 );
-    m_timerControllerUpdate.start(
+    m_timerGpsTimeout.setPeriodicInterval( 5000 );
+    m_timerGpsTimeout.start(
             Poco::TimerCallback< Scoreboard >(
-                    *this, &Scoreboard::timeoutControllerUpdate ) );
+                    *this, &Scoreboard::timeoutGpsSensorNode ) );
 
-    m_timerSonarFrontUpdate.setPeriodicInterval( 5000 );
-    m_timerSonarFrontUpdate.start(
+    m_timerImuTimeout.setPeriodicInterval( 5000 );
+    m_timerImuTimeout.start(
             Poco::TimerCallback< Scoreboard >(
-                    *this, &Scoreboard::timeoutSonarFrontUpdate ) );
+                    *this, &Scoreboard::timeoutImuNode ) );
 
-    m_timerSendImuDataUpdate.setPeriodicInterval( 750 );
-    m_timerSendImuDataUpdate.start(
-            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendImuDataUpdate ) );
+    m_timerMotionTimeout.setPeriodicInterval( 5000 );
+    m_timerMotionTimeout.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutMotionNode ) );
+
+    m_timerSonarFrontTimeout.setPeriodicInterval( 5000 );
+    m_timerSonarFrontTimeout.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutSonarFrontNode ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -307,10 +409,48 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::logSystemStatus( Poco::Timer& timer )
+{
+#if 0
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( sm_isGpsAlive )
+    {
+        if ( sm_gpsHasFix )
+        {
+            log.information(
+                Poco::Logger::format(
+                    "Scoreboard: GPS latitude: $0 longitude: $1",
+                    Poco::NumberFormatter::format( sm_gpsLatitudeAvg, 5 ),
+                    Poco::NumberFormatter::format( sm_gpsLongitudeAvg, 5 ) ) );
+        }
+        else
+        {
+            log.information( "Scoreboard: GPS has no position fix" );
+        }
+    }
+    else
+    {
+        log.information( "Scoreboard: GPS is not alive" );
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::sendManagerUpdate( Poco::Timer& timer )
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
 
+    // send a heartbeat as if we are odr-controller (which is deprecated)
+
+    uint32_t hbeatmsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_heartbeat );
+
+    CANMessage::QueueToSend( new CANMessage( hbeatmsgid ) );
+
     // send the manager status message
 
     uint32_t updatemsgid = can_build_message_id(
@@ -318,13 +458,9 @@
 
     CANMessage::QueueToSend( new CANMessage( updatemsgid ) );
 
-    // lock the scoreboard data for read-only access
-
-    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-
     // update the emergency / all-clear status
 
-    if ( sm_isEmergencyActive )
+    if ( Scoreboard::isEmergencyActive() )
     {
         uint32_t msgid = can_build_message_id(
                     can_node_odr_manager, can_node_broadcast, can_dataid_emergency );
@@ -356,7 +492,31 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::timeoutMotionUpdate( Poco::Timer& timer )
+void Scoreboard::timeoutDisplayNode( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isDisplayAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::timeoutGpsSensorNode( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isGpsAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::timeoutImuNode( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isImuAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::timeoutMotionNode( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_isMotionAlive = false;
@@ -364,15 +524,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::timeoutControllerUpdate( Poco::Timer& timer )
-{
-    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
-    sm_isControllerAlive = false;
-}
-
-// ----------------------------------------------------------------------------------------
-
-void Scoreboard::timeoutSonarFrontUpdate( Poco::Timer& timer )
+void Scoreboard::timeoutSonarFrontNode( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_isSonarFrontAlive = false;
@@ -380,58 +532,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-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::recvEmergency( uint8_t srcNode )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -440,9 +540,21 @@
 
     switch ( srcNode )
     {
+        case can_node_odr_display:
+            sm_DisplayEmergency = true;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = true;
+            break;
+
         case can_node_odr_motion:
             sm_MotionEmergency = true;
             break;
+
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = true;
+            break;
     }
 }
 
@@ -454,15 +566,21 @@
 
     switch ( srcNode )
     {
+        case can_node_odr_display:
+            sm_DisplayEmergency = false;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = false;
+            break;
+
         case can_node_odr_motion:
             sm_MotionEmergency = false;
             break;
-    }
 
-    if ( sm_isEmergencyActive )
-    {
-        sm_isEmergencyActive = sm_MotionEmergency; // | sm_xxxEmergency
-
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = false;
+            break;
     }
 }
 
@@ -474,48 +592,30 @@
 
     switch ( srcNode )
     {
-        case can_node_odr_controller:
-            m_timerControllerUpdate.restart();
-            sm_isControllerAlive = true;
+        case can_node_odr_display:
+            m_timerDisplayTimeout.restart();
+            sm_isDisplayAlive = true;
+            break;
+
+        case can_node_sensor_gps:
+            m_timerGpsTimeout.restart();
+            sm_isGpsAlive = true;
+            break;
+
+        case can_node_odr_motion:
+            m_timerMotionTimeout.restart();
+            sm_isMotionAlive = 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::recvSonarFrontStatus( bool enabled )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
 
-    m_timerSonarFrontUpdate.restart();
+    m_timerSonarFrontTimeout.restart();
     sm_isSonarFrontAlive = true;
 
     sm_isSonarFrontEnabled = enabled;
@@ -538,6 +638,146 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvGpsFixInfo( CANMessage* msg )
+{
+    can_data_gps_fix info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_gpsHasFix = info.satellites > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvGpsLatitude( CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double lat = static_cast< double >( info.degrees );
+    double min = static_cast< double >( info.min_thousandths )
+               / static_cast< double >( can_data_gps_min_multiplier );
+    lat += ( min / 60.0L );
+    
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_gpsLatitudes.push_back( lat );
+    while ( sm_gpsLatitudes.size() > 10 )
+    {
+        sm_gpsLatitudes.pop_front();
+    }
+
+    std::list< double >::iterator itr = sm_gpsLatitudes.begin();
+    double latsTotal = 0.0;
+
+    while ( itr != sm_gpsLatitudes.end() )
+    {
+        latsTotal += *itr++;
+    }
+
+    sm_gpsLatitudeAvg = latsTotal / sm_gpsLatitudes.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvGpsLongitude( CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double lon = static_cast< double >( info.degrees );
+    double min = static_cast< double >( info.min_thousandths )
+               / static_cast< double >( can_data_gps_min_multiplier );
+    lon += ( min / 60.0L );
+    
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_gpsLongitudes.push_back( lon );
+    while ( sm_gpsLongitudes.size() > 10 )
+    {
+        sm_gpsLongitudes.pop_front();
+    }
+
+    std::list< double >::iterator itr = sm_gpsLongitudes.begin();
+    double lonsTotal = 0.0;
+
+    while ( itr != sm_gpsLongitudes.end() )
+    {
+        lonsTotal += *itr++;
+    }
+
+    sm_gpsLongitudeAvg = lonsTotal / sm_gpsLongitudes.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvImuRoll( CANMessage* msg )
+{
+    can_data_imu_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double roll = static_cast< double >( info.data )
+                / static_cast< double >( can_data_imu_multiplier );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isImuAlive = true;
+    m_timerImuTimeout.restart();
+    
+    sm_imuRoll = roll;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvImuPitch( CANMessage* msg )
+{
+    can_data_imu_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double pitch = static_cast< double >( info.data )
+                 / static_cast< double >( can_data_imu_multiplier );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isImuAlive = true;
+    m_timerImuTimeout.restart();
+    
+    sm_imuPitch = pitch;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvImuYaw( CANMessage* msg )
+{
+    can_data_imu_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double yaw = static_cast< double >( info.data )
+               / static_cast< double >( can_data_imu_multiplier );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isImuAlive = true;
+    m_timerImuTimeout.restart();
+    
+    sm_imuYaw = yaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvStartProgram( CANMessage* msg )
+{
+    can_data_pgm_selection info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_hasActiveProgram = true;
+    sm_activeProgram = info.pgm_id;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -584,14 +824,6 @@
                         recvHeartbeat( srcNode );
                         break;
 
-                    case can_dataid_odrctl_update:
-                        recvControllerUpdate( msg );
-                        break;
-
-                    case can_dataid_odrctl_button:
-                        recvButtonUpdate( msg );
-                        break;
-
                     case can_dataid_sonar_front_state_enabled:
                         recvSonarFrontStatus( true /* enabled */ );
                         break;
@@ -604,6 +836,34 @@
                         recvSonarFrontStatus( true /* enabled */ );
                         recvSonarFrontValues( msg );
                         break;
+
+                    case can_dataid_gps_fix:
+                        recvGpsFixInfo( msg );
+                        break;
+
+                    case can_dataid_gps_latitude:
+                        recvGpsLatitude( msg );
+                        break;
+
+                    case can_dataid_gps_longitude:
+                        recvGpsLongitude( msg );
+                        break;
+
+                    case can_dataid_imu_roll:
+                        recvImuRoll( msg );
+                        break;
+
+                    case can_dataid_imu_pitch:
+                        recvImuPitch( msg );
+                        break;
+
+                    case can_dataid_imu_yaw:
+                        recvImuYaw( msg );
+                        break;
+
+                    case can_dataid_odr_start_pgm:
+                        recvStartProgram( msg );
+                        break;
                 }
             }
         }
--- a/main/robots/odr/Scoreboard.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/Scoreboard.h	Sun Jul 13 09:15:53 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2010-2013 Bob Cook
+//  Copyright (c) 2010-2014 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
@@ -33,6 +33,8 @@
 #define BCDRL_ROBOTS_ODR_SCOREBOARD_H
 
 #include <stdint.h>
+#include <queue>
+#include <list>
 
 #include <Poco/Event.h>
 #include <Poco/Runnable.h>
@@ -47,16 +49,28 @@
 class Scoreboard : public Poco::Runnable
 {
     public:
+        // overall emergency state
+        static bool isEmergencyActive();
+
+        // odr-display
+        static bool isDisplayAlive();
+        static void sendManagerMessage( const char* msg );
+
+        // odr-gps
+        static bool   isGpsAlive();
+        static bool   gpsHasFix();
+        static double gpsLatitude();
+        static double gpsLongitude();
+
+        // imu
+        static bool   isImuAlive();
+        static double imuRoll();
+        static double imuPitch();
+        static double imuYaw();
+
         // odr-motion
         static bool isMotionAlive();
 
-        // 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 bool    isSonarFrontEnabled();
@@ -65,15 +79,15 @@
         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();
+        // programs
+        static bool    isProgramRunning();
+        static uint8_t activeProgram();
+        static void    setActiveProgram( uint8_t program );
+        static void    stopActiveProgram();
 
         // navigation
+        static int8_t navMaximumSpeed();
+        static void   navSetMaximumSpeed( int8_t max );
         static double navTargetHeading();
         static void   navSetTargetHeading( double heading );
 
@@ -84,51 +98,88 @@
         void timeToQuit();
 
     private:
-        static Poco::RWLock    sm_rwLock;
-        static bool            sm_isEmergencyActive;
-        static bool            sm_MotionEmergency;
-        static bool            sm_isMotionAlive;
-        static bool            sm_isControllerAlive;
-        static bool            sm_isEStopActive;
-        static bool            sm_isButtonOneDown;
-        static bool            sm_isButtonTwoDown;
+        // scoreboard lock
+        static Poco::RWLock sm_rwLock;
+
+        // message broadcast
         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;
+
+        // overall emergency state
+        static bool sm_isEmergencyActive;
+
+        // odr-display
+        static bool sm_DisplayEmergency;
+        static bool sm_isDisplayAlive;
+
+        // odr-gps
+        static bool            sm_GpsEmergency;
+        static bool            sm_isGpsAlive;
+        static bool            sm_gpsHasFix;
+        static std::list< double > sm_gpsLatitudes;
+        static double                sm_gpsLatitudeAvg;
+        static std::list< double > sm_gpsLongitudes;
+        static double                sm_gpsLongitudeAvg;
+
+        // odr-motion
+        static bool sm_MotionEmergency;
+        static bool sm_isMotionAlive;
+
+        // odr-sonar-front
+        static bool    sm_SonarFrontEmergency;
+        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;
+
+        // imu
         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;
+
+        // program
+        static bool    sm_hasActiveProgram;
+        static uint8_t sm_activeProgram;
+
+        // navigation
+        static int8_t  sm_navMaximumSpeed;
+        static double  sm_navTargetHeading;
 
     private:
         std::string m_loggerName;
         Poco::Event m_quitEvent;
+        Poco::Timer m_timerLogStatus;
         Poco::Timer m_timerSendManagerUpdate;
-        Poco::Timer m_timerMotionUpdate;
-        Poco::Timer m_timerControllerUpdate;
-        Poco::Timer m_timerSonarFrontUpdate;
-        Poco::Timer m_timerSendImuDataUpdate;
+        Poco::Timer m_timerDisplayTimeout;
+        Poco::Timer m_timerGpsTimeout;
+        Poco::Timer m_timerImuTimeout;
+        Poco::Timer m_timerMotionTimeout;
+        Poco::Timer m_timerSonarFrontTimeout;
 
     private:
+        void logSystemStatus( Poco::Timer& timer );
         void sendManagerUpdate( Poco::Timer& timer );
-        void timeoutMotionUpdate( Poco::Timer& timer );
-        void timeoutControllerUpdate( Poco::Timer& timer );
-        void timeoutSonarFrontUpdate( Poco::Timer& timer );
-        void sendImuDataUpdate( Poco::Timer& timer );
+        void timeoutDisplayNode( Poco::Timer& timer );
+        void timeoutGpsSensorNode( Poco::Timer& timer );
+        void timeoutMotionNode( Poco::Timer& timer );
+        void timeoutSonarFrontNode( Poco::Timer& timer );
+        void timeoutImuNode( Poco::Timer& timer );
         void recvEmergency( uint8_t srcNode );
         void recvAllClear( uint8_t srcNode );
         void recvHeartbeat( uint8_t srcNode );
-        void recvControllerUpdate( CANMessage* msg );
-        void recvButtonUpdate( CANMessage* msg );
         void recvSonarFrontStatus( bool enabled );
         void recvSonarFrontValues( CANMessage* msg );
+        void recvGpsFixInfo( CANMessage* msg );
+        void recvGpsLatitude( CANMessage* msg );
+        void recvGpsLongitude( CANMessage* msg );
+        void recvImuRoll( CANMessage* msg );
+        void recvImuPitch( CANMessage* msg );
+        void recvImuYaw( CANMessage* msg );
+        void recvStartProgram( CANMessage* msg );
 };
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SquareCourseTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,97 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SquareCourseTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "SquareCourseTask.h"
+
+#include <cmath>
+
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const Poco::Timestamp::TimeDiff sRunTimeUntilDirectionChange
+                                                      = Poco::Timestamp::resolution() * 60;
+
+// ----------------------------------------------------------------------------------------
+
+SquareCourseTask::SquareCourseTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_heading( 0.0 ),
+      m_nextSwitchTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+SquareCourseTask::~SquareCourseTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SquareCourseTask::update()
+{
+    if ( m_nextSwitchTime.isElapsed( sRunTimeUntilDirectionChange ) )
+    {
+        m_nextSwitchTime.update();
+
+        m_heading += 90.0;
+        if ( m_heading > 359.0 ) { m_heading = 0.0; }
+
+        log().information(
+            Poco::Logger::format(
+                "SquareCourseTask: new heading $0",
+                Poco::NumberFormatter::format( m_heading ) ) );
+
+        Scoreboard::navSetTargetHeading( m_heading );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool SquareCourseTask::wantsControl()
+{
+    return false; // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SquareCourseTask::takeControl()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SquareCourseTask.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,56 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SquareCourseTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+#define BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class SquareCourseTask : public TaskObject
+{
+    public:
+        SquareCourseTask( const std::string& loggerName );
+        virtual ~SquareCourseTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        double          m_heading;
+        Poco::Timestamp m_nextSwitchTime;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/StartupTask.cpp	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/StartupTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 //
-//  Subsumption task to enact safety measures under certain unsafe conditions.
+//  Subsumption task to hold operations until a specific program is selected.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -33,7 +33,6 @@
 
 #include "MotorsAndServos.h"
 #include "Scoreboard.h"
-#include "Sonar.h"
 
 // ----------------------------------------------------------------------------------------
 
@@ -43,10 +42,7 @@
 
 StartupTask::StartupTask( const std::string& loggerName )
     : TaskObject( loggerName ),
-      m_lastControllerStatus( Scoreboard::isControllerAlive() ),
-      m_lastMessageTime(),
-      m_gotButtonPress( false ),
-      m_gotButtonRelease( false ),
+      m_haveStartedProgram( Scoreboard::isProgramRunning() ),
       m_startRunTime()
 {
 }
@@ -61,56 +57,30 @@
 
 void StartupTask::update()
 {
-    // we need the odr-controller to be running
-    
-    if ( ! Scoreboard::isControllerAlive() )
+    // have an active program yet? if not, wait in this state until we do
+
+    if ( ! Scoreboard::isProgramRunning() )
     {
-        if ( m_lastControllerStatus )
-        {
-            log().information( "StartupTask: controller is not alive" );
-            m_lastControllerStatus = false;
-        }
-
-        return; // nothing else can be done
-    }
-    else if ( ! m_lastControllerStatus )
-    {
-        log().information( "StartupTask: controller is alive" );
-        m_lastControllerStatus = true;
+        m_haveStartedProgram = false;
+        return;
     }
 
-    // check for the button press then release event
+    // program running; this might be the start of that event
 
-    if ( ! m_gotButtonPress )
+    if ( ! m_haveStartedProgram )
     {
-        m_gotButtonPress = Scoreboard::isButtonOneDown();
-        if ( m_gotButtonPress ) log().information( "StartupTask: got button press" );
+        m_haveStartedProgram = true;
+        log().information( "StartupTask: starting program" );
+        m_startRunTime.update();
     }
 
-    if ( m_gotButtonPress && ! m_gotButtonRelease )
-    {
-        m_gotButtonRelease = ! Scoreboard::isButtonOneDown();
-        if ( m_gotButtonRelease ) log().information( "StartupTask: got button release" );
-
-        if ( m_gotButtonRelease )
-        {
-            log().information( "StartupTask: starting run" );
-            m_startRunTime.update();
+    // the program is running, check for timeout
 
-//            Scoreboard::navSetTargetHeading( Scoreboard::imuYaw() );
-        }
-    }
-
-    // if we are running, check for timeout
-
-    if ( m_gotButtonPress && m_gotButtonRelease )
+    if ( m_startRunTime.isElapsed( TimeToRun ) )
     {
-        if ( m_startRunTime.isElapsed( TimeToRun ) )
-        {
-            log().information( "StartupTask: ending run" );
-            m_gotButtonPress   = false;
-            m_gotButtonRelease = false;
-        }
+        Scoreboard::stopActiveProgram();
+        m_haveStartedProgram = false;
+        log().information( "StartupTask: ending run" );
     }
 }
 
@@ -118,14 +88,9 @@
 
 bool StartupTask::wantsControl()
 {
-    if ( ! m_lastControllerStatus )
+    if ( ! m_haveStartedProgram )
     {
-        return true; // take control if the odr-controller is not yet alive
-    }
-
-    if ( ! m_gotButtonPress || ! m_gotButtonRelease )
-    {
-        return true; // take control if the button hasn't been pressed/released yet
+        return true; // take control if no program started yet
     }
 
     return false; // otherwise not for us
@@ -135,18 +100,9 @@
 
 void StartupTask::takeControl()
 {
-    if ( ! m_lastControllerStatus )
-    {
-        return; // nothing can be done until the odr-controller is up
-    }
+    // display a helpful message
 
-    // send instructions for display on the odr-controller
-
-    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
-    {
-        Scoreboard::sendManagerMessage( "Press A" );
-        m_lastMessageTime.update();
-    }
+    Scoreboard::sendManagerMessage( "Waiting" );
 
     // make sure we aren't moving
 
--- a/main/robots/odr/StartupTask.h	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/StartupTask.h	Sun Jul 13 09:15:53 2014 -0700
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 //
-//  Subsumption task to enact safety measures under certain unsafe conditions.
+//  Subsumption task to hold operations until a specific program is selected.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2014 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
@@ -46,10 +46,7 @@
         virtual void takeControl();
 
     private:
-        bool            m_lastControllerStatus;
-        Poco::Timestamp m_lastMessageTime;
-        bool            m_gotButtonPress;
-        bool            m_gotButtonRelease;
+        bool            m_haveStartedProgram;
         Poco::Timestamp m_startRunTime;
 };
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.cpp	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,140 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TrackWaypointsTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate to a list of successive GPS locations (waypoints).
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+//  The algorithm to compute heading (bearing) and distance between GPS coordinates was
+//  found at this website: http://www.movable-type.co.uk/scripts/latlong.html
+//
+// ----------------------------------------------------------------------------------------
+
+#include "TrackWaypointsTask.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const Poco::Timestamp::TimeDiff sRunTimeUntilWaypointCheck
+                                                      = Poco::Timestamp::resolution() * 5;
+
+// ----------------------------------------------------------------------------------------
+
+TrackWaypointsTask::TrackWaypointsTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_waypoints(),
+      m_nextUpdateTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+TrackWaypointsTask::~TrackWaypointsTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::update()
+{
+    if ( m_nextUpdateTime.isElapsed( sRunTimeUntilWaypointCheck ) )
+    {
+        m_nextUpdateTime.update();
+
+        // decide if we have reached the waypoint
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool TrackWaypointsTask::wantsControl()
+{
+    return false; // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::takeControl()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double deg_to_rad( double degrees )
+{
+    return degrees * M_PI / 180.0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double rad_to_deg( double radians )
+{
+    return radians * 180.0 / M_PI;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::refreshWaypointTrack()
+{
+    // compute the desired heading to the waypoint from the current location
+
+    double currLat = Scoreboard::gpsLatitude();
+    double currLon = Scoreboard::gpsLongitude();
+
+    double deltaLonRads  = ::deg_to_rad( m_waypointLongitude - currLon );
+    double currLatRads   = ::deg_to_rad( currLat );
+    double targetLatRads = ::deg_to_rad( m_waypointLatitude );
+
+    double y = ::sin( deltaLonRads ) * ::cos( targetLatRads );
+    double x = ::cos( currLatRads ) * ::sin( targetLatRads ) -
+               ::sin( currLatRads ) * ::cos( targetLatRads ) * ::cos( deltaLonRads );
+
+    m_headingToWaypoint = ::rad_to_deg( ::atan2( y, x ) );
+
+    // compute the distance to the desired waypoint
+
+    static const double R = 6371.0; // km
+
+    double deltaLatRads = ::deg_to_rad( m_waypointLatitude - currLat );
+
+    double a = ::sin( deltaLatRads / 2.0 ) * ::sin( deltaLatRads / 2.0 ) +
+               ::sin( deltaLonRads / 2.0 ) * ::sin( deltaLonRads / 2.0 ) *
+               ::cos( currLatRads ) * ::cos( targetLatRads );
+
+    double c = 2.0 * ::atan2( ::sqrt( a ), ::sqrt( 1.0 - a ) );
+
+    m_distanceToWaypoint = R * c;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.h	Sun Jul 13 09:15:53 2014 -0700
@@ -0,0 +1,76 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TrackWaypointsTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+#define BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+
+#include "TaskObject.h"
+
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+typedef struct
+{
+    double latitude;
+    double longitude;
+   
+} WaypointType;
+
+typedef std::vector< WaypointType > WaypointVectorType;
+
+// ----------------------------------------------------------------------------------------
+
+class TrackWaypointsTask : public TaskObject
+{
+    public:
+        TrackWaypointsTask( const std::string& loggerName );
+        virtual ~TrackWaypointsTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        void refreshWaypointTrack();
+
+    private:
+        WaypointVectorType m_waypoints;
+        Poco::Timestamp    m_nextUpdateTime;
+        double             m_waypointLatitude;
+        double             m_waypointLongitude;
+        double             m_headingToWaypoint;
+        double             m_distanceToWaypoint;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/jamfile	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/jamfile	Sun Jul 13 09:15:53 2014 -0700
@@ -34,12 +34,19 @@
 
 # -----------------------------------------------------------------------------------------
 
-COMMON_SOURCES = 
+ODRAPP_SOURCES = 
     main.cpp
     ODRApp.cpp
-    ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
+    MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
     AvoidBySonarTask.cpp CruisingTask.cpp NavigateTask.cpp SafetyTask.cpp StartupTask.cpp
+    SquareCourseTask.cpp TrackWaypointsTask.cpp
+    packages.common.can.pkg
+    packages.linux.can.pkg
+    ;
+
+IMUAPP_SOURCES =
+    ImuApp.cpp ImuReader.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;
@@ -49,8 +56,11 @@
 
 # -----------------------------------------------------------------------------------------
 
-ubuntu_executable odr_ubuntu : $(COMMON_SOURCES) $(POCO_SHARED) ;
-overo_executable  odr        : $(COMMON_SOURCES) $(POCO_STATIC) ;
+ubuntu_executable odr_ubuntu : $(ODRAPP_SOURCES) $(POCO_SHARED) ;
+overo_executable  odr        : $(ODRAPP_SOURCES) $(POCO_STATIC) ;
+
+ubuntu_executable imu_ubuntu : $(IMUAPP_SOURCES) $(POCO_SHARED) ;
+overo_executable  imu        : $(IMUAPP_SOURCES) $(POCO_STATIC) ;
 
 # -----------------------------------------------------------------------------------------
 
--- a/main/robots/odr/odr_ubuntu.xml	Sat Jul 12 13:20:14 2014 -0700
+++ b/main/robots/odr/odr_ubuntu.xml	Sun Jul 13 09:15:53 2014 -0700
@@ -32,8 +32,5 @@
         <logIncoming>no</logIncoming>
         <logOutgoing>no</logOutgoing>
     </can>
-    <imu>
-        <serialPortPath>/dev/ttyUSB0</serialPortPath>
-    </imu>
 </config>