Wednesday, February 16, 2011

Distance

I think I have pretty conclusively shown that getting distance out of the accelerometers and the motionplus is not going to work.  What might work is using the IR camera - I now have a "sensor bar", which contains no sensors and is essentially a couple of banks of LEDs about 20cm apart  (three at each end of the bar, spaced by about 1cm).  I haven't dismantled it, but I suspect they are just bright LEDs with an infra-red filter in front of them (you can see the actual LEDs by pointing a phone camera at them).  The code below checks whether two spots are visible, and if so it works out the apparent distance between them.  When the wiimote is close to the sensor bar, it is able to resolve the individual LEDs so it may pick up more than two spots - a metre away seems to be the "safe" distance where the two groups of three LEDs look like two large infra-red sources.

What we have found is that this works pretty well for distance measurements - placing the sensor bar on a desk and the wiimote on another desk a couple of metres away results in a stable measurement of the apparent separation between the two sources, and moving the wiimote a fairly short distance gives a change in this apparent separation which is also consistent and could be translated into a distance from the sensor bar with about 1cm accuracy (i.e. moving the wiimote forward or back about 1cm causes a change in the apparent separation of one camera pixel).  For example, with d = 1m the apparent separation might be 160 pixels, and with d=1.1m the separation might be 150 pixels (apparently closer together, because the light sources are further away).  With some calibration, and the selection of alternative light sources and positions, this could be used to give accurate position measurement.



#!/usr/bin/python

import cwiid
from time import sleep
from math import sqrt

print "Press 1+2 on the Wiimote now"
wiimote = cwiid.Wiimote()
wiimote.rumble = 1
sleep(0.2)
wiimote.rumble = 0
print "OK"

# Rumble to indicate a connection
wiimote.enable(cwiid.FLAG_MESG_IFC)
wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_IR 

loop = True
distance = 0
last_distance = 0
last_ir = ""

while (loop):
    sleep(0.01)
    messages = wiimote.get_mesg()
    for mesg in messages:
        # Button:
        if mesg[0] == cwiid.MESG_BTN:
            if mesg[1] & cwiid.BTN_HOME:
                print "Ending Program"
                loop = False
        # Infra-red
        elif mesg[0] == cwiid.MESG_IR:
            sources = mesg[1]
            output = "IR:  "
            spots = []
            for spot in sources:
                if spot:
                    # output = output + "{0}".format(spot)
                    output = output + \
                        "{0} {1:12}".format(spot["size"], spot["pos"])
                    spots.append(spot)
            if len(spots) == 2:
                deltax = spots[0]["pos"][0] - spots[1]["pos"][0]
                deltay = spots[0]["pos"][1] - spots[1]["pos"][1]
                distance = int(sqrt(deltax * deltax + deltay * deltay))
                if distance != last_distance:
                    print "Distance : ", distance
                    last_distance = distance
            else:
                if output != last_ir:
                    print output
                    last_ir = output
        else:
            print mesg