changeset 271:5a43fb1075a1 main

Merge from upstream
author Bob Cook <bob@bobcookdev.com>
date Mon, 02 May 2016 19:35:56 -0700
parents c5c1dd29499c (current diff) bf47051f7919 (diff)
children 2edb08c3098a
files
diffstat 11 files changed, 1204 insertions(+), 12 deletions(-) [+]
line wrap: on
line diff
--- a/main/jrules/ubuntu.unix.inc	Mon May 02 19:35:31 2016 -0700
+++ b/main/jrules/ubuntu.unix.inc	Mon May 02 19:35:56 2016 -0700
@@ -43,9 +43,11 @@
 UBUNTU_FLTK_INC = "/home/bob/software/fltk/deploy/usr/include" ;
 UBUNTU_FLTK_LIBDIR = "/home/bob/software/fltk/deploy/usr/lib" ;
 
+UBUNTU_LIBUSB_INC = "/usr/include/libusb-1.0" ;
+
 UBUNTU_STD_LIBDIR = "/usr/lib" ;
 
-UBUNTU_SYSINCDIR = $(UBUNTU_POCO_INC) $(UBUNTU_FLTK_INC) ;
+UBUNTU_SYSINCDIR = $(UBUNTU_POCO_INC) $(UBUNTU_FLTK_INC) $(UBUNTU_LIBUSB_INC) ;
 UBUNTU_SYSLIBDIR = $(UBUNTU_POCO_LIBDIR) $(UBUNTU_FLTK_LIBDIR) $(UBUNTU_STD_LIBDIR) ;
 
 # -----------------------------------------------------------------------------------------
--- a/main/packages/linux/can/CANMsgProcessor.cpp	Mon May 02 19:35:31 2016 -0700
+++ b/main/packages/linux/can/CANMsgProcessor.cpp	Mon May 02 19:35:56 2016 -0700
@@ -9,7 +9,7 @@
 //  into a global queue for processing by other threads. Messages are pulled from another
 //  global queue for writing to the socket.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-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
@@ -111,8 +111,8 @@
             CANSocket sock;
             sock.bind( m_canInterfaceName );
 
-            Poco::Timespan pollDelay( 100 * 1000 ); // 100ms
-            Poco::Timespan writeWaitTime( 100 * 1000 );
+            Poco::Timespan pollDelay( 100 ); // 0.1ms
+            Poco::Timespan writeWaitTime( 1 * 1000 ); // 1ms
 
             for ( ;; )
             {
@@ -134,7 +134,7 @@
 
                 // handle queued writes
 
-                Poco::AutoPtr<CANMessage> msg( CANMessage::WaitDequeueToSend( 100 ) ); // 100ms
+                Poco::AutoPtr<CANMessage> msg( CANMessage::WaitDequeueToSend( 0 ) );
 
                 if ( msg.get() != 0 )
                 {
--- a/main/robots/odr/NavigateTask.cpp	Mon May 02 19:35:31 2016 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Mon May 02 19:35:56 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	Mon May 02 19:35:31 2016 -0700
+++ b/main/robots/odr/ODRApp.cpp	Mon May 02 19:35:56 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	Mon May 02 19:35:56 2016 -0700
@@ -0,0 +1,419 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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"
+
+// ----------------------------------------------------------------------------------------
+
+// incoming messages: start tracking, stop tracking
+// outgoing messages: heartbeat, track info
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::sendHeartbeat( Poco::Timer& timer )
+{
+    // if the PixyCam is not online then we don't send any messages
+
+    if ( ! PixyReader::isPixyAlive() )
+    {
+        return;
+    }
+
+    uint32_t heartbeatmsgid = can_build_message_id(
+                    can_node_sensor_pixy, can_node_broadcast, can_dataid_heartbeat );
+
+    CANMessage::QueueToSend( new CANMessage( heartbeatmsgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+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
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvSetPixyTracking( bool enabled )
+{
+    if ( enabled )
+    {
+        m_timerSendPixyDataUpdate.start(
+            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );
+    }
+    else
+    {
+        m_timerSendPixyDataUpdate.stop();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvEmergency( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isEmergencyActive = true;
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            sm_DisplayEmergency = true;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = true;
+            break;
+
+        case can_node_odr_motion:
+            sm_MotionEmergency = true;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = true;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvAllClear( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            sm_DisplayEmergency = false;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = false;
+            break;
+
+        case can_node_odr_motion:
+            sm_MotionEmergency = false;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = false;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvHeartbeat( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            m_timerDisplayTimeout.restart();
+            sm_isDisplayAlive = true;
+            break;
+
+        case can_node_sensor_gps:
+            m_timerGpsTimeout.restart();
+            sm_isGpsAlive = true;
+            break;
+
+        case can_node_odr_motion:
+            m_timerMotionTimeout.restart();
+            sm_isMotionAlive = true;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::runLoop( const std::string& loggerName )
+{
+    Poco::Logger& log = Poco::Logger::get( loggerName );
+
+    for ( ;; )
+    {
+        log.information( std::string( "PixyApp::runLoop() starting" ) );
+
+        try
+        {
+            for ( ;; )
+            {
+                CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms
+
+                if ( msg == 0 && m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "PixyApp::runLoop() stopping" ) );
+                    return;
+                }
+
+                if ( msg == 0 )
+                {
+                    continue;
+                }
+
+                uint8_t  srcNode;
+                uint8_t  dstNode;
+                uint16_t dataId;
+
+                can_parse_message_id( 
+                        msg->msgIdentifier(), &srcNode, &dstNode, &dataId );
+
+                switch ( dataId )
+                {
+                    case can_dataid_emergency:
+                        recvEmergency( srcNode );
+                        break;
+
+                    case can_dataid_all_clear:
+                        recvAllClear( srcNode );
+                        break;
+
+                    case can_dataid_heartbeat:
+                        recvHeartbeat( srcNode );
+                        break;
+
+                    case can_dataid_pixy_start_tracking:
+                        recvSetPixyTracking( true );
+                        break;
+
+                    case can_dataid_pixy_stop_tracking:
+                        recvSetPixyTracking( false );
+                        break;
+                }
+            }
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyApp::runLoop() caught Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyApp::runLoop() caught std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            log.error( "PixyApp::runLoop() caught unknown exception" );
+        }
+
+        // sleep for 1/2 second after exception processing, but don't exit
+        Poco::Thread::sleep( 500 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+PixyApp::PixyApp()
+    : m_helpRequested( false ),
+      m_quitEvent( false /* not auto-reset */ ),
+      m_timerSendHeartbeat(),
+      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_timerSendHeartbeat.setPeriodicInterval( 750 /* milliseconds */ );
+    m_timerSendHeartbeat.start(
+            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendHeartbeat ) );
+
+    m_timerSendPixyDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
+
+    runLoop( logger().name() );
+
+    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	Mon May 02 19:35:56 2016 -0700
@@ -0,0 +1,85 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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/Event.h>
+#include <Poco/Timer.h>
+#include <Poco/Util/ServerApplication.h>
+
+namespace Poco
+{
+    namespace Util
+    {
+        class OptionSet;
+    }
+}
+
+#include <stdint.h>
+#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 sendHeartbeat( Poco::Timer& timer );
+        void sendPixyDataUpdate( Poco::Timer& timer );
+        void recvEmergency( uint8_t srcNode );
+        void recvAllClear( uint8_t srcNode );
+        void recvHeartbeat( uint8_t srcNode );
+        void recvSetPixyTracking( bool enabled );
+        void runLoop( const std::string& loggerName );
+
+    private:
+        bool        m_helpRequested;
+        Poco::Event m_quitEvent;
+        Poco::Timer m_timerSendHeartbeat;
+        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	Mon May 02 19:35:56 2016 -0700
@@ -0,0 +1,541 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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 = false;
+Poco::Timestamp PixyReader::sm_pixyLastUpdate;
+bool            PixyReader::sm_hasTarget = false;
+int8_t          PixyReader::sm_targetPosition;
+
+// ----------------------------------------------------------------------------------------
+
+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" );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static const int8_t MAX_LUT_VALUE =  10;
+static const int8_t MIN_LUT_VALUE = -10;
+
+static int8_t XCoordinateToPosition( uint16_t xCoord )
+{
+    static std::vector< int8_t > s_positionLut;
+
+    if ( s_positionLut.size() == 0 )
+    {
+        s_positionLut.reserve( PIXY_MAX_X - PIXY_MIN_X + 1 );
+
+        double scale_factor = static_cast< double >( MAX_LUT_VALUE * 2 );
+        scale_factor /= static_cast< double >( PIXY_MAX_X - PIXY_MIN_X );
+
+        for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
+        {
+            double v = static_cast< double >( i );
+
+            if ( i < ( PIXY_MAX_X / 2 ) )
+            {
+                v = static_cast< double >( PIXY_MAX_X / 2 ) - v;
+                v *= -1.0;
+            }
+            else
+            {
+                v -= static_cast< double >( PIXY_MAX_X / 2 );
+            }
+
+            v *= scale_factor;
+
+            s_positionLut.push_back( static_cast< int8_t >( v ) );
+        }
+
+        s_positionLut[ 0 ] = MIN_LUT_VALUE; // force boundries
+    }
+
+    if ( s_positionLut.size() <= xCoord )
+    {
+        return MAX_LUT_VALUE;
+    }
+
+    return s_positionLut[ xCoord ];
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::pixyIsOnline()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isPixyAlive = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+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;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool PixyReader::hasTarget( int8_t* position )
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( sm_hasTarget && position != NULL )
+    {
+        *position = sm_targetPosition;
+    }
+    
+    return sm_hasTarget;
+}
+
+// ----------------------------------------------------------------------------------------
+
+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()" ) );
+
+#if 0
+    for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
+    {
+        log.information(
+            Poco::Logger::format(
+                "PixyReader::configurePixyCam(): $0 = $1",
+                Poco::NumberFormatter::format( i ),
+                Poco::NumberFormatter::format( XCoordinateToPosition( i ) ) ) );
+    }
+#endif
+
+    int result = pixy_init();
+    if ( result != 0 )
+    {
+        pixyIsOffline();
+
+        log.error(
+            Poco::Logger::format(
+                "PixyReader::configurePixyCam() failed to init: $0 ($1)",
+                PixyErrorCodeToString( result ),
+                Poco::NumberFormatter::format( result ) ) );
+
+        return false;
+    }
+
+    pixyIsOnline();
+
+    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 )
+    {
+        pixyIsOffline();
+
+        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 )
+    {
+        pixyIsOffline();
+
+        log.error(
+            Poco::Logger::format(
+                "PixyReader::processPixyBlocks() failed to read blocks: $0 ($1)",
+                PixyErrorCodeToString( numBlocks ),
+                Poco::NumberFormatter::format( numBlocks ) ) );
+        return;
+    }
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    if ( numBlocks == 0 )
+    {
+        sm_hasTarget = false;
+        return; // weird but true
+    }
+
+    // compute the ratio of height to width and surface area for each block, rejecting
+    // blocks that are really the wrong ratio
+
+    double heightToWidthRatio[ PIXY_BLOCK_BUFFER_SIZE ];
+    int    surfaceArea[ PIXY_BLOCK_BUFFER_SIZE ];
+
+    for ( int i = 0; i < numBlocks; ++i )
+    {
+        if ( pixyBlocks[ i ].height < 10 || pixyBlocks[ i ].width < 10 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+            continue;
+        }
+
+        double ratio = (double)(pixyBlocks[ i ].height) / (double)(pixyBlocks[ i ].width);
+
+        if ( ratio < 1.0 || ratio > 2.0 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+            continue;
+        }
+
+        surfaceArea[ i ] = pixyBlocks[ i ].height * pixyBlocks[ i ].width;
+        heightToWidthRatio[ i ] = ratio;
+
+        if ( surfaceArea[ i ] < 1000 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+        }
+    }
+
+    // find the largest block
+
+    int largestSurfaceArea = surfaceArea[ 0 ];
+    int indexOfLargest = 0;
+
+    for ( int i = 1; i < numBlocks; ++i )
+    {
+        if ( surfaceArea[ i ] > largestSurfaceArea )
+        {
+            largestSurfaceArea = surfaceArea[ i ];
+            indexOfLargest = i;
+        }
+    }
+
+    for ( int i = 0; i < numBlocks; ++i )
+    {
+        if ( surfaceArea[ i ] == 0 )
+        {
+            continue;
+        }
+
+#if 1
+        log.information(
+            Poco::Logger::format( "received pixy x: $0 y: $1 ratio: $2 area: $3",
+                Poco::NumberFormatter::format( pixyBlocks[ i ].x ),
+                Poco::NumberFormatter::format( pixyBlocks[ i ].y ),
+                Poco::NumberFormatter::format( heightToWidthRatio[ i ], 6, 4 ),
+                Poco::NumberFormatter::format( surfaceArea[ i ] ) ) );
+#endif
+    }
+
+    if ( largestSurfaceArea > 0 )
+    {
+        sm_hasTarget      = true;
+        sm_targetPosition = XCoordinateToPosition( pixyBlocks[ indexOfLargest ].x );
+
+        log.information(
+            Poco::Logger::format( "choose candidate block $0 (ratio $1, area $2, pos $3)",
+                Poco::NumberFormatter::format( indexOfLargest ),
+                Poco::NumberFormatter::format( heightToWidthRatio[ indexOfLargest ], 6, 4 ),
+                Poco::NumberFormatter::format( surfaceArea[ indexOfLargest ] ),
+                Poco::NumberFormatter::format( sm_targetPosition ) ) );
+    }
+    else
+    {
+        sm_hasTarget = false;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+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;
+            }
+
+            // read data from the PixyCam until told to quit
+
+            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	Mon May 02 19:35:56 2016 -0700
@@ -0,0 +1,81 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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 bool isPixyAlive();
+        static bool hasTarget( int8_t* position = NULL );
+
+    public:
+        PixyReader( const std::string& loggerName );
+        virtual ~PixyReader();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static void pixyIsOnline();
+        static void pixyIsOffline();
+
+    private:
+        static Poco::RWLock    sm_rwLock;
+        static bool            sm_isPixyAlive;
+        static Poco::Timestamp sm_pixyLastUpdate;
+        static bool            sm_hasTarget;
+        static int8_t          sm_targetPosition;
+
+    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	Mon May 02 19:35:31 2016 -0700
+++ b/main/robots/odr/SquareCourseTask.cpp	Mon May 02 19:35:56 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	Mon May 02 19:35:31 2016 -0700
+++ b/main/robots/odr/jamfile	Mon May 02 19:35:56 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 ;
+
 # -----------------------------------------------------------------------------------------
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/pixy_ubuntu.xml	Mon May 02 19:35:56 2016 -0700
@@ -0,0 +1,36 @@
+<config>
+    <logging>
+        <formatters>
+            <f1>
+                <class>PatternFormatter</class>
+                <pattern>%q %m/%d/%y-%H:%M:%S.%F %t</pattern>
+                <times>UTC</times>
+            </f1>
+        </formatters>
+        <channels>
+            <c1>
+                <class>FileChannel</class>
+                <formatter>f1</formatter>
+                <path>/home/bob/devroot/main/robots/odr/pixy.log</path>
+                <rotation>1M</rotation>
+                <archive>number</archive>
+                <purgeCount>4</purgeCount>
+            </c1>
+        </channels>
+        <loggers>
+            <primary>
+                <channel>c1</channel>
+                <level>information</level>
+            </primary>
+        </loggers>
+    </logging>
+    <application>
+        <logger>primary</logger>
+    </application>
+    <can>
+        <interfaceName>can0</interfaceName>
+        <logIncoming>no</logIncoming>
+        <logOutgoing>no</logOutgoing>
+    </can>
+</config>
+