changeset 265:1d246b3260c4 main

Add a new module to interface with the Pixycam. Very incomplete. Other tweaking to the ODR app.
author Bob Cook <bob@bobcookdev.com>
date Fri, 25 Mar 2016 12:11:00 -0700
parents cb0d6aab498d
children 3aeab6193316
files main/robots/odr/NavigateTask.cpp main/robots/odr/ODRApp.cpp main/robots/odr/PixyApp.cpp main/robots/odr/PixyApp.h main/robots/odr/PixyReader.cpp main/robots/odr/PixyReader.h main/robots/odr/SquareCourseTask.cpp main/robots/odr/jamfile
diffstat 8 files changed, 788 insertions(+), 7 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/NavigateTask.cpp	Fri Mar 25 12:09:41 2016 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Fri Mar 25 12:11:00 2016 -0700
@@ -7,7 +7,7 @@
 //
 //  Subsumption task to drive forward in a particular heading.
 //
-//  Copyright (c) 2013 Bob Cook
+//  Copyright (c) 2013-2014 Bob Cook
 //
 //  Permission is hereby granted, free of charge, to any person obtaining a copy
 //  of this software and associated documentation files (the "Software"), to deal
@@ -31,6 +31,8 @@
 
 #include "NavigateTask.h"
 
+#include "packages/common/can/can_messages.h"
+
 #include <cmath>
 
 #include <Poco/Logger.h>
@@ -58,7 +60,7 @@
 
         if ( diffFromZero < diffFrom180 )
         {
-            if ( current < 0.0 ) diffFromZero *= -1.0;
+            if ( current > 0.0 ) diffFromZero *= -1.0;
             return diffFromZero;
         }
         else
@@ -95,6 +97,11 @@
 
 bool NavigateTask::wantsControl()
 {
+    if ( Scoreboard::activeProgram() == odr_pgm_wander )
+    {
+        return false;
+    }
+
     double headingDiff = ComputeHeadingDiff( Scoreboard::navTargetHeading(),
                                              Scoreboard::imuYaw() );
 
@@ -137,6 +144,12 @@
     MotorsAndServos::servoPositions( -servoPosition, servoPosition );
 
     int8_t speed = Scoreboard::navMaximumSpeed();
+
+    if ( servoPosition < -4 || servoPosition > 4 )
+    {
+        speed = 10;
+    }
+
     MotorsAndServos::motorSpeeds( speed, speed );
 }
 
--- a/main/robots/odr/ODRApp.cpp	Fri Mar 25 12:09:41 2016 -0700
+++ b/main/robots/odr/ODRApp.cpp	Fri Mar 25 12:11:00 2016 -0700
@@ -162,11 +162,11 @@
 //    m_tasks.push_back(
 //            Poco::SharedPtr< TaskObject >( new TrackWaypointsTask( logger().name() ) ) );
 
-//    m_tasks.push_back(
-//            Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );
 
-//    m_tasks.push_back(
-//            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
 
     m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new CruisingTask( logger().name() ) ) );
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/PixyApp.cpp	Fri Mar 25 12:11:00 2016 -0700
@@ -0,0 +1,215 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/PixyApp.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Application object for the Pixycam sensor bridge.
+//
+//  Copyright (c) 2016 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <iostream>
+#include <memory>
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+#include <Poco/Logger.h>
+#include <Poco/Thread.h>
+#include <Poco/Util/Application.h>
+#include <Poco/Util/HelpFormatter.h>
+#include <Poco/Util/LoggingSubsystem.h>
+#include <Poco/Util/Option.h>
+#include <Poco/Util/OptionSet.h>
+
+#include "PixyApp.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"
+
+#include "PixyReader.h"
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::sendPixyDataUpdate( Poco::Timer& timer )
+{
+    // if the PixyCam is not online then we don't send any messages
+
+    if ( ! PixyReader::isPixyAlive() )
+    {
+        return;
+    }
+
+#if 0
+    // ROLL
+
+    imudatamsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_roll );
+
+    data.data = static_cast< int32_t >( PixyReader::imuRoll() * can_data_imu_multiplier );
+
+    CANMessage::QueueToSend(
+            new CANMessage( imudatamsgid,
+                            reinterpret_cast< uint8_t* >( &data ),
+                            sizeof( data ) ) );
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+PixyApp::PixyApp()
+    : m_helpRequested( false ),
+      m_runloopActive( false ),
+      m_timerSendPixyDataUpdate()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+PixyApp::~PixyApp()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::initialize( Poco::Util::Application& self )
+{
+    loadConfiguration();
+    Poco::Util::ServerApplication::initialize( self );
+
+    addSubsystem( new Poco::Util::LoggingSubsystem() );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::uninitialize()
+{
+    Poco::Util::ServerApplication::uninitialize();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::defineOptions( Poco::Util::OptionSet& options )
+{
+    Poco::Util::ServerApplication::defineOptions( options );
+
+    options.addOption(
+        Poco::Util::Option( "help", "h", "display argument help information" )
+            .required( false )
+            .repeatable( false )
+            .callback( Poco::Util::OptionCallback<PixyApp>( this,
+                                                            &PixyApp::handleHelp ) ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::handleHelp( const std::string& name, const std::string& value )
+{
+    Poco::Util::HelpFormatter helpFormatter( options() );
+    helpFormatter.setCommand( commandName() );
+    helpFormatter.format( std::cout );
+    stopOptionsProcessing();
+    m_helpRequested = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int PixyApp::main( const std::vector<std::string>& args )
+{
+    if ( m_helpRequested )
+    {
+        return Poco::Util::Application::EXIT_OK;
+    }
+
+    loadConfiguration();
+
+    logger().information( "------------------------------------------------------" );
+    logger().information( "PixyApp::main() started" );
+
+    std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
+    logger().information( "using CANbus interface name \"" + canInterfaceName + "\"" );
+
+    Poco::Thread    canMsgProcessorThread;
+    CANMsgProcessor canMsgProcessor( canInterfaceName, logger().name() );
+    canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
+    canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
+    canMsgProcessorThread.start( canMsgProcessor );
+
+    Poco::Thread pixyReaderThread;
+    PixyReader   pixyReader( logger().name() );
+    pixyReaderThread.start( pixyReader );
+
+    m_timerSendPixyDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
+    m_timerSendPixyDataUpdate.start(
+            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );
+
+    m_runloopActive = true;
+    while ( m_runloopActive ) Poco::Thread::sleep( 5000 /* milliseconds */ );
+
+    pixyReader.timeToQuit();
+    pixyReaderThread.join();
+
+    canMsgProcessor.timeToQuit();
+    canMsgProcessorThread.join();
+
+    logger().information( "PixyApp::main() stopping" );
+
+    return Poco::Util::Application::EXIT_OK;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int main( int argc, char** argv )
+{
+    int result = -1;
+
+    try
+    {
+        PixyApp app;
+        result = app.run( argc, argv );
+    }
+    catch ( const Poco::Exception& ex )
+    {
+        std::cerr << "Failed to start application, Poco::Exception: "
+            << ex.displayText() << std::endl;
+    }
+    catch ( const std::exception& ex )
+    {
+        std::cerr << "Failed to start application, std::exception: "
+            << ex.what() << std::endl;
+    }
+    catch ( ... )
+    {
+        std::cerr << "Failed to start application, unknown exception" << std::endl;
+    }
+
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/PixyApp.h	Fri Mar 25 12:11:00 2016 -0700
@@ -0,0 +1,76 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/PixyApp.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//    
+//  Application object for the Pixycam sensor bridge.
+//
+//  Copyright (c) 2016 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_PIXYAPP_H
+#define BCDRL_ROBOTS_ODR_PIXYAPP_H
+
+#include <Poco/Timer.h>
+#include <Poco/Util/ServerApplication.h>
+
+namespace Poco
+{
+    namespace Util
+    {
+        class OptionSet;
+    }
+}
+
+#include <string>
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+class PixyApp : public Poco::Util::ServerApplication
+{
+    public:
+        PixyApp();
+        virtual ~PixyApp();
+
+    protected:
+        void initialize( Poco::Util::Application& self );
+        void uninitialize();
+        void defineOptions( Poco::Util::OptionSet& options );
+        void handleHelp( const std::string& name, const std::string& value );
+        int  main( const std::vector< std::string >& args );
+
+    private:
+        void sendPixyDataUpdate( Poco::Timer& timer );
+
+    private:
+        bool        m_helpRequested;
+        bool        m_runloopActive;
+        Poco::Timer m_timerSendPixyDataUpdate;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_PIXYAPP_H
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/PixyReader.cpp	Fri Mar 25 12:11:00 2016 -0700
@@ -0,0 +1,373 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/PixyReader.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a Pixycam.
+//
+//  Copyright (c) 2016 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 "PixyReader.h"
+
+#include <stdio.h>
+#include <pixy.h>
+
+//#include <algorithm>
+//#include <cmath>
+//#include <stdexcept>
+//#include <utility>
+//#include <vector>
+
+#include <Poco/Exception.h>
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+#include <Poco/Thread.h>
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/common/util/misc.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock    PixyReader::sm_rwLock;
+bool            PixyReader::sm_isPixyAlive;
+Poco::Timestamp PixyReader::sm_pixyLastUpdate;
+
+// ----------------------------------------------------------------------------------------
+
+static std::string PixyErrorCodeToString( int error )
+{
+    switch ( error )
+    {
+        case 0:
+            return std::string( "NO_ERROR" );
+
+        case PIXY_ERROR_USB_IO:
+            return std::string( "USB_IO" );
+
+        case PIXY_ERROR_USB_NOT_FOUND:
+            return std::string( "USB_NOT_FOUND" );
+
+        case PIXY_ERROR_USB_BUSY:
+            return std::string( "USB_BUSY" );
+
+        case PIXY_ERROR_USB_NO_DEVICE:
+            return std::string( "USB_NO_DEVICE" );
+
+        case PIXY_ERROR_INVALID_PARAMETER:
+            return std::string( "INVALID_PARAMETER" );
+
+        case PIXY_ERROR_CHIRP:
+            return std::string( "CHIRP" );
+
+        case PIXY_ERROR_INVALID_COMMAND:
+            return std::string( "INVALID_COMMAND" );
+
+        default:
+            return std::string( "unknown" );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::pixyIsOffline()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isPixyAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool PixyReader::isPixyAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( ! sm_isPixyAlive )
+    {
+        return false;
+    }
+
+    // check if the IMU is actually offline; more than 2 seconds silence means yes
+
+    static const Poco::Timestamp::TimeDiff TwoSeconds = Poco::Timestamp::resolution() * 2;
+
+    if ( sm_pixyLastUpdate.elapsed() > TwoSeconds )
+    {
+        sm_isPixyAlive = false;
+    }
+
+    return sm_isPixyAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+PixyReader::PixyReader( const std::string& loggerName )
+    : Poco::Runnable(),
+      m_loggerName( loggerName ),
+      m_quitEvent( false /* not auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+PixyReader::~PixyReader()
+{
+    closePixyCam();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::closePixyCam()
+{
+    pixy_close();
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool PixyReader::configurePixyCam()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    log.information( std::string( "PixyReader::configurePixyCam()" ) );
+
+    int result = pixy_init();
+    if ( result != 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "PixyReader::configurePixyCam() failed to init: $0 ($1)",
+                PixyErrorCodeToString( result ),
+                Poco::NumberFormatter::format( result ) ) );
+        return false;
+    }
+
+    uint16_t pixy_version_major;
+    uint16_t pixy_version_minor;
+    uint16_t pixy_version_build;
+
+    result = pixy_get_firmware_version( &pixy_version_major,
+                                        &pixy_version_minor,
+                                        &pixy_version_build );
+    if ( result != 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "PixyReader::configurePixyCam() failed to read f/w version: $0 ($1)",
+                PixyErrorCodeToString( result ),
+                Poco::NumberFormatter::format( result ) ) );
+        return false;
+    }
+
+    log.information(
+        Poco::Logger::format(
+            "PixyReader::configurePixyCam() firmware version $0.$1.$2",
+            Poco::NumberFormatter::format( pixy_version_major ),
+            Poco::NumberFormatter::format( pixy_version_minor ),
+            Poco::NumberFormatter::format( pixy_version_build ) ) );
+
+    log.information( "PixyReader::configurePixyCam() successfully initalized" );
+
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::processPixyBlocks()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    // maximum number of Pixy Blocks to process at once
+    static const int PIXY_BLOCK_BUFFER_SIZE = 25;
+
+    // Pixy Block buffer
+    struct Block pixyBlocks[ PIXY_BLOCK_BUFFER_SIZE ];
+
+    int numBlocks = pixy_get_blocks( PIXY_BLOCK_BUFFER_SIZE, pixyBlocks );
+
+    if ( numBlocks < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "PixyReader::processPixyBlocks() failed to read blocks: $0 ($1)",
+                PixyErrorCodeToString( numBlocks ),
+                Poco::NumberFormatter::format( numBlocks ) ) );
+        return;
+    }
+
+    // the returned block x,y is the center of the rectangle
+    // we should probably filter out things of the wrong aspect ratio
+
+#if 0
+    for ( int i = 0; i < numBlocks; ++i )
+    {
+        log.information(
+            Poco::Logger::format( "received pixy x: $0 y: $1 height: $2 width: $3",
+                Poco::NumberFormatter::format( pixyBlocks[ i ].x ),
+                Poco::NumberFormatter::format( pixyBlocks[ i ].y ),
+                Poco::NumberFormatter::format( pixyBlocks[ i ].height ),
+                Poco::NumberFormatter::format( pixyBlocks[ i ].width ) ) );
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::run()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    log.information( std::string( "PixyReader::run() starting" ) );
+
+    for ( ;; )
+    {
+        try
+        {
+            // now try to find the PixyCam
+
+            if ( ! configurePixyCam() )
+            {
+                Poco::Thread::sleep( 10000 ); // 10 sec
+                continue;
+            }
+
+            for ( ;; )
+            {
+                if ( m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "PixyReader::run() stopping" ) );
+                    return;
+                }
+
+                if ( pixy_blocks_are_new() > 0 )
+                {
+                    processPixyBlocks();
+                }
+
+                //Poco::Thread::sleep( 1000 ); // 1 sec
+            }
+#if 0
+            resetParseState();
+
+            // keep the last 10 values to average together (approx 0.5 seconds elapsed)
+
+            std::vector< double > valuesYaw;
+            valuesYaw.reserve( 10 );
+
+            // timeout after 10 consecutive runs of not getting data
+
+            int timeoutNoData = 0;
+
+            // loop while we are still getting data
+
+            for ( ;; )
+            {
+                if ( m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "PixyReader::run() stopping" ) );
+                    return;
+                }
+
+                char   buffer[ 64 ];
+                size_t length = array_sizeof( buffer );
+
+                if ( ! readFromSerialPort( buffer, &length ) )
+                {
+                    break; // error, drop out and reinit the port
+                }
+
+                if ( length == 0 )
+                {
+                    if ( ++timeoutNoData == 10 )
+                    {
+                        PixyReader::imuIsOffline();
+                    }
+                    continue;
+                }
+
+                // got some bytes, try to parse them
+
+                timeoutNoData = 0;
+
+                if ( processPixyData( buffer, length ) )
+                {
+                    // new values available, save them
+
+                    valuesYaw.push_back( calibrateYawValue( m_parseValueYaw ) );
+
+                    // if we've reached 10 values, compute the average and update the
+                    // scoreboard; then clear the vector making room for new values
+
+                    if ( valuesYaw.size() == 10 )
+                    {
+                        double averageYaw = computeAverageYaw( valuesYaw );
+
+                        PixyReader::imuUpdate( m_parseValueRoll,
+                                              m_parseValuePitch,
+                                              averageYaw );
+
+                        // clear the vector, to accumulate more samples
+
+                        valuesYaw.clear();
+                    }
+                }
+            }
+#endif
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyReader::run() caught Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyReader::run() caught std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            log.error( "PixyReader::run() caught unknown exception" );
+        }
+
+        // sleep for 1/2 second after exception processing, but don't exit
+        Poco::Thread::sleep( 500 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/PixyReader.h	Fri Mar 25 12:11:00 2016 -0700
@@ -0,0 +1,89 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/PixyReader.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a Pixycam.
+//
+//  Copyright (c) 2016 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_PIXYREADER_H
+#define BCDRL_ROBOTS_ODR_PIXYREADER_H
+
+#include <stdint.h>
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+#include <Poco/Timer.h>
+#include <Poco/Timestamp.h>
+
+// ----------------------------------------------------------------------------------------
+
+class PixyReader : public Poco::Runnable
+{
+    public:
+        static void   pixyIsOffline();
+        static bool   isPixyAlive();
+
+    public:
+        PixyReader( const std::string& loggerName );
+        virtual ~PixyReader();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        typedef enum
+        {
+            PARSE_STATE_LOOKING_FOR_START,
+            PARSE_STATE_START_SYMBOL_A,
+            PARSE_STATE_START_SYMBOL_N,
+            PARSE_STATE_START_SYMBOL_G,
+            PARSE_STATE_START_SYMBOL_COLON,
+            PARSE_STATE_READ_ROLL,
+            PARSE_STATE_READ_PITCH,
+            PARSE_STATE_READ_YAW
+
+        }   imu_parse_state_t;
+
+    private:
+        static Poco::RWLock    sm_rwLock;
+        static bool            sm_isPixyAlive;
+        static Poco::Timestamp sm_pixyLastUpdate;
+
+    private:
+        std::string       m_loggerName;
+        Poco::Event       m_quitEvent;
+
+    private:
+        void closePixyCam();
+        bool configurePixyCam();
+        void processPixyBlocks();
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_PIXYREADER_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/SquareCourseTask.cpp	Fri Mar 25 12:09:41 2016 -0700
+++ b/main/robots/odr/SquareCourseTask.cpp	Fri Mar 25 12:11:00 2016 -0700
@@ -7,7 +7,7 @@
 //
 //  Subsumption task to navigate in a square shape.
 //
-//  Copyright (c) 2013 Bob Cook
+//  Copyright (c) 2013-2014 Bob Cook
 //
 //  Permission is hereby granted, free of charge, to any person obtaining a copy
 //  of this software and associated documentation files (the "Software"), to deal
@@ -31,6 +31,8 @@
 
 #include "SquareCourseTask.h"
 
+#include "packages/common/can/can_messages.h"
+
 #include <cmath>
 
 #include <Poco/Logger.h>
@@ -63,6 +65,11 @@
 
 void SquareCourseTask::update()
 {
+    if ( Scoreboard::activeProgram() != odr_pgm_small_box )
+    {
+        return;
+    }
+
     if ( m_nextSwitchTime.isElapsed( sRunTimeUntilDirectionChange ) )
     {
         m_nextSwitchTime.update();
--- a/main/robots/odr/jamfile	Fri Mar 25 12:09:41 2016 -0700
+++ b/main/robots/odr/jamfile	Fri Mar 25 12:11:00 2016 -0700
@@ -51,6 +51,12 @@
     packages.linux.can.pkg
     ;
 
+PIXYAPP_SOURCES = 
+    PixyApp.cpp PixyReader.cpp
+    packages.common.can.pkg
+    packages.linux.can.pkg
+    ;
+
 POCO_SHARED = PocoFoundation.so PocoNet.so PocoUtil.so PocoXML.so ;
 POCO_STATIC = PocoFoundation.a  PocoNet.a  PocoUtil.a  PocoXML.a ;
 
@@ -62,5 +68,7 @@
 ubuntu_executable imu_ubuntu : $(IMUAPP_SOURCES) $(POCO_SHARED) ;
 overo_executable  imu        : $(IMUAPP_SOURCES) $(POCO_STATIC) ;
 
+ubuntu_executable pixy_ubuntu : $(PIXYAPP_SOURCES) $(POCO_SHARED) pixyusb.a pthread.a usb-1.0.so boost_thread.so boost_system.so boost_chrono.so ;
+
 # -----------------------------------------------------------------------------------------