changeset 135:caee904383e5 main

First commit of the subsumption code.
author Bob Cook <bob@bobcookdev.com>
date Thu, 12 Jan 2012 21:27:19 -0800
parents 98172ca268ae
children 6a258aafc9d7
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/AvoidBySonarTask.h main/robots/odr/CruisingTask.cpp main/robots/odr/CruisingTask.h main/robots/odr/MotorsAndServos.cpp main/robots/odr/MotorsAndServos.h main/robots/odr/ODRApp.cpp main/robots/odr/ODRApp.h main/robots/odr/SafetyTask.cpp main/robots/odr/SafetyTask.h main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h main/robots/odr/Sonar.cpp main/robots/odr/Sonar.h main/robots/odr/TaskObject.cpp main/robots/odr/TaskObject.h main/robots/odr/jamfile main/robots/odr/odr_ubuntu.xml
diffstat 18 files changed, 1000 insertions(+), 65 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,181 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/AvoidBySonarTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Virtual base class for a subsumption task object.
+//
+//  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 "AvoidBySonarTask.h"
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+#include "Sonar.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const unsigned AVOIDANCE_ZONE = 0x0600;
+static const unsigned MAX_TURN_ZONE  = 0x0200;
+static const unsigned TRAPPED_ZONE   = 0x0100;
+
+// ----------------------------------------------------------------------------------------
+
+static int8_t CalculateTurn( unsigned sensorValue )
+{
+    if ( sensorValue >= AVOIDANCE_ZONE )
+    {
+        return 0; // no turn
+    }
+
+    if ( sensorValue <= MAX_TURN_ZONE )
+    {
+        return 12; // max turn
+    }
+
+    // the wheels have a range of -12 to 12, but we are only calculating the positive
+    // angle - the caller will figure out the rest
+
+    const unsigned turnPerSensorInc = ( AVOIDANCE_ZONE - MAX_TURN_ZONE ) / 12;
+    unsigned rawTurn = ( AVOIDANCE_ZONE - sensorValue ) / turnPerSensorInc;
+
+    return static_cast< int8_t >( rawTurn );
+}
+
+// ----------------------------------------------------------------------------------------
+
+AvoidBySonarTask::AvoidBySonarTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_lastMessageTime(),
+      m_frontLeft( 0 ),
+      m_frontCenter( 0 ),
+      m_frontRight( 0 )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+AvoidBySonarTask::~AvoidBySonarTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void AvoidBySonarTask::update()
+{
+    if ( Scoreboard::isSonarFrontAlive() && ! Scoreboard::isSonarFrontEnabled() )
+    {
+        Sonar::setFrontEnabled();
+    }
+
+    m_frontLeft   = Scoreboard::sonarFrontLeft();
+    m_frontCenter = Scoreboard::sonarFrontCenter();
+    m_frontRight  = Scoreboard::sonarFrontRight();
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool AvoidBySonarTask::wantsControl()
+{
+    return ( m_frontLeft < AVOIDANCE_ZONE
+                || m_frontCenter < AVOIDANCE_ZONE
+                || m_frontRight < AVOIDANCE_ZONE );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool AvoidBySonarTask::isTrapped() const
+{
+    return ( m_frontLeft < TRAPPED_ZONE
+                && m_frontCenter < TRAPPED_ZONE
+                && m_frontRight < TRAPPED_ZONE );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void AvoidBySonarTask::takeControl()
+{
+    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
+    {
+        Scoreboard::sendManagerMessage( "Avoiding" );
+        m_lastMessageTime.update();
+    }
+
+    if ( isTrapped() )
+    {
+        MotorsAndServos::motorSpeeds( 0, 0 );
+        return; // stuck!
+    }
+
+    if ( m_frontCenter < AVOIDANCE_ZONE )
+    {
+        int8_t turn = CalculateTurn( m_frontCenter );
+
+        if ( m_frontLeft < ( m_frontRight + 0x0010 ) ) // dead-zone of 0x0010
+        {
+            MotorsAndServos::servoPositions( -turn, turn );
+        }
+        else
+        {
+            MotorsAndServos::servoPositions( turn, -turn );
+        }
+    }
+    else if ( m_frontLeft < AVOIDANCE_ZONE && m_frontRight < AVOIDANCE_ZONE )
+    {
+        if ( m_frontLeft < ( m_frontRight + TRAPPED_ZONE ) )
+        {
+            int8_t turn = CalculateTurn( m_frontLeft + TRAPPED_ZONE );
+            MotorsAndServos::servoPositions( -turn, turn );
+        }
+        else if ( m_frontRight < ( m_frontLeft + TRAPPED_ZONE ) )
+        {
+            int8_t turn = CalculateTurn( m_frontRight + TRAPPED_ZONE );
+            MotorsAndServos::servoPositions( turn, -turn );
+        }
+        else
+        {
+            MotorsAndServos::servoPositions( 0, 0 ); // straight
+        }
+    }
+    else if ( m_frontLeft < AVOIDANCE_ZONE )
+    {
+        int8_t turn = CalculateTurn( m_frontLeft );
+        MotorsAndServos::servoPositions( -turn, turn );
+    }
+    else if ( m_frontRight < AVOIDANCE_ZONE )
+    {
+        int8_t turn = CalculateTurn( m_frontRight );
+        MotorsAndServos::servoPositions( turn, -turn );
+    }
+    else
+    {
+        MotorsAndServos::servoPositions( 0, 0 ); // unexpected...
+    }
+
+    MotorsAndServos::motorSpeeds( 32, 32 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/AvoidBySonarTask.h	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,61 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/AvoidBySonar.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Virtual base class for a subsumption task object.
+//
+//  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_ODR_AVOIDBYSONARTASK_H
+#define BCDRL_ROBOTS_ODR_AVOIDBYSONARTASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class AvoidBySonarTask : public TaskObject
+{
+    public:
+        AvoidBySonarTask( const std::string& loggerName );
+        virtual ~AvoidBySonarTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        bool isTrapped() const;
+
+    private:
+        Poco::Timestamp m_lastMessageTime;
+        unsigned        m_frontLeft;
+        unsigned        m_frontCenter;
+        unsigned        m_frontRight;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_AVOIDBYSONARTASK_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/CruisingTask.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,80 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/CruisingTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to drive straight forward. Lowest priority task.
+//
+//  Copyright (c) 2011 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "CruisingTask.h"
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+CruisingTask::CruisingTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_lastMessageTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+CruisingTask::~CruisingTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CruisingTask::update()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool CruisingTask::wantsControl()
+{
+    return true; // always wants control, should be lowest priority
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CruisingTask::takeControl()
+{
+    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
+    {
+        Scoreboard::sendManagerMessage( "Cruising" );
+        m_lastMessageTime.update();
+    }
+
+    MotorsAndServos::servoPositions( 0, 0 );
+    MotorsAndServos::motorSpeeds( 32, 32 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/CruisingTask.h	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,55 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/CruisingTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to drive straight forward. Lowest priority task.
+//
+//  Copyright (c) 2011 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_CRUISINGTASK_H
+#define BCDRL_ROBOTS_ODR_CRUISINGTASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class CruisingTask : public TaskObject
+{
+    public:
+        CruisingTask( const std::string& loggerName );
+        virtual ~CruisingTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        Poco::Timestamp m_lastMessageTime;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_CRUISINGTASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/MotorsAndServos.cpp	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/MotorsAndServos.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -31,9 +31,6 @@
 
 #include "MotorsAndServos.h"
 
-#include <Poco/AutoPtr.h>
-#include <Poco/Logger.h>
-
 #include "packages/common/can/can_helpers.h"
 #include "packages/common/can/can_messages.h"
 #include "packages/common/can/can_nodes.h"
@@ -43,20 +40,26 @@
 
 // ----------------------------------------------------------------------------------------
 
-MotorsAndServos::MotorsAndServos()
-{
-}
-
-// ----------------------------------------------------------------------------------------
-
-MotorsAndServos::~MotorsAndServos()
-{
-}
+bool   MotorsAndServos::sm_neverSentServoCmd = true;
+int8_t MotorsAndServos::sm_lastServoFront = 0;
+int8_t MotorsAndServos::sm_lastServoRear  = 0;
+bool   MotorsAndServos::sm_neverSentMotorCmd = true;
+int8_t MotorsAndServos::sm_lastMotorFront = 0;
+int8_t MotorsAndServos::sm_lastMotorRear  = 0;
 
 // ----------------------------------------------------------------------------------------
 
 void MotorsAndServos::servoPositions( int8_t front, int8_t rear )
 {
+    if ( front == MotorsAndServos::sm_lastServoFront
+         && rear == MotorsAndServos::sm_lastServoRear )
+    {
+        if ( ! MotorsAndServos::sm_neverSentServoCmd )
+        {
+            return;
+        }
+    }
+
     can_data_servo_position servodata;
     servodata.servo_front = front;
     servodata.servo_rear  = rear;
@@ -69,12 +72,25 @@
             new CANMessage( servomsgid,
                             reinterpret_cast< uint8_t* >( &servodata ),
                             sizeof(servodata) ) );
+
+    MotorsAndServos::sm_neverSentServoCmd = false;
+    MotorsAndServos::sm_lastServoFront = front;
+    MotorsAndServos::sm_lastServoRear  = rear;
 }
 
 // ----------------------------------------------------------------------------------------
 
 void MotorsAndServos::motorSpeeds( int8_t front, int8_t rear )
 {
+    if ( front == MotorsAndServos::sm_lastMotorFront
+         && rear == MotorsAndServos::sm_lastMotorRear )
+    {
+        if ( ! MotorsAndServos::sm_neverSentMotorCmd )
+        {
+            return;
+        }
+    }
+
     can_data_motor_speed motordata;
     motordata.motor_front = front;
     motordata.motor_rear  = rear;
@@ -87,6 +103,10 @@
             new CANMessage( motormsgid,
                             reinterpret_cast< uint8_t* >( &motordata ),
                             sizeof(motordata) ) );
+
+    MotorsAndServos::sm_neverSentMotorCmd = false;
+    MotorsAndServos::sm_lastMotorFront = front;
+    MotorsAndServos::sm_lastMotorRear  = rear;
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/MotorsAndServos.h	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/MotorsAndServos.h	Thu Jan 12 21:27:19 2012 -0800
@@ -39,11 +39,16 @@
 class MotorsAndServos
 {
     public:
-        MotorsAndServos();
-        virtual ~MotorsAndServos();
+        static void servoPositions( int8_t front, int8_t rear );
+        static void motorSpeeds( int8_t front, int8_t rear );
 
-        void servoPositions( int8_t front, int8_t rear );
-        void motorSpeeds( int8_t front, int8_t rear );
+    private:
+        static bool   sm_neverSentServoCmd;
+        static int8_t sm_lastServoFront;
+        static int8_t sm_lastServoRear;
+        static bool   sm_neverSentMotorCmd;
+        static int8_t sm_lastMotorFront;
+        static int8_t sm_lastMotorRear;
 
 };
 
--- a/main/robots/odr/ODRApp.cpp	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/ODRApp.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -35,7 +35,6 @@
 #include <string>
 #include <vector>
 
-#include <Poco/AutoPtr.h>
 #include <Poco/Logger.h>
 #include <Poco/Thread.h>
 #include <Poco/Util/Application.h>
@@ -46,8 +45,14 @@
 
 #include "ODRApp.h"
 
+#include "AvoidBySonarTask.h"
+#include "CruisingTask.h"
 #include "MotorsAndServos.h"
+#include "SafetyTask.h"
 #include "Scoreboard.h"
+#include "Sonar.h"
+#include "StartupTask.h"
+#include "TaskObject.h"
 
 #include "packages/common/can/can_helpers.h"
 
@@ -58,6 +63,8 @@
 
 ODRApp::ODRApp()
     : m_helpRequested( false ),
+      m_runTaskTimer(),
+      m_tasks(),
       m_runloopActive( false ) 
 {
 }
@@ -136,7 +143,28 @@
     Scoreboard   scoreboard( logger().name() );
     scoreboardThread.start( scoreboard );
 
+    m_tasks.clear();
+
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new StartupTask( logger().name() ) ) );
+
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new SafetyTask( logger().name() ) ) );
+
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
+
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new CruisingTask( logger().name() ) ) );
+
+    m_runTaskTimer.setPeriodicInterval( 50 ); // every 50ms, 20 Hz
+    Poco::TimerCallback< ODRApp > cb( *this, &ODRApp::runTasks );
+    m_runTaskTimer.start( cb );
+
     m_runloopActive = true;
+    while ( m_runloopActive ) Poco::Thread::sleep( 5000 );
+
+#if 0
 
     while ( m_runloopActive )
     {
@@ -161,9 +189,10 @@
             logger().error( "Failed in runloop(), unknown exception" );
         }
     }
+#endif
 
     Scoreboard::sendManagerMessage( "Goodbye" );
-    Poco::Thread::sleep( 2000 );
+    Poco::Thread::sleep( 5000 );
 
     scoreboard.timeToQuit();
     scoreboardThread.join();
@@ -214,25 +243,85 @@
         }
 
         Scoreboard::sendManagerMessage( "Running" );
-        
-        Scoreboard::setSonarFrontEnabled();
+        Sonar::setFrontEnabled();
 
         if ( Scoreboard::isEStopActive() )
         {
             logger().information( "estop is active" );
         }
 
-        MotorsAndServos().motorSpeeds( 32, 32 );
-        MotorsAndServos().servoPositions( 12, -12 );
+        MotorsAndServos::motorSpeeds( 32, 32 );
+        MotorsAndServos::servoPositions( 12, -12 );
         Poco::Thread::sleep( 20000 );
-        MotorsAndServos().motorSpeeds( 0, 0 );
-        MotorsAndServos().servoPositions( 0, 0 );
+        MotorsAndServos::motorSpeeds( 0, 0 );
+        MotorsAndServos::servoPositions( 0, 0 );
 
         Scoreboard::sendManagerMessage( "Done" );
-        Scoreboard::setSonarFrontDisabled();
+        Sonar::setFrontDisabled();
         Poco::Thread::sleep( 5000 );
     }
 }
 
 // ----------------------------------------------------------------------------------------
 
+void ODRApp::runTasks( Poco::Timer& timer )
+{
+    // refresh all the tasks
+    
+    for ( TaskVectorType::iterator itr = m_tasks.begin(); itr != m_tasks.end(); ++itr )
+    {
+        try
+        {
+            (*itr)->update();
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runTasks(), Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runTasks(), std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            logger().error( "Failed in runTasks(), unknown exception" );
+        }
+    }
+
+    // give control to the highest priority task
+    
+    for ( TaskVectorType::iterator itr = m_tasks.begin(); itr != m_tasks.end(); ++itr )
+    {
+        try
+        {
+            if ( (*itr)->wantsControl() )
+            {
+                (*itr)->takeControl();
+                return;
+            }
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runTasks(), Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            logger().error(
+                    Poco::Logger::format(
+                        "Failed in runTasks(), std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            logger().error( "Failed in runTasks(), unknown exception" );
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ODRApp.h	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/ODRApp.h	Thu Jan 12 21:27:19 2012 -0800
@@ -32,8 +32,12 @@
 #ifndef BCDRL_ROBOTS_ODR_ODRAPP_H
 #define BCDRL_ROBOTS_ODR_ODRAPP_H
 
+#include <Poco/SharedPtr.h>
+#include <Poco/Timer.h>
 #include <Poco/Util/ServerApplication.h>
 
+#include "TaskObject.h"
+
 namespace Poco
 {
     namespace Util
@@ -54,18 +58,24 @@
         virtual ~ODRApp();
 
     protected:
-        void initialize(Poco::Util::Application& self);
+        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);
+        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:
+        typedef std::vector< Poco::SharedPtr< TaskObject > > TaskVectorType;
 
     private:
         void runloop();
+        void runTasks( Poco::Timer& timer );
 
     private:
-        bool m_helpRequested;
-        bool m_runloopActive;
+        bool           m_helpRequested;
+        Poco::Timer    m_runTaskTimer;
+        TaskVectorType m_tasks;
+        bool        m_runloopActive;
 };
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SafetyTask.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,139 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SafetyTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to enact safety measures under certain unsafe conditions.
+//
+//  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 "SafetyTask.h"
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+SafetyTask::SafetyTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_lastMessageTime(),
+      m_lastEstopStatus( Scoreboard::isEStopActive() ),
+      m_lastControllerStatus( Scoreboard::isControllerAlive() ),
+      m_lastSonarFrontStatus( Scoreboard::isSonarFrontAlive() )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+SafetyTask::~SafetyTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SafetyTask::update()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool SafetyTask::wantsControl()
+{
+    // odr-controller
+    
+    if ( ! Scoreboard::isControllerAlive() )
+    {
+        if ( m_lastControllerStatus )
+        {
+            log().information( "SafetyTask: controller is not alive" );
+            m_lastControllerStatus = false;
+        }
+
+        return true;
+    }
+    else if ( ! m_lastControllerStatus )
+    {
+        log().information( "SafetyTask: controller is alive" );
+        m_lastControllerStatus = true;
+    }
+
+    // odr-sonar-front
+
+    if ( ! Scoreboard::isSonarFrontAlive() )
+    {
+        if ( m_lastSonarFrontStatus )
+        {
+            log().information( "SafetyTask: front sonar is not alive" );
+            m_lastSonarFrontStatus = false;
+        }
+
+        return true;
+    }
+    else if ( ! m_lastSonarFrontStatus )
+    {
+        log().information( "SafetyTask: front sonar is alive" );
+        m_lastSonarFrontStatus = true;
+    }
+
+    // emergency stop
+    
+    if ( Scoreboard::isEStopActive() )
+    {
+        if ( ! m_lastEstopStatus )
+        {
+            log().information( "SafetyTask: estop is active" );
+            m_lastEstopStatus = true;
+        }
+
+//        return true;
+    }
+    else if ( m_lastEstopStatus )
+    {
+        log().information( "SafetyTask: estop is not active" );
+        m_lastEstopStatus = false;
+    }
+
+    // otherwise no issues
+
+    return false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SafetyTask::takeControl()
+{
+    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
+    {
+        Scoreboard::sendManagerMessage( "Unsafe!" );
+        m_lastMessageTime.update();
+    }
+
+    MotorsAndServos::servoPositions( 0, 0 );
+    MotorsAndServos::motorSpeeds( 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SafetyTask.h	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,58 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SafetyTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to enact safety measures under certain unsafe conditions.
+//
+//  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_ODR_SAFETYTASK_H
+#define BCDRL_ROBOTS_ODR_SAFETYTASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class SafetyTask : public TaskObject
+{
+    public:
+        SafetyTask( const std::string& loggerName );
+        virtual ~SafetyTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        Poco::Timestamp m_lastMessageTime;
+        bool            m_lastEstopStatus;
+        bool            m_lastControllerStatus;
+        bool            m_lastSonarFrontStatus;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_SAFETYTASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/Scoreboard.cpp	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/Scoreboard.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -126,28 +126,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-void Scoreboard::setSonarFrontEnabled()
-{
-    uint32_t msgid = can_build_message_id( can_node_odr_manager,
-                                           can_node_odr_sonar_front,
-                                           can_dataid_sonar_front_enable );
-
-    CANMessage::QueueToSend( new CANMessage( msgid ) );
-}
-
-// ----------------------------------------------------------------------------------------
-
-void Scoreboard::setSonarFrontDisabled()
-{
-    uint32_t msgid = can_build_message_id( can_node_odr_manager,
-                                           can_node_odr_sonar_front,
-                                           can_dataid_sonar_front_disable );
-
-    CANMessage::QueueToSend( new CANMessage( msgid ) );
-}
-
-// ----------------------------------------------------------------------------------------
-
 uint16_t Scoreboard::sonarFrontLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
@@ -180,7 +158,7 @@
       m_timerControllerUpdate(),
       m_timerSonarFrontUpdate()
 {
-    m_timerSendManagerUpdate.setPeriodicInterval( 5000 );
+    m_timerSendManagerUpdate.setPeriodicInterval( 2500 );
     m_timerSendManagerUpdate.start(
             Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
@@ -215,8 +193,7 @@
     uint32_t updatemsgid = can_build_message_id(
                     can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
-    CANMessage::QueueToSend(
-            new CANMessage( updatemsgid, 0, 0 ) );
+    CANMessage::QueueToSend( new CANMessage( updatemsgid ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -245,10 +222,6 @@
     {
         case can_node_odr_controller:
             break;
-
-        case can_node_odr_sonar_front:
-            sm_isSonarFrontAlive = true;
-            break;
     }
 }
 
@@ -286,12 +259,28 @@
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
 
+    m_timerSonarFrontUpdate.restart();
     sm_isSonarFrontAlive = true;
+
     sm_isSonarFrontEnabled = enabled;
 }
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvSonarFrontValues( CANMessage* msg )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    can_data_sonar_front info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    sm_sonarFrontLeft   = info.left;
+    sm_sonarFrontCenter = info.center;
+    sm_sonarFrontRight  = info.right;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -345,6 +334,10 @@
                     case can_dataid_sonar_front_state_disabled:
                         recvSonarFrontStatus( false /* not enabled */ );
                         break;
+
+                    case can_dataid_sonar_front:
+                        recvSonarFrontValues( msg );
+                        break;
                 }
             }
         }
--- a/main/robots/odr/Scoreboard.h	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/Scoreboard.h	Thu Jan 12 21:27:19 2012 -0800
@@ -56,8 +56,6 @@
         // odr-sonar-front
         static bool     isSonarFrontAlive();
         static bool     isSonarFrontEnabled();
-        static void     setSonarFrontEnabled();
-        static void     setSonarFrontDisabled();
         static uint16_t sonarFrontLeft();
         static uint16_t sonarFrontCenter();
         static uint16_t sonarFrontRight();
@@ -95,6 +93,7 @@
         void recvControllerUpdate( CANMessage* msg );
         void recvButtonUpdate( CANMessage* msg );
         void recvSonarFrontStatus( bool enabled );
+        void recvSonarFrontValues( CANMessage* msg );
 };
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/Sonar.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,63 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/Sonar.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Sonar Controller object for the OutDoor Robot
+//
+//  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 "Sonar.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"
+
+// ----------------------------------------------------------------------------------------
+
+void Sonar::setFrontEnabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_enable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Sonar::setFrontDisabled()
+{
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_sonar_front,
+                                           can_dataid_sonar_front_disable );
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/Sonar.h	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,54 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/Sonar.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Sonar Controller object for the OutDoor Robot
+//
+//  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_ODR_SONAR_H
+#define BCDRL_ROBOTS_ODR_SONAR_H
+
+#include <stdint.h>
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+#include <Poco/Timer.h>
+
+// ----------------------------------------------------------------------------------------
+
+class Sonar
+{
+    public:
+        static void setFrontEnabled();
+        static void setFrontDisabled();
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_SONAR_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TaskObject.cpp	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,59 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TaskObject.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Virtual base class for a subsumption task object.
+//
+//  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 "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+const Poco::Timestamp::TimeDiff TaskObject::TwoSeconds = Poco::Timestamp::resolution() * 2;
+
+// ----------------------------------------------------------------------------------------
+
+TaskObject::TaskObject( const std::string& loggerName )
+    : m_logger( Poco::Logger::get( loggerName ) )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+TaskObject::~TaskObject()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+Poco::Logger& TaskObject::log()
+{
+    return m_logger;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TaskObject.h	Thu Jan 12 21:27:19 2012 -0800
@@ -0,0 +1,67 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TaskObject.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Virtual base class for a subsumption task object.
+//
+//  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_ODR_TASKOBJECT_H
+#define BCDRL_ROBOTS_ODR_TASKOBJECT_H
+
+#include <vector>
+
+#include <Poco/Logger.h>
+#include <Poco/Timestamp.h>
+
+// ----------------------------------------------------------------------------------------
+
+class TaskObject
+{
+    public:
+        typedef std::vector< TaskObject* >  VectorType;
+
+    public:
+        static const Poco::Timestamp::TimeDiff TwoSeconds;
+
+    public:
+        TaskObject( const std::string& loggerName );
+        virtual ~TaskObject();
+        virtual void update() = 0;
+        virtual bool wantsControl() = 0;
+        virtual void takeControl() = 0;
+
+    protected:
+        Poco::Logger& log();
+
+    private:
+        Poco::Logger& m_logger;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_TASKOBJECT_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/jamfile	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/jamfile	Thu Jan 12 21:27:19 2012 -0800
@@ -36,7 +36,9 @@
 
 COMMON_SOURCES = 
     main.cpp
-    ODRApp.cpp MotorsAndServos.cpp Scoreboard.cpp
+    ODRApp.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
+    TaskObject.cpp
+    AvoidBySonarTask.cpp CruisingTask.cpp SafetyTask.cpp StartupTask.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;
--- a/main/robots/odr/odr_ubuntu.xml	Tue Dec 27 12:38:44 2011 -0800
+++ b/main/robots/odr/odr_ubuntu.xml	Thu Jan 12 21:27:19 2012 -0800
@@ -29,8 +29,8 @@
     </application>
     <can>
         <interfaceName>can0</interfaceName>
-        <logIncoming>yes</logIncoming>
-        <logOutgoing>yes</logOutgoing>
+        <logIncoming>no</logIncoming>
+        <logOutgoing>no</logOutgoing>
     </can>
 </config>