changeset 191:e23198d929e9 main

Continued improvements to implementation.
author Bob Cook <bob@bobcookdev.com>
date Thu, 13 Jun 2013 21:00:19 -0700
parents 3a5b59c73b7f
children 153436c2a354
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/CruisingTask.cpp main/robots/odr/MotorsAndServos.cpp main/robots/odr/NavigateTask.cpp main/robots/odr/ODRApp.cpp main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h
diffstat 7 files changed, 126 insertions(+), 18 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/AvoidBySonarTask.cpp	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Thu Jun 13 21:00:19 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	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/CruisingTask.cpp	Thu Jun 13 21:00:19 2013 -0700
@@ -73,7 +73,7 @@
     }
 
     MotorsAndServos::servoPositions( 0, 0 );
-    MotorsAndServos::motorSpeeds( 32, 32 );
+    MotorsAndServos::motorSpeeds( 15, 15 );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/MotorsAndServos.cpp	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/MotorsAndServos.cpp	Thu Jun 13 21:00:19 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	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Thu Jun 13 21:00:19 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	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/ODRApp.cpp	Thu Jun 13 21:00:19 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	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Thu Jun 13 21:00:19 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	Thu Jun 13 21:00:02 2013 -0700
+++ b/main/robots/odr/Scoreboard.h	Thu Jun 13 21:00:19 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 );