Richard Sewell
This is the code we showed at Uncraftivism
MotionFinder.cpp@1:70d90598d2e7, 2009-12-10 (annotated)
- Committer:
- jarkman
- Date:
- Thu Dec 10 21:16:57 2009 +0000
- Revision:
- 1:70d90598d2e7
- Parent:
- 0:57f4fdadc97f
- Child:
- 2:01115080f6da
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jarkman | 0:57f4fdadc97f | 1 | #include "stdafx.h" |
| jarkman | 0:57f4fdadc97f | 2 | |
| jarkman | 0:57f4fdadc97f | 3 | #include "mbed.h" |
| jarkman | 0:57f4fdadc97f | 4 | #include "Frame.h" |
| jarkman | 0:57f4fdadc97f | 5 | #include "ServoMinder.h" |
| jarkman | 0:57f4fdadc97f | 6 | #include "MotionFinder.h" |
| jarkman | 0:57f4fdadc97f | 7 | |
| jarkman | 0:57f4fdadc97f | 8 | |
| jarkman | 0:57f4fdadc97f | 9 | #include "ServoMinder.h" |
| jarkman | 0:57f4fdadc97f | 10 | |
| jarkman | 0:57f4fdadc97f | 11 | |
| jarkman | 0:57f4fdadc97f | 12 | |
| jarkman | 0:57f4fdadc97f | 13 | extern Logger pcSerial; |
| jarkman | 0:57f4fdadc97f | 14 | |
| jarkman | 0:57f4fdadc97f | 15 | // Motion detection for the mbed |
| jarkman | 0:57f4fdadc97f | 16 | |
| jarkman | 0:57f4fdadc97f | 17 | |
| jarkman | 0:57f4fdadc97f | 18 | MotionFinder::MotionFinder( ServoMinder *xServoMinder, ServoMinder *yServoMinder ) |
| jarkman | 0:57f4fdadc97f | 19 | { |
| jarkman | 0:57f4fdadc97f | 20 | m_backgroundFrame = NULL; |
| jarkman | 0:57f4fdadc97f | 21 | m_resultFrame = NULL; |
| jarkman | 0:57f4fdadc97f | 22 | m_attentionX = 0.5; |
| jarkman | 0:57f4fdadc97f | 23 | m_attentionY = 0.5; |
| jarkman | 0:57f4fdadc97f | 24 | |
| jarkman | 0:57f4fdadc97f | 25 | |
| jarkman | 0:57f4fdadc97f | 26 | |
| jarkman | 0:57f4fdadc97f | 27 | m_xServoMinder = xServoMinder; |
| jarkman | 0:57f4fdadc97f | 28 | m_yServoMinder = yServoMinder; |
| jarkman | 0:57f4fdadc97f | 29 | |
| jarkman | 1:70d90598d2e7 | 30 | |
| jarkman | 1:70d90598d2e7 | 31 | |
| jarkman | 1:70d90598d2e7 | 32 | |
| jarkman | 0:57f4fdadc97f | 33 | m_xServoMinder->moveTo( 1.0 ); |
| jarkman | 0:57f4fdadc97f | 34 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 35 | m_xServoMinder->moveTo( 0.0 ); |
| jarkman | 0:57f4fdadc97f | 36 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 37 | m_xServoMinder->moveTo( 0.5 ); |
| jarkman | 0:57f4fdadc97f | 38 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 39 | |
| jarkman | 1:70d90598d2e7 | 40 | |
| jarkman | 0:57f4fdadc97f | 41 | m_yServoMinder->moveTo( 1.0 ); |
| jarkman | 0:57f4fdadc97f | 42 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 43 | m_yServoMinder->moveTo( 0.0 ); |
| jarkman | 0:57f4fdadc97f | 44 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 45 | m_yServoMinder->moveTo( 0.5 ); |
| jarkman | 0:57f4fdadc97f | 46 | wait( 1 ); |
| jarkman | 0:57f4fdadc97f | 47 | } |
| jarkman | 0:57f4fdadc97f | 48 | |
| jarkman | 0:57f4fdadc97f | 49 | |
| jarkman | 0:57f4fdadc97f | 50 | |
| jarkman | 0:57f4fdadc97f | 51 | MotionFinder::~MotionFinder() |
| jarkman | 0:57f4fdadc97f | 52 | { |
| jarkman | 0:57f4fdadc97f | 53 | if( m_backgroundFrame != NULL ) |
| jarkman | 0:57f4fdadc97f | 54 | Frame::releaseFrame( & m_backgroundFrame ); |
| jarkman | 0:57f4fdadc97f | 55 | |
| jarkman | 0:57f4fdadc97f | 56 | if( m_resultFrame != NULL ) |
| jarkman | 0:57f4fdadc97f | 57 | Frame::releaseFrame( & m_resultFrame ); |
| jarkman | 0:57f4fdadc97f | 58 | |
| jarkman | 0:57f4fdadc97f | 59 | } |
| jarkman | 0:57f4fdadc97f | 60 | |
| jarkman | 0:57f4fdadc97f | 61 | void MotionFinder::newBackground( Frame *frame ) |
| jarkman | 0:57f4fdadc97f | 62 | { |
| jarkman | 0:57f4fdadc97f | 63 | Frame::releaseFrame( & m_backgroundFrame ); |
| jarkman | 0:57f4fdadc97f | 64 | return processFrame( frame ); |
| jarkman | 0:57f4fdadc97f | 65 | } |
| jarkman | 0:57f4fdadc97f | 66 | |
| jarkman | 0:57f4fdadc97f | 67 | void MotionFinder::processFrame( Frame *frame ) |
| jarkman | 0:57f4fdadc97f | 68 | { |
| jarkman | 0:57f4fdadc97f | 69 | if( frame == NULL || frame->m_bad ) |
| jarkman | 0:57f4fdadc97f | 70 | { |
| jarkman | 0:57f4fdadc97f | 71 | if( m_resultFrame != NULL ) |
| jarkman | 1:70d90598d2e7 | 72 | m_resultFrame->m_bad = false; |
| jarkman | 0:57f4fdadc97f | 73 | return; |
| jarkman | 0:57f4fdadc97f | 74 | } |
| jarkman | 0:57f4fdadc97f | 75 | |
| jarkman | 0:57f4fdadc97f | 76 | |
| jarkman | 0:57f4fdadc97f | 77 | if( m_backgroundFrame == NULL ) |
| jarkman | 0:57f4fdadc97f | 78 | { |
| jarkman | 0:57f4fdadc97f | 79 | m_backgroundFrame = frame; |
| jarkman | 0:57f4fdadc97f | 80 | |
| jarkman | 0:57f4fdadc97f | 81 | m_delta = 1 << (m_backgroundFrame->m_bitsPerPixel - 4); // smallest interesting change - make sure this is nonzero for 4-bit iamges! |
| jarkman | 0:57f4fdadc97f | 82 | |
| jarkman | 0:57f4fdadc97f | 83 | if( m_delta < 2 ) |
| jarkman | 0:57f4fdadc97f | 84 | m_delta = 2; |
| jarkman | 0:57f4fdadc97f | 85 | |
| jarkman | 0:57f4fdadc97f | 86 | Frame::cloneFrame( &m_resultFrame, m_backgroundFrame ); |
| jarkman | 0:57f4fdadc97f | 87 | m_resultFrame->m_bad = false; |
| jarkman | 0:57f4fdadc97f | 88 | return; |
| jarkman | 0:57f4fdadc97f | 89 | } |
| jarkman | 0:57f4fdadc97f | 90 | |
| jarkman | 0:57f4fdadc97f | 91 | if( frame->m_numPixels != m_backgroundFrame->m_numPixels ) |
| jarkman | 0:57f4fdadc97f | 92 | { |
| jarkman | 0:57f4fdadc97f | 93 | m_resultFrame->m_bad = false; |
| jarkman | 0:57f4fdadc97f | 94 | return; |
| jarkman | 0:57f4fdadc97f | 95 | } |
| jarkman | 0:57f4fdadc97f | 96 | |
| jarkman | 0:57f4fdadc97f | 97 | |
| jarkman | 0:57f4fdadc97f | 98 | uint32_t sumX = 0; |
| jarkman | 0:57f4fdadc97f | 99 | uint32_t sumY = 0; |
| jarkman | 0:57f4fdadc97f | 100 | uint32_t sumN = 0; |
| jarkman | 0:57f4fdadc97f | 101 | uint16_t x = 0, y = 0; |
| jarkman | 0:57f4fdadc97f | 102 | |
| jarkman | 0:57f4fdadc97f | 103 | for( uint32_t i = 0; i < frame->m_numPixels; i += 1 ) |
| jarkman | 0:57f4fdadc97f | 104 | { |
| jarkman | 0:57f4fdadc97f | 105 | x ++; |
| jarkman | 0:57f4fdadc97f | 106 | if( x >= frame->m_width ) |
| jarkman | 0:57f4fdadc97f | 107 | { |
| jarkman | 0:57f4fdadc97f | 108 | y++; |
| jarkman | 0:57f4fdadc97f | 109 | x = 0; |
| jarkman | 0:57f4fdadc97f | 110 | } |
| jarkman | 0:57f4fdadc97f | 111 | |
| jarkman | 0:57f4fdadc97f | 112 | uint16_t pb = m_backgroundFrame->getPixel( i ); |
| jarkman | 0:57f4fdadc97f | 113 | uint16_t pf = frame->getPixel( i ); |
| jarkman | 0:57f4fdadc97f | 114 | |
| jarkman | 0:57f4fdadc97f | 115 | if( ( pf > pb && pf - pb > m_delta ) || ( pf < pb && pb - pf > m_delta )) |
| jarkman | 0:57f4fdadc97f | 116 | { |
| jarkman | 0:57f4fdadc97f | 117 | // different from background |
| jarkman | 0:57f4fdadc97f | 118 | m_resultFrame->setPixel( i, pf ); |
| jarkman | 0:57f4fdadc97f | 119 | sumX += x; |
| jarkman | 0:57f4fdadc97f | 120 | sumY += y; |
| jarkman | 0:57f4fdadc97f | 121 | sumN ++; |
| jarkman | 0:57f4fdadc97f | 122 | |
| jarkman | 0:57f4fdadc97f | 123 | } |
| jarkman | 0:57f4fdadc97f | 124 | else |
| jarkman | 0:57f4fdadc97f | 125 | { |
| jarkman | 0:57f4fdadc97f | 126 | // same-ish as background |
| jarkman | 0:57f4fdadc97f | 127 | m_resultFrame->setPixel( i, 0 ); |
| jarkman | 0:57f4fdadc97f | 128 | |
| jarkman | 0:57f4fdadc97f | 129 | |
| jarkman | 0:57f4fdadc97f | 130 | } |
| jarkman | 0:57f4fdadc97f | 131 | |
| jarkman | 0:57f4fdadc97f | 132 | //and make the background a little bit more like this pixel, to adjust to slow changes in lighting |
| jarkman | 0:57f4fdadc97f | 133 | if( pf > pb ) |
| jarkman | 0:57f4fdadc97f | 134 | m_backgroundFrame->setPixel( i, pb +1 ); |
| jarkman | 0:57f4fdadc97f | 135 | |
| jarkman | 0:57f4fdadc97f | 136 | if( pf < pb ) |
| jarkman | 0:57f4fdadc97f | 137 | m_backgroundFrame->setPixel( i, pb - 1 ); |
| jarkman | 0:57f4fdadc97f | 138 | |
| jarkman | 0:57f4fdadc97f | 139 | } |
| jarkman | 0:57f4fdadc97f | 140 | |
| jarkman | 0:57f4fdadc97f | 141 | uint32_t cogX = 0; |
| jarkman | 0:57f4fdadc97f | 142 | uint32_t cogY = 0; |
| jarkman | 0:57f4fdadc97f | 143 | |
| jarkman | 0:57f4fdadc97f | 144 | if( frame->m_numPixels < 1 ) |
| jarkman | 0:57f4fdadc97f | 145 | frame->m_numPixels = 1; |
| jarkman | 0:57f4fdadc97f | 146 | |
| jarkman | 0:57f4fdadc97f | 147 | uint32_t percentage = (sumN * 100) / frame->m_numPixels; |
| jarkman | 0:57f4fdadc97f | 148 | |
| jarkman | 0:57f4fdadc97f | 149 | pcSerial.printf("\r\n%d percent changed pixels\r\n", (int) percentage); |
| jarkman | 0:57f4fdadc97f | 150 | |
| jarkman | 0:57f4fdadc97f | 151 | if( percentage < 3 ) // no real target, no COG |
| jarkman | 0:57f4fdadc97f | 152 | { |
| jarkman | 0:57f4fdadc97f | 153 | pcSerial.printf("No COG\r\n"); |
| jarkman | 0:57f4fdadc97f | 154 | |
| jarkman | 0:57f4fdadc97f | 155 | // could implement some looking-around in this state |
| jarkman | 0:57f4fdadc97f | 156 | |
| jarkman | 0:57f4fdadc97f | 157 | } |
| jarkman | 0:57f4fdadc97f | 158 | else if( sumN > 0 ) |
| jarkman | 0:57f4fdadc97f | 159 | { |
| jarkman | 0:57f4fdadc97f | 160 | cogX = sumX / sumN; |
| jarkman | 0:57f4fdadc97f | 161 | cogY = sumY / sumN; |
| jarkman | 0:57f4fdadc97f | 162 | |
| jarkman | 0:57f4fdadc97f | 163 | m_attentionX = ((float)cogX / frame->m_width); |
| jarkman | 0:57f4fdadc97f | 164 | m_attentionY = ((float)cogY / frame->m_width); // use the larger dimension so x & y get the same scaling |
| jarkman | 0:57f4fdadc97f | 165 | |
| jarkman | 0:57f4fdadc97f | 166 | pcSerial.printf("COG is %d, %d\r\n", (int) cogX, (int) cogY); |
| jarkman | 0:57f4fdadc97f | 167 | |
| jarkman | 0:57f4fdadc97f | 168 | } |
| jarkman | 0:57f4fdadc97f | 169 | |
| jarkman | 0:57f4fdadc97f | 170 | m_xServoMinder->moveTo( 1 - m_attentionX ); |
| jarkman | 1:70d90598d2e7 | 171 | m_yServoMinder->moveTo( m_attentionY ); |
| jarkman | 0:57f4fdadc97f | 172 | |
| jarkman | 0:57f4fdadc97f | 173 | |
| jarkman | 0:57f4fdadc97f | 174 | Frame::releaseFrame( &frame ); |
| jarkman | 0:57f4fdadc97f | 175 | |
| jarkman | 0:57f4fdadc97f | 176 | m_resultFrame->m_bad = false; |
| jarkman | 0:57f4fdadc97f | 177 | return; |
| jarkman | 0:57f4fdadc97f | 178 | } |
| jarkman | 0:57f4fdadc97f | 179 | |
| jarkman | 0:57f4fdadc97f | 180 | |
| jarkman | 0:57f4fdadc97f | 181 |