changeset 197:ff6cfa2ea1ee main

Pull from upstream.
author Bob Cook <bob@bobcookdev.com>
date Wed, 21 Aug 2013 21:02:30 -0700
parents aa2665165208 (current diff) 153436c2a354 (diff)
children 75c3c9c8868f
files
diffstat 15 files changed, 544 insertions(+), 21 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/cantool/CanToolApp.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/cantool/CanToolApp.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -69,8 +69,10 @@
     loadConfiguration();
     Poco::Util::Application::initialize( self );
 
+    m_commandDispatchTable[ "estop" ] = CommandEStop;
     m_commandDispatchTable[ "listen" ] = CommandListen;
-    m_commandDispatchTable[ "motors" ] = CommandMotors;
+    m_commandDispatchTable[ "speed" ] = CommandSpeed;
+    m_commandDispatchTable[ "steer" ] = CommandSteer;
     m_commandDispatchTable[ "sonarfront" ] = CommandSonarFront;
 }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdEStop.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -0,0 +1,78 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdEStop.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "estop" implementation.
+//
+//  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
+//  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 "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"
+
+// ----------------------------------------------------------------------------------------
+
+void CommandEStop( const std::vector< std::string >& args )
+{
+    if ( args.size() < 2 )
+    {
+        std::cerr << "error: estop <on|clear>" << std::endl;
+        return;
+    }
+
+    uint32_t msgid;
+
+    if ( args[ 1 ] == "on" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_broadcast,
+                                      can_dataid_emergency );
+    }
+    else if ( args[ 1 ] == "clear" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_broadcast,
+                                      can_dataid_all_clear );
+    }
+    else
+    {
+        std::cerr << "error: estop <on|clear>" << std::endl;
+        return;
+    }
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+    std::cout << "sent " << args[ 1 ] << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/CmdListen.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/cantool/CmdListen.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -63,9 +63,15 @@
         case can_node_odr_manager:
             return "odr-manager";
 
+        case can_node_odr_controller:
+            return "odr-controller";
+
         case can_node_odr_motion:
             return "odr-motion";
 
+        case can_node_odr_sonar_front:
+            return "odr-sonar-front";
+
         default:
             return "(unknown node)";
     }
@@ -119,6 +125,14 @@
 
         switch ( dataId )
         {
+            case can_dataid_emergency:
+                std::cout << "emergency from " << NodeName( srcNode ) << std::endl;
+                break;
+
+            case can_dataid_all_clear:
+                std::cout << "all-clear from " << NodeName( srcNode ) << std::endl;
+                break;
+
             case can_dataid_heartbeat:
                 std::cout << "heartbeat from " << NodeName( srcNode ) << std::endl;
                 break;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdSteer.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -0,0 +1,101 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdSteer.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "steer" implementation.
+//
+//  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 <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: steer <front_angle> <rear_angle>" << std::endl;
+    std::cerr << "       where \"angle\" is specified as [-12,12]" << std::endl;
+    std::cerr << "       (enter negative values as: \"steer -- -10 -10\"" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CommandSteer( const std::vector< std::string >& args )
+{
+    if ( args.size() < 3 )
+    {
+        PrintHelp();
+        return;
+    }
+
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_motion,
+                                           can_dataid_wheel_position );
+    can_data_wheel_position msg;
+
+    try
+    {
+        int front = Poco::NumberParser::parse( args[ 1 ] );
+        int rear  = Poco::NumberParser::parse( args[ 2 ] );
+
+        if ( front < -12 || front > 12 || rear < -12 || rear > 12 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.angle_front = front;
+        msg.angle_rear  = rear;
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent "
+        << Poco::NumberFormatter::format( msg.angle_front )
+        << "," 
+        << Poco::NumberFormatter::format( msg.angle_rear ) 
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/Commands.h	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/cantool/Commands.h	Wed Aug 21 21:02:30 2013 -0700
@@ -34,9 +34,12 @@
 
 // ----------------------------------------------------------------------------------------
 
+void CommandEStop( const std::vector< std::string >& args );
+
 void CommandListen( const std::vector< std::string >& args );
 
-void CommandMotors( const std::vector< std::string >& args );
+void CommandSpeed( const std::vector< std::string >& args );
+void CommandSteer( const std::vector< std::string >& args );
 
 void CommandSonarFront( const std::vector< std::string >& args );
 
--- a/main/robots/cantool/jamfile	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/cantool/jamfile	Wed Aug 21 21:02:30 2013 -0700
@@ -37,7 +37,7 @@
 COMMON_SOURCES = 
     main.cpp
     CanToolApp.cpp
-    CmdListen.cpp CmdMotors.cpp CmdSonarFront.cpp
+    CmdEStop.cpp CmdListen.cpp CmdSpeed.cpp CmdSonarFront.cpp CmdSteer.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;
--- a/main/robots/odr/AvoidBySonarTask.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -240,7 +240,7 @@
         MotorsAndServos::servoPositions( 0, 0 ); // unexpected...
     }
 
-    MotorsAndServos::motorSpeeds( 32, 32 );
+    MotorsAndServos::motorSpeeds( 15, 15 );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/CruisingTask.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/CruisingTask.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -73,7 +73,7 @@
     }
 
     MotorsAndServos::servoPositions( 0, 0 );
-    MotorsAndServos::motorSpeeds( 32, 32 );
+    MotorsAndServos::motorSpeeds( 15, 15 );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/MotorsAndServos.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/MotorsAndServos.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -60,13 +60,13 @@
         }
     }
 
-    can_data_servo_position servodata;
-    servodata.servo_front = front;
-    servodata.servo_rear  = rear;
+    can_data_wheel_position servodata;
+    servodata.angle_front = front;
+    servodata.angle_rear  = rear;
 
     uint32_t servomsgid
         = can_build_message_id(
-                can_node_odr_manager, can_node_odr_controller, can_dataid_servo_position );
+                can_node_odr_manager, can_node_odr_motion, can_dataid_wheel_position );
 
     CANMessage::QueueToSend(
             new CANMessage( servomsgid,
@@ -91,13 +91,13 @@
         }
     }
 
-    can_data_motor_speed motordata;
-    motordata.motor_front = front;
-    motordata.motor_rear  = rear;
+    can_data_wheel_speed motordata;
+    motordata.rpm_front = front;
+    motordata.rpm_rear  = rear;
 
     uint32_t motormsgid
         = can_build_message_id(
-                can_node_odr_manager, can_node_odr_controller, can_dataid_motor_speed );
+                can_node_odr_manager, can_node_odr_motion, can_dataid_wheel_speed );
 
     CANMessage::QueueToSend(
             new CANMessage( motormsgid,
--- a/main/robots/odr/NavigateTask.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 //
-//  Subsumption task to drive straight forward. Lowest priority task.
+//  Subsumption task to drive forward in a particular heading.
 //
-//  Copyright (c) 2011 Bob Cook
+//  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
@@ -33,6 +33,9 @@
 
 #include <cmath>
 
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
 #include "MotorsAndServos.h"
 #include "Scoreboard.h"
 
@@ -122,8 +125,15 @@
         servoPosition = 12;
     }
 
+    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 ) ) );
+
     MotorsAndServos::servoPositions( -servoPosition, servoPosition );
-    MotorsAndServos::motorSpeeds( 32, 32 );
+    MotorsAndServos::motorSpeeds( 15, 15 );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/ODRApp.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/ODRApp.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -5,9 +5,9 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 //
-//  Application object implementation for demonstrating the CANSocket class.    
+//  Application object implementation for the outdoor robot project.
 //
-//  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
@@ -203,7 +203,7 @@
 void ODRApp::runTasks( Poco::Timer& timer )
 {
     // refresh all the tasks
-    
+   
     for ( TaskVectorType::iterator itr = m_tasks.begin(); itr != m_tasks.end(); ++itr )
     {
         try
@@ -257,6 +257,8 @@
             logger().error( "Failed in runTasks(), unknown exception" );
         }
     }
+
+    logger().error( "No task accepted control" );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/Scoreboard.cpp	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -47,6 +47,9 @@
 // ----------------------------------------------------------------------------------------
 
 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;
@@ -64,7 +67,7 @@
 double          Scoreboard::sm_imuRoll;
 double          Scoreboard::sm_imuPitch;
 double          Scoreboard::sm_imuYaw;
-double          Scoreboard::sm_navTargetHeading;
+double          Scoreboard::sm_navTargetHeading = 100.0;
 
 // ----------------------------------------------------------------------------------------
 
@@ -260,6 +263,7 @@
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
       m_timerSendManagerUpdate(),
+      m_timerMotionUpdate(),
       m_timerControllerUpdate(),
       m_timerSonarFrontUpdate(),
       m_timerSendImuDataUpdate()
@@ -268,6 +272,11 @@
     m_timerSendManagerUpdate.start(
             Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
+    m_timerMotionUpdate.setPeriodicInterval( 5000 );
+    m_timerMotionUpdate.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutMotionUpdate ) );
+
     m_timerControllerUpdate.setPeriodicInterval( 5000 );
     m_timerControllerUpdate.start(
             Poco::TimerCallback< Scoreboard >(
@@ -300,14 +309,37 @@
 
 void Scoreboard::sendManagerUpdate( Poco::Timer& timer )
 {
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    // send the manager status message
+
     uint32_t updatemsgid = can_build_message_id(
                     can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );
 
     CANMessage::QueueToSend( new CANMessage( updatemsgid ) );
 
+    // lock the scoreboard data for read-only access
+
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
 
-    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+    // update the emergency / all-clear status
+
+    if ( sm_isEmergencyActive )
+    {
+        uint32_t msgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_emergency );
+
+        CANMessage::QueueToSend( new CANMessage( msgid ) );
+    }
+    else
+    {
+        uint32_t msgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_all_clear );
+
+        CANMessage::QueueToSend( new CANMessage( msgid ) );
+    }
+
+    // monitor the length of pending CAN messages, log when it looks too long
 
     unsigned send_q_len = CANMessage::SendQueueSize();
     unsigned recv_q_len = CANMessage::ReceiveQueueSize();
@@ -324,6 +356,14 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::timeoutMotionUpdate( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isMotionAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::timeoutControllerUpdate( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -392,6 +432,42 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvEmergency( uint8_t srcNode )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isEmergencyActive = true;
+
+    switch ( srcNode )
+    {
+        case can_node_odr_motion:
+            sm_MotionEmergency = true;
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvAllClear( uint8_t srcNode )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    switch ( srcNode )
+    {
+        case can_node_odr_motion:
+            sm_MotionEmergency = false;
+            break;
+    }
+
+    if ( sm_isEmergencyActive )
+    {
+        sm_isEmergencyActive = sm_MotionEmergency; // | sm_xxxEmergency
+
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::recvHeartbeat( uint8_t srcNode )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -399,6 +475,8 @@
     switch ( srcNode )
     {
         case can_node_odr_controller:
+            m_timerControllerUpdate.restart();
+            sm_isControllerAlive = true;
             break;
     }
 }
@@ -494,6 +572,14 @@
 
                 switch ( dataId )
                 {
+                    case can_dataid_emergency:
+                        recvEmergency( srcNode );
+                        break;
+
+                    case can_dataid_all_clear:
+                        recvAllClear( srcNode );
+                        break;
+
                     case can_dataid_heartbeat:
                         recvHeartbeat( srcNode );
                         break;
--- a/main/robots/odr/Scoreboard.h	Wed Aug 21 21:02:10 2013 -0700
+++ b/main/robots/odr/Scoreboard.h	Wed Aug 21 21:02:30 2013 -0700
@@ -47,6 +47,9 @@
 class Scoreboard : public Poco::Runnable
 {
     public:
+        // odr-motion
+        static bool isMotionAlive();
+
         // odr-controller
         static bool isControllerAlive();
         static bool isEStopActive();
@@ -82,6 +85,9 @@
 
     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;
@@ -105,15 +111,19 @@
         std::string m_loggerName;
         Poco::Event m_quitEvent;
         Poco::Timer m_timerSendManagerUpdate;
+        Poco::Timer m_timerMotionUpdate;
         Poco::Timer m_timerControllerUpdate;
         Poco::Timer m_timerSonarFrontUpdate;
         Poco::Timer m_timerSendImuDataUpdate;
 
     private:
         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 recvEmergency( uint8_t srcNode );
+        void recvAllClear( uint8_t srcNode );
         void recvHeartbeat( uint8_t srcNode );
         void recvControllerUpdate( CANMessage* msg );
         void recvButtonUpdate( CANMessage* msg );
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/StartupTask.cpp	Wed Aug 21 21:02:30 2013 -0700
@@ -0,0 +1,158 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/StartupTask.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 "StartupTask.h"
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+#include "Sonar.h"
+
+// ----------------------------------------------------------------------------------------
+
+const Poco::Timestamp::TimeDiff  TimeToRun = Poco::Timestamp::resolution() * 300;
+
+// ----------------------------------------------------------------------------------------
+
+StartupTask::StartupTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_lastControllerStatus( Scoreboard::isControllerAlive() ),
+      m_lastMessageTime(),
+      m_gotButtonPress( false ),
+      m_gotButtonRelease( false ),
+      m_startRunTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+StartupTask::~StartupTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void StartupTask::update()
+{
+    // we need the odr-controller to be running
+    
+    if ( ! Scoreboard::isControllerAlive() )
+    {
+        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;
+    }
+
+    // check for the button press then release event
+
+    if ( ! m_gotButtonPress )
+    {
+        m_gotButtonPress = Scoreboard::isButtonOneDown();
+        if ( m_gotButtonPress ) log().information( "StartupTask: got button press" );
+    }
+
+    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();
+
+//            Scoreboard::navSetTargetHeading( Scoreboard::imuYaw() );
+        }
+    }
+
+    // if we are running, check for timeout
+
+    if ( m_gotButtonPress && m_gotButtonRelease )
+    {
+        if ( m_startRunTime.isElapsed( TimeToRun ) )
+        {
+            log().information( "StartupTask: ending run" );
+            m_gotButtonPress   = false;
+            m_gotButtonRelease = false;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool StartupTask::wantsControl()
+{
+    if ( ! m_lastControllerStatus )
+    {
+        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 false; // otherwise not for us
+}
+
+// ----------------------------------------------------------------------------------------
+
+void StartupTask::takeControl()
+{
+    if ( ! m_lastControllerStatus )
+    {
+        return; // nothing can be done until the odr-controller is up
+    }
+
+    // send instructions for display on the odr-controller
+
+    if ( m_lastMessageTime.isElapsed( TaskObject::TwoSeconds ) )
+    {
+        Scoreboard::sendManagerMessage( "Press A" );
+        m_lastMessageTime.update();
+    }
+
+    // make sure we aren't moving
+
+    MotorsAndServos::servoPositions( 0, 0 );
+    MotorsAndServos::motorSpeeds( 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/StartupTask.h	Wed Aug 21 21:02:30 2013 -0700
@@ -0,0 +1,59 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/StartupTask.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_STARTUPTASK_H
+#define BCDRL_ROBOTS_ODR_STARTUPTASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class StartupTask : public TaskObject
+{
+    public:
+        StartupTask( const std::string& loggerName );
+        virtual ~StartupTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        bool            m_lastControllerStatus;
+        Poco::Timestamp m_lastMessageTime;
+        bool            m_gotButtonPress;
+        bool            m_gotButtonRelease;
+        Poco::Timestamp m_startRunTime;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_STARTUPTASK_H
+// ----------------------------------------------------------------------------------------
+