changeset 145:fa0eaf92e263 main

Updates for the new four-sensor front sonar module.
author Bob Cook <bob@bobcookdev.com>
date Sat, 07 Jul 2012 16:57:49 -0700
parents 1f998d3a559a
children a01a4a349d58
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/AvoidBySonarTask.h main/robots/odr/MotorsAndServos.cpp main/robots/odr/MotorsAndServos.h main/robots/odr/ODRApp.cpp main/robots/odr/ODRApp.h main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h main/robots/odr/TaskObject.cpp main/robots/odr/TaskObject.h
diffstat 10 files changed, 67 insertions(+), 126 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/AvoidBySonarTask.cpp	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Sat Jul 07 16:57:49 2012 -0700
@@ -37,9 +37,9 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const unsigned AVOIDANCE_ZONE = 0x0600;
-static const unsigned MAX_TURN_ZONE  = 0x0200;
-static const unsigned TRAPPED_ZONE   = 0x0100;
+static const unsigned AVOIDANCE_ZONE = 0x2000;
+static const unsigned MAX_TURN_ZONE  = 0x1000;
+static const unsigned TRAPPED_ZONE   = 0x0600;
 
 // ----------------------------------------------------------------------------------------
 
@@ -70,7 +70,8 @@
     : TaskObject( loggerName ),
       m_lastMessageTime(),
       m_frontLeft( 0 ),
-      m_frontCenter( 0 ),
+      m_frontCenterLeft( 0 ),
+      m_frontCenterRight( 0 ),
       m_frontRight( 0 )
 {
 }
@@ -90,9 +91,10 @@
         Sonar::setFrontEnabled();
     }
 
-    m_frontLeft   = Scoreboard::sonarFrontLeft();
-    m_frontCenter = Scoreboard::sonarFrontCenter();
-    m_frontRight  = Scoreboard::sonarFrontRight();
+    m_frontLeft        = Scoreboard::sonarFrontLeft();
+    m_frontCenterLeft  = Scoreboard::sonarFrontCenterLeft();
+    m_frontCenterRight = Scoreboard::sonarFrontCenterRight();
+    m_frontRight       = Scoreboard::sonarFrontRight();
 }
 
 // ----------------------------------------------------------------------------------------
@@ -100,7 +102,8 @@
 bool AvoidBySonarTask::wantsControl()
 {
     return ( m_frontLeft < AVOIDANCE_ZONE
-                || m_frontCenter < AVOIDANCE_ZONE
+                || m_frontCenterLeft < AVOIDANCE_ZONE
+                || m_frontCenterRight < AVOIDANCE_ZONE
                 || m_frontRight < AVOIDANCE_ZONE );
 }
 
@@ -109,7 +112,8 @@
 bool AvoidBySonarTask::isTrapped() const
 {
     return ( m_frontLeft < TRAPPED_ZONE
-                && m_frontCenter < TRAPPED_ZONE
+                && m_frontCenterLeft < TRAPPED_ZONE
+                && m_frontCenterRight < TRAPPED_ZONE
                 && m_frontRight < TRAPPED_ZONE );
 }
 
@@ -129,29 +133,19 @@
         return; // stuck!
     }
 
-    if ( m_frontCenter < AVOIDANCE_ZONE )
+    uint16_t minLeft = ( m_frontCenterLeft < m_frontLeft ? m_frontCenterLeft : m_frontLeft );
+    uint16_t minRight = ( m_frontCenterRight < m_frontRight ? m_frontCenterRight : m_frontRight );
+
+    if ( minLeft < AVOIDANCE_ZONE && minRight < AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( m_frontCenter );
-
-        if ( m_frontLeft < ( m_frontRight + 0x0010 ) ) // dead-zone of 0x0010
+        if ( minLeft < ( minRight + TRAPPED_ZONE ) )
         {
+            int8_t turn = CalculateTurn( minLeft + TRAPPED_ZONE );
             MotorsAndServos::servoPositions( -turn, turn );
         }
-        else
+        else if ( minRight < ( minLeft + TRAPPED_ZONE ) )
         {
-            MotorsAndServos::servoPositions( turn, -turn );
-        }
-    }
-    else if ( m_frontLeft < AVOIDANCE_ZONE && m_frontRight < AVOIDANCE_ZONE )
-    {
-        if ( m_frontLeft < ( m_frontRight + TRAPPED_ZONE ) )
-        {
-            int8_t turn = CalculateTurn( m_frontLeft + TRAPPED_ZONE );
-            MotorsAndServos::servoPositions( -turn, turn );
-        }
-        else if ( m_frontRight < ( m_frontLeft + TRAPPED_ZONE ) )
-        {
-            int8_t turn = CalculateTurn( m_frontRight + TRAPPED_ZONE );
+            int8_t turn = CalculateTurn( minRight + TRAPPED_ZONE );
             MotorsAndServos::servoPositions( turn, -turn );
         }
         else
@@ -159,14 +153,14 @@
             MotorsAndServos::servoPositions( 0, 0 ); // straight
         }
     }
-    else if ( m_frontLeft < AVOIDANCE_ZONE )
+    else if ( minLeft < AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( m_frontLeft );
+        int8_t turn = CalculateTurn( minLeft );
         MotorsAndServos::servoPositions( -turn, turn );
     }
-    else if ( m_frontRight < AVOIDANCE_ZONE )
+    else if ( minRight < AVOIDANCE_ZONE )
     {
-        int8_t turn = CalculateTurn( m_frontRight );
+        int8_t turn = CalculateTurn( minRight );
         MotorsAndServos::servoPositions( turn, -turn );
     }
     else
--- a/main/robots/odr/AvoidBySonarTask.h	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/AvoidBySonarTask.h	Sat Jul 07 16:57:49 2012 -0700
@@ -51,7 +51,8 @@
     private:
         Poco::Timestamp m_lastMessageTime;
         unsigned        m_frontLeft;
-        unsigned        m_frontCenter;
+        unsigned        m_frontCenterLeft;
+        unsigned        m_frontCenterRight;
         unsigned        m_frontRight;
 };
 
--- a/main/robots/odr/MotorsAndServos.cpp	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/MotorsAndServos.cpp	Sat Jul 07 16:57:49 2012 -0700
@@ -111,3 +111,17 @@
 
 // ----------------------------------------------------------------------------------------
 
+int8_t MotorsAndServos::maxSpeed()
+{
+    return 128;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t MotorsAndServos::minSpeed()
+{
+    return 32;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/MotorsAndServos.h	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/MotorsAndServos.h	Sat Jul 07 16:57:49 2012 -0700
@@ -41,6 +41,8 @@
     public:
         static void servoPositions( int8_t front, int8_t rear );
         static void motorSpeeds( int8_t front, int8_t rear );
+        static int8_t maxSpeed();
+        static int8_t minSpeed();
 
     private:
         static bool   sm_neverSentServoCmd;
--- a/main/robots/odr/ODRApp.cpp	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/ODRApp.cpp	Sat Jul 07 16:57:49 2012 -0700
@@ -164,33 +164,6 @@
     m_runloopActive = true;
     while ( m_runloopActive ) Poco::Thread::sleep( 5000 );
 
-#if 0
-
-    while ( m_runloopActive )
-    {
-        try
-        {
-            runloop();
-        }
-        catch ( const Poco::Exception& ex )
-        {
-            logger().error(
-                    Poco::Logger::format(
-                        "Failed in runloop(), Poco::Exception: $0", ex.displayText() ) );
-        }
-        catch ( const std::exception& ex )
-        {
-            logger().error(
-                    Poco::Logger::format(
-                        "Failed in runloop(), std::exception: $0", ex.what() ) );
-        }
-        catch ( ... )
-        {
-            logger().error( "Failed in runloop(), unknown exception" );
-        }
-    }
-#endif
-
     Scoreboard::sendManagerMessage( "Goodbye" );
     Poco::Thread::sleep( 5000 );
 
@@ -207,63 +180,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-void ODRApp::runloop()
-{
-    for ( ;; )
-    {
-        logger().information( "waiting for the ODR controller" );
-        while ( ! Scoreboard::isControllerAlive() )
-        {
-            Poco::Thread::sleep( 250 );
-        }
-        logger().information( "the ODR controller is alive" );
-
-        Scoreboard::sendManagerMessage( "Press A" );
-
-        bool didTellLogAboutSonar = false;
-        while ( ! Scoreboard::isButtonOneDown() )
-        {
-            Poco::Thread::sleep( 250 );
-
-            if ( Scoreboard::isButtonTwoDown() )
-            {
-                m_runloopActive = false;
-                return;
-            }
-
-            if ( Scoreboard::isSonarFrontAlive() && ! didTellLogAboutSonar )
-            {
-                logger().information( "the front sonar is alive" );
-                didTellLogAboutSonar = true;
-            }
-        }
-        while ( Scoreboard::isButtonOneDown() )
-        {
-            Poco::Thread::sleep( 250 );
-        }
-
-        Scoreboard::sendManagerMessage( "Running" );
-        Sonar::setFrontEnabled();
-
-        if ( Scoreboard::isEStopActive() )
-        {
-            logger().information( "estop is active" );
-        }
-
-        MotorsAndServos::motorSpeeds( 32, 32 );
-        MotorsAndServos::servoPositions( 12, -12 );
-        Poco::Thread::sleep( 20000 );
-        MotorsAndServos::motorSpeeds( 0, 0 );
-        MotorsAndServos::servoPositions( 0, 0 );
-
-        Scoreboard::sendManagerMessage( "Done" );
-        Sonar::setFrontDisabled();
-        Poco::Thread::sleep( 5000 );
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
 void ODRApp::runTasks( Poco::Timer& timer )
 {
     // refresh all the tasks
--- a/main/robots/odr/ODRApp.h	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/ODRApp.h	Sat Jul 07 16:57:49 2012 -0700
@@ -68,7 +68,6 @@
         typedef std::vector< Poco::SharedPtr< TaskObject > > TaskVectorType;
 
     private:
-        void runloop();
         void runTasks( Poco::Timer& timer );
 
     private:
--- a/main/robots/odr/Scoreboard.cpp	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Sat Jul 07 16:57:49 2012 -0700
@@ -53,7 +53,8 @@
 bool         Scoreboard::sm_isSonarFrontAlive;
 bool         Scoreboard::sm_isSonarFrontEnabled;
 uint16_t     Scoreboard::sm_sonarFrontLeft;
-uint16_t     Scoreboard::sm_sonarFrontCenter;
+uint16_t     Scoreboard::sm_sonarFrontCenterLeft;
+uint16_t     Scoreboard::sm_sonarFrontCenterRight;
 uint16_t     Scoreboard::sm_sonarFrontRight;
 
 // ----------------------------------------------------------------------------------------
@@ -134,10 +135,18 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint16_t Scoreboard::sonarFrontCenter()
+uint16_t Scoreboard::sonarFrontCenterLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_sonarFrontCenter;
+    return sm_sonarFrontCenterLeft;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint16_t Scoreboard::sonarFrontCenterRight()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_sonarFrontCenterRight;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -274,9 +283,10 @@
     can_data_sonar_front info;
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
-    sm_sonarFrontLeft   = info.left;
-    sm_sonarFrontCenter = info.center;
-    sm_sonarFrontRight  = info.right;
+    sm_sonarFrontLeft        = info.left;
+    sm_sonarFrontCenterLeft  = info.center_left;
+    sm_sonarFrontCenterRight = info.center_right;
+    sm_sonarFrontRight       = info.right;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -336,6 +346,7 @@
                         break;
 
                     case can_dataid_sonar_front:
+                        recvSonarFrontStatus( true /* enabled */ );
                         recvSonarFrontValues( msg );
                         break;
                 }
--- a/main/robots/odr/Scoreboard.h	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/Scoreboard.h	Sat Jul 07 16:57:49 2012 -0700
@@ -57,7 +57,8 @@
         static bool     isSonarFrontAlive();
         static bool     isSonarFrontEnabled();
         static uint16_t sonarFrontLeft();
-        static uint16_t sonarFrontCenter();
+        static uint16_t sonarFrontCenterLeft();
+        static uint16_t sonarFrontCenterRight();
         static uint16_t sonarFrontRight();
 
     public:
@@ -75,7 +76,8 @@
         static bool         sm_isSonarFrontAlive;
         static bool         sm_isSonarFrontEnabled;
         static uint16_t     sm_sonarFrontLeft;
-        static uint16_t     sm_sonarFrontCenter;
+        static uint16_t     sm_sonarFrontCenterLeft;
+        static uint16_t     sm_sonarFrontCenterRight;
         static uint16_t     sm_sonarFrontRight;
 
     private:
--- a/main/robots/odr/TaskObject.cpp	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/TaskObject.cpp	Sat Jul 07 16:57:49 2012 -0700
@@ -33,7 +33,8 @@
 
 // ----------------------------------------------------------------------------------------
 
-const Poco::Timestamp::TimeDiff TaskObject::TwoSeconds = Poco::Timestamp::resolution() * 2;
+const Poco::Timestamp::TimeDiff TaskObject::TwoSeconds  = Poco::Timestamp::resolution()* 2;
+const Poco::Timestamp::TimeDiff TaskObject::FiveSeconds = Poco::Timestamp::resolution()* 5;
 
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/odr/TaskObject.h	Sat Jul 07 14:52:52 2012 -0700
+++ b/main/robots/odr/TaskObject.h	Sat Jul 07 16:57:49 2012 -0700
@@ -46,6 +46,7 @@
 
     public:
         static const Poco::Timestamp::TimeDiff TwoSeconds;
+        static const Poco::Timestamp::TimeDiff FiveSeconds;
 
     public:
         TaskObject( const std::string& loggerName );