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