MotionFinder.cpp

00001 #include "stdafx.h"
00002 
00003 #include "mbed.h"
00004 #include "Frame.h"
00005 #include "ServoMinder.h"
00006 #include "MotionFinder.h"
00007 
00008 
00009 #include "ServoMinder.h"
00010 #include "Blinker.h"
00011 
00012 
00013 
00014 extern Logger pcSerial;
00015 extern Blinker *blinker;
00016 
00017 // Motion detection for the mbed
00018 
00019 
00020 MotionFinder::MotionFinder( ServoMinder *xServoMinder, ServoMinder *yServoMinder )
00021 {
00022     m_backgroundFrame = NULL;
00023     m_resultFrame = NULL;
00024     m_attentionX = 0.5;
00025     m_attentionY = 0.5;
00026 
00027 
00028 
00029     m_xServoMinder = xServoMinder;
00030     m_yServoMinder = yServoMinder;
00031     
00032    
00033     
00034 /*
00035     m_xServoMinder->moveTo( 1.0 );
00036     wait( 1 );
00037     m_xServoMinder->moveTo( 0.0 );
00038     wait( 1 );
00039     m_xServoMinder->moveTo( 0.5 );
00040     wait( 1 );
00041 
00042     
00043     m_yServoMinder->moveTo( 0.7 );
00044     wait( 1 );
00045     m_yServoMinder->moveTo( 0.0 );
00046     wait( 2 );
00047     m_yServoMinder->moveTo( 0.5 );
00048     wait( 2 );
00049     */
00050 }
00051 
00052     
00053 
00054 MotionFinder::~MotionFinder()
00055 {
00056     if( m_backgroundFrame != NULL )
00057         Frame::releaseFrame( & m_backgroundFrame );
00058 
00059     if( m_resultFrame != NULL )
00060         Frame::releaseFrame( & m_resultFrame );
00061 
00062 }
00063 
00064 void MotionFinder::newBackground( Frame *frame )
00065 {
00066     Frame::releaseFrame( & m_backgroundFrame );
00067     return processFrame( frame );
00068 }
00069 
00070 void MotionFinder::processFrame( Frame *frame )
00071 {
00072     if( frame == NULL || frame->m_bad )
00073     {
00074         if( m_resultFrame != NULL )
00075             m_resultFrame->m_bad = false;
00076         return;
00077     }
00078 
00079 
00080     if( m_backgroundFrame == NULL )
00081     {
00082         m_backgroundFrame = frame;
00083 
00084         m_delta = 1 << (m_backgroundFrame->m_bitsPerPixel - 4); // smallest interesting change - make sure this is nonzero for 4-bit iamges!
00085 
00086         if( m_delta < 2 )
00087             m_delta = 2;
00088 
00089         Frame::cloneFrame( &m_resultFrame, m_backgroundFrame );
00090         m_resultFrame->m_bad = false;
00091         return;
00092     }
00093 
00094     if( frame->m_numPixels != m_backgroundFrame->m_numPixels )
00095     {       
00096         m_resultFrame->m_bad = false;
00097         return;
00098     }
00099 
00100 
00101     uint32_t sumX = 0;
00102     uint32_t sumY = 0;
00103     uint32_t sumN = 0;
00104     uint16_t x = 0, y = 0;
00105 
00106     for( uint32_t i = 0; i < frame->m_numPixels; i += 1 )
00107     {
00108         x ++;
00109         if( x >= frame->m_width )
00110         {
00111             y++;
00112             x = 0;
00113         }
00114 
00115         uint16_t pb = m_backgroundFrame->getPixel( i );
00116         uint16_t pf = frame->getPixel( i );
00117 
00118         if( ( pf > pb && pf - pb > m_delta ) || ( pf < pb && pb - pf > m_delta ))
00119         {
00120             // different from background
00121             m_resultFrame->setPixel( i, pf );
00122             sumX += x;
00123             sumY += y;
00124             sumN ++;
00125 
00126         }
00127         else
00128         {
00129             // same-ish as background
00130             m_resultFrame->setPixel( i, 0 );
00131 
00132 
00133         }
00134         
00135         //and make the background a little bit more like this pixel, to adjust to slow changes in lighting
00136         if( pf > pb )
00137             m_backgroundFrame->setPixel( i, pb +1 );
00138                 
00139         if( pf < pb )
00140             m_backgroundFrame->setPixel( i, pb - 1 );
00141 
00142     }
00143 
00144     uint32_t cogX = 0;
00145     uint32_t cogY = 0;
00146 
00147     if( frame->m_numPixels < 1 )
00148         frame->m_numPixels = 1;
00149 
00150     uint32_t percentage = (sumN * 100) / frame->m_numPixels;
00151     
00152     pcSerial.printf("\r\n%d percent changed pixels\r\n", (int) percentage);
00153     
00154     if( percentage < 3 ) // no real target, no COG
00155     {
00156         pcSerial.printf("No COG\r\n");
00157 
00158        m_xServoMinder->setSpeed( 0.02 );
00159        m_yServoMinder->setSpeed( 0.02 );
00160        
00161         blinker->setBoredom( 1 );
00162         
00163         m_attentionX = m_attentionX + (((0.5 - m_attentionX))/20);
00164         m_attentionY = m_attentionY + (((0.7 - m_attentionY))/20);
00165        // could implement some looking-around in this state
00166        
00167 
00168 
00169     }
00170     else if( sumN > 0 )
00171     {
00172          m_xServoMinder->setSpeed( 0.25 );
00173          m_yServoMinder->setSpeed( 0.25 );
00174          
00175         cogX = sumX / sumN;
00176         cogY = sumY / sumN;
00177 
00178         m_attentionX = ((float)cogX  / frame->m_width);
00179         m_attentionY = ((float)cogY  / frame->m_width); // use the larger dimension so x & y get the same scaling
00180 
00181         //blinker->setBoredom( 0 );
00182         float boredom = (2*percentage)/100.0;
00183         if(boredom > 1) boredom = 1;
00184         blinker->setBoredom( boredom );
00185 
00186 
00187         pcSerial.printf("COG is %d, %d\r\n", (int) cogX, (int) cogY);
00188 
00189     }
00190 
00191     m_xServoMinder->moveTo( 1 - m_attentionX );
00192     m_yServoMinder->moveTo(  m_attentionY ); 
00193 
00194 
00195     Frame::releaseFrame( &frame );
00196 
00197     m_resultFrame->m_bad = false;
00198     return;
00199 }
00200 
00201 
00202