Tracking cats internally

Discussion about the ZBasic language including the System Library. If you're not sure where to post your message, do it here. However, do not make test posts here; that's the purpose of the Sandbox.
Post Reply
sturgessb
Posts: 287
Joined: 25 April 2008, 6:34 AM
Location: Norwich, UK

Tracking cats internally

Post by sturgessb »

Hi all

Working on a fun project to track cats internally around houses. I need to track 4 individual cats around a standard 3 bedroom house using an array of sensors around the house.

I am using the camera from a wiimote, to detect the 4 brightest IR signatures in the field of view (Camera attached to ceilings looking down). Each IR blob is returned with a X and Y co-ordinate from the camera.

The cats wear a IR emitting collar and to identify which cat is which I have each collar flashing at a set frequency. 10hz, 20hz, 40hz and 80hz.

Each time a blob comes into view i record into into FIFO array with its x/y position and timestamp. When each blob comes in i look through that array bottom up and look for a recording that is in a similar position, i then calculate the delta time between those two recordings to workout which cat that is.

Obviously in loops where the IR light is still on the delta time is just the time of each loop cycle, but as these are not in the range i am looking for it doesn't seem to be an issue.

The camera outputs data at around 200hz.

While this is working the timing can be a little off at times and sometimes get some readings where cat 3 shows as 4 etc. I'm just wondering if any of you chaps have any ideas on how I might be able to achieve the above more efficiently, and with perhaps a bit more error detection. My shift array left function is also a little crude.

Code below, any feedback welcome.

Cheers

B

Code: Select all

Option ConsoleSpeed 115200
Option SignOn Off

'STACKS
public ComTaskStack(1 to 200) as byte

'I2C
PUBLIC i2c_Result as INTEGER
PUBLIC i2c_Result_bol as BOOLEAN
PUBLIC i2c_OutData(1 to 16) as BYTE
PUBLIC i2c_InData(1 to 16) as BYTE

'BLOB PROCESSING
PUBLIC blob_i as Byte
PUBLIC blob_x(1 to 4) as Integer
PUBLIC blob_y(1 to 4) as Integer
PUBLIC blob_extra(1 to 4) as Integer
PUBLIC blob_size(1 to 4) as Integer
PUBLIC timenow as Single
PUBLIC blob_position_x(1 To 20) as Integer
PUBLIC blob_position_y(1 To 20) as Integer
PUBLIC blob_lastseen(1 To 20) as Single
PUBLIC i as Byte
PUBLIC o as Byte


'CAT DATA
PUBLIC cat_i as Byte
PUBLIC cat_visible(1 to 4) as Boolean
PUBLIC cat_x(1 to 4) as Integer
PUBLIC cat_y(1 to 4) as Integer
PUBLIC cat_freq(1 to 4) as Single
PUBLIC cat_lastseen(1 to 4) as Single

'RANDOM
PUBLIC teststring as BoundedString(255)

'SETTINGS 
PUBLIC movementthreshold as Integer = 40



Sub Main()
	Call putpin(25,0) 'led on

	CallTask "Comms", ComTaskStack
	
	call OpenI2C(0, 11, 12, 10)

	'CONFIGURE CAMERA
	i2c_OutData(1) = &H30
	i2c_OutData(2) = &H01
	i2c_Result = I2CCmd(0, &HB0, 2, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H00
	i2c_OutData(2) = &H00
	i2c_OutData(3) = &H00
	i2c_OutData(4) = &H00
	i2c_OutData(5) = &H00
	i2c_OutData(6) = &H00
	i2c_OutData(7) = &H00
	i2c_OutData(8) = &H90
	i2c_Result = I2CCmd(0, &HB0, 8, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H07
	i2c_OutData(2) = &H00
	i2c_OutData(3) = &H41
	i2c_Result = I2CCmd(0, &HB0, 3, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H1A
	i2c_OutData(2) = &H40
	i2c_OutData(3) = &H00
	i2c_Result = I2CCmd(0, &HB0, 3, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H33
	i2c_OutData(2) = &H03
	i2c_Result = I2CCmd(0, &HB0, 2, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H30
	i2c_OutData(2) = &H08
	i2c_Result = I2CCmd(0, &HB0, 2, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)
	
	i2c_OutData(1) = &H37
	i2c_Result = I2CCmd(0, &HB0, 1, i2c_OutData, 0, i2c_InData)
	Call delay(0.1)



do
	'GET CAMERA DATA
	i2c_OutData(1) = &H37
	i2c_Result = I2CCmd(0, &HB0, 1, i2c_OutData, 0, i2c_InData)
	i2c_Result = I2CCmd(0, &HB1, 0, i2c_OutData,12, i2c_InData)
		
	blob_x(1) = cint(i2c_InData(1))
	blob_y(1) = cint(i2c_InData(2))
	blob_extra(1) = cint(i2c_InData(3))
	blob_x(1) = blob_x(1) + Shl(blob_extra(1) And &H30, 4)
	blob_y(1) = blob_y(1) + Shl(blob_extra(1) And &Hc0, 2) 
	
	blob_x(2) = cint(i2c_InData(4))
	blob_y(2) = cint(i2c_InData(5))
	blob_extra(2) = cint(i2c_InData(6))
	blob_x(2) = blob_x(2) + Shl(blob_extra(2) And &H30, 4)
	blob_y(2) = blob_y(2) + Shl(blob_extra(2) And &Hc0, 2)
	
	blob_x(3) = cint(i2c_InData(7))
	blob_y(3) = cint(i2c_InData(8))
	blob_extra(3) = cint(i2c_InData(9))
	blob_x(3) = blob_x(3) + Shl(blob_extra(3) And &H30, 4)
	blob_y(3) = blob_y(3) + Shl(blob_extra(3) And &Hc0, 2)
	
	blob_x(4) = cint(i2c_InData(10))
	blob_y(4) = cint(i2c_InData(11))
	blob_extra(4) = cint(i2c_InData(12))
	blob_x(4) = blob_x(4) + Shl(blob_extra(4) And &H30, 4)
	blob_y(4) = blob_y(4) + Shl(blob_extra(4) And &Hc0, 2)
	
	timenow = Timer()
	
	For blob_i = 1 To 4
		if &#40;blob_x&#40;blob_i&#41; <> 1023&#41; AND &#40;blob_y&#40;blob_i&#41; <> 1023&#41; then
			Dim match as Byte = 0
			For i = 1 To UBound&#40;blob_position_x&#41;
				o = &#40;UBound&#40;blob_position_x&#41; - &#40;i-1&#41;&#41;
				if match = 0 Then
					if nearby&#40;blob_x&#40;blob_i&#41;,blob_position_x&#40;o&#41;,blob_y&#40;blob_i&#41;,blob_position_y&#40;o&#41;,movementthreshold&#41; = True then
						match = o
					end if
				end if
			Next i
		
			Dim frequency as Single
			if match <> 0 then
				blob_position_x&#40;match&#41; = blob_x&#40;blob_i&#41;
				blob_position_y&#40;match&#41; = blob_y&#40;blob_i&#41;
				frequency = &#40;timenow - blob_lastseen&#40;match&#41;&#41;
				
				if frequency > 0.0075 And frequency < 0.0175 then '0.0125
					cat_visible&#40;1&#41; = True
					cat_x&#40;1&#41; = blob_position_x&#40;match&#41;
					cat_y&#40;1&#41; = blob_position_y&#40;match&#41;
					cat_lastseen&#40;1&#41; = timenow
				end if
				if frequency > 0.020 And frequency < 0.030 then '0.025
					cat_visible&#40;2&#41; = True
					cat_x&#40;2&#41; = blob_position_x&#40;match&#41;
					cat_y&#40;2&#41; = blob_position_y&#40;match&#41;
					cat_lastseen&#40;2&#41; = timenow
				end if
				if frequency > 0.045 And frequency < 0.055 then '0.050
					cat_visible&#40;3&#41; = True
					cat_x&#40;3&#41; = blob_position_x&#40;match&#41;
					cat_y&#40;3&#41; = blob_position_y&#40;match&#41;
					cat_lastseen&#40;3&#41; = timenow
				end if
				if frequency > 0.095 And frequency < 0.105 then  '0.0100
					cat_visible&#40;4&#41; = True
					cat_x&#40;4&#41; = blob_position_x&#40;match&#41;
					cat_y&#40;4&#41; = blob_position_y&#40;match&#41;
					cat_lastseen&#40;4&#41; = timenow
				end if
				blob_lastseen&#40;match&#41; = timenow
			else
				Call shiftarrayleft&#40;&#41;
				blob_position_x&#40;20&#41; = blob_x&#40;blob_i&#41;
				blob_position_y&#40;20&#41; = blob_y&#40;blob_i&#41;
			end if
		end if
	Next blob_i
	
	
        'timeout cat visible readings
	For cat_i = 1 To 4
		if &#40;timenow - cat_lastseen&#40;cat_i&#41;&#41; > 0.2 then
			cat_visible&#40;cat_i&#41; = False
			cat_x&#40;cat_i&#41; = 0
		end if
	Next cat_i
	
	'do we want to smooth the x and y data? and lowpass filter it?	
loop

End Sub


Public sub shiftarrayleft&#40;&#41; 
	'shift arrays left FIFO
	blob_position_x&#40;1&#41; = blob_position_x&#40;2&#41;
	blob_position_x&#40;2&#41; = blob_position_x&#40;3&#41;
	blob_position_x&#40;3&#41; = blob_position_x&#40;4&#41;
	blob_position_x&#40;4&#41; = blob_position_x&#40;5&#41;
	blob_position_x&#40;5&#41; = blob_position_x&#40;6&#41;
	blob_position_x&#40;6&#41; = blob_position_x&#40;7&#41;
	blob_position_x&#40;7&#41; = blob_position_x&#40;8&#41;
	blob_position_x&#40;8&#41; = blob_position_x&#40;9&#41;
	blob_position_x&#40;9&#41; = blob_position_x&#40;10&#41;
	blob_position_x&#40;10&#41; = blob_position_x&#40;11&#41;
	blob_position_x&#40;11&#41; = blob_position_x&#40;12&#41;
	blob_position_x&#40;12&#41; = blob_position_x&#40;13&#41;
	blob_position_x&#40;13&#41; = blob_position_x&#40;14&#41;
	blob_position_x&#40;14&#41; = blob_position_x&#40;15&#41;
	blob_position_x&#40;15&#41; = blob_position_x&#40;16&#41;
	blob_position_x&#40;16&#41; = blob_position_x&#40;17&#41;
	blob_position_x&#40;17&#41; = blob_position_x&#40;18&#41;
	blob_position_x&#40;18&#41; = blob_position_x&#40;19&#41;
	blob_position_x&#40;19&#41; = blob_position_x&#40;20&#41;
	
	blob_position_y&#40;1&#41; = blob_position_y&#40;2&#41;
	blob_position_y&#40;2&#41; = blob_position_y&#40;3&#41;
	blob_position_y&#40;3&#41; = blob_position_y&#40;4&#41;
	blob_position_y&#40;4&#41; = blob_position_y&#40;5&#41;
	blob_position_y&#40;5&#41; = blob_position_y&#40;6&#41;
	blob_position_y&#40;6&#41; = blob_position_y&#40;7&#41;
	blob_position_y&#40;7&#41; = blob_position_y&#40;8&#41;
	blob_position_y&#40;8&#41; = blob_position_y&#40;9&#41;
	blob_position_y&#40;9&#41; = blob_position_y&#40;10&#41;
	blob_position_y&#40;10&#41; = blob_position_y&#40;11&#41;
	blob_position_y&#40;11&#41; = blob_position_y&#40;12&#41;
	blob_position_y&#40;12&#41; = blob_position_y&#40;13&#41;
	blob_position_y&#40;13&#41; = blob_position_y&#40;14&#41;
	blob_position_y&#40;14&#41; = blob_position_y&#40;15&#41;
	blob_position_y&#40;15&#41; = blob_position_y&#40;16&#41;
	blob_position_y&#40;16&#41; = blob_position_y&#40;17&#41;
	blob_position_y&#40;17&#41; = blob_position_y&#40;18&#41;
	blob_position_y&#40;18&#41; = blob_position_y&#40;19&#41;
	blob_position_y&#40;19&#41; = blob_position_y&#40;20&#41;
	
	blob_lastseen&#40;1&#41; = blob_lastseen&#40;2&#41;
	blob_lastseen&#40;2&#41; = blob_lastseen&#40;3&#41;
	blob_lastseen&#40;3&#41; = blob_lastseen&#40;4&#41;
	blob_lastseen&#40;4&#41; = blob_lastseen&#40;5&#41;
	blob_lastseen&#40;5&#41; = blob_lastseen&#40;6&#41;
	blob_lastseen&#40;6&#41; = blob_lastseen&#40;7&#41;
	blob_lastseen&#40;7&#41; = blob_lastseen&#40;8&#41;
	blob_lastseen&#40;8&#41; = blob_lastseen&#40;9&#41;
	blob_lastseen&#40;9&#41; = blob_lastseen&#40;10&#41;
	blob_lastseen&#40;10&#41; = blob_lastseen&#40;11&#41;
	blob_lastseen&#40;11&#41; = blob_lastseen&#40;12&#41;
	blob_lastseen&#40;12&#41; = blob_lastseen&#40;13&#41;
	blob_lastseen&#40;13&#41; = blob_lastseen&#40;14&#41;
	blob_lastseen&#40;14&#41; = blob_lastseen&#40;15&#41;
	blob_lastseen&#40;15&#41; = blob_lastseen&#40;16&#41;
	blob_lastseen&#40;16&#41; = blob_lastseen&#40;17&#41;
	blob_lastseen&#40;17&#41; = blob_lastseen&#40;18&#41;
	blob_lastseen&#40;18&#41; = blob_lastseen&#40;19&#41;
	blob_lastseen&#40;19&#41; = blob_lastseen&#40;20&#41;
End sub



Public Sub comms&#40;&#41;
	do			
		debug.print "cat1&#58;"&cstr&#40;cat_x&#40;1&#41;&#41; & "    " & "cat2&#58;"&cstr&#40;cat_x&#40;2&#41;&#41; & "    " & "cat3&#58;"&cstr&#40;cat_x&#40;3&#41;&#41; & "    " & "cat4&#58;"&cstr&#40;cat_x&#40;4&#41;&#41;
		call sleep&#40;0.1&#41;
	loop
End Sub
Top
stevech
Posts: 715
Joined: 22 February 2006, 20:56 PM

Post by stevech »

Doable but not easy to code, have each IR emitter transmit uniquely.
Ways I've seen this done:
1) Receiver on (cat) receives a beacon sent wirelessly house-wide. Each IR emitter then uses a time offset from the beacon. In analyzing the received IR blip, the time offset from the beacon can be used to discriminate.

2) Same concept, but beacon is the vertical sync of the TV raster, if standard TV signals are used.

3) IR emitter sends a serial bit stream, like start bit, some data bits. Receiver's data analysis software can find this bit stream and decode it.

Maybe easier than cameras and IR is a $4 RF transceiver PC board (thumbnail sized), as from Dorji via Tindie (China) or the more expensive HopeRF. These cover a typical home with a small antenna, at either 433MHz or 868/900MHz bands. Connect radio by SPI to a small microprocessor w/3V battery and software that does power-down sleeping. I'm using these radios for a small network. The radios us GFSK modulation (not cheap on-off keying), have hardware CRC error checks, radio addressing, etc. Lots of freeware for them - the two mentioned above both use SiLabs 4432 chips on the little PCB. So every radio has a transmitted ID.

Another scheme is to put receivers at many places, as mileposts. The IR or RF signal strength at a milepost is the location estimate.

Lastly: I've seen position tracking with IR emitters, with 2 or 3 cameras per room and photogrammetry on to determine X, Y, Z location given the camera lens focal length is known, as are the camera pointing angles. One I saw had the IR emitters pulse-code their ID. Signal processing of the rasterized video allowed an FFT to find the data stream.

But the little radios are by far easier.
Top
sturgessb
Posts: 287
Joined: 25 April 2008, 6:34 AM
Location: Norwich, UK

Post by sturgessb »

Thanks, but I think I have it.

Just by averaging the delta times I can now reliably identify up to 4 beacons at once and get X/Y cords to a resolution of 2cm in the room. Happy with that!

I might ditch the 433mhz radio proximity backup as it just seems like overkill. We'll see how much headroom it takes.

Will post more details when its all a bit more polished.

B
Top
DocJC
Posts: 112
Joined: 16 March 2006, 6:23 AM
Location: Cleveland, OH
Contact:
Contact DocJC

Post by DocJC »

Sounds like an interesting project.

I wondered why you decided to have the IR emitters at multiples of the base frequency? Special reason, or just did it that way?

When doing data discrimination it is often easier if the signals are NOT multiples of each other, as if one is doing FFT/DFT analysis there is often some energy at the harmonic from the lower frequency emitter. IIRC the classic DTMF frequencies were selected so as not to be harmonically related.

Additionally, if the camera is outputting 200 images / sec, then I would think it would be easier to reliably detect the lower frequency emitters, (Shannon-Nyquist Theorem, etc., similar to the highest frequency one can measure and easily recognize on a digital O'scope, given its sampling rate). Ignoring the above comment about harmonics, for now, one might be able to detect 10, 15, 20, and 25 Hz signals just as well as the current frequencies.

Anyway, just wondered about that aspect of your project.

JC
Top
Post Reply

Return to “ZBasic Language”