WiiMote IR Camera with Lego NXT Brick – LeJOS

This previous post talks about how to connect a IR Camera from a WiiMote to an Arduino.

There’s also this one, talking about how to connect it to the .NET micro framework, FEZ Domino board (which also has hardware details on how to extract the mentionned camera and solder it on its own little board…).

It’s therefore time I wrote a quick post, on how to interface this wonderful camera with a Lego NXT Brick, programmed in Java thanks to LeJOS !

WiiMote IR Camera and Lego NXT - LeJOS

IR LED Tracking

Hardware

Using the previously created board for the IR Camera, BUT it turns out (after a few frustrating hours…) that the two 2.2KOhms pullup resistors on the I2C bus are too much !

The NXT (you can google it up for more details) is slightly peculiar on its I2C requirements, in that the pull up resistors have to be quite high in value (82KOhms – from the docs). This is apparently because it has some 4.3KOhmns internal resistors of its own, in order to protect the ports.

So I’ve cut out the original resistors of my board (it turns out the Arduino can read the camera fine without them anyhow…. Arduino is reaaaaly great in its simplicity and tolerance ! Never managed to burn one, it’s not fussy with its I2C bus, etc. …) and soldered two 100kOhms ones….

Update 10.06.2011 : After more tinkering due to interference between the I2C bus and some close by motors, I managed to lower the pullup resistors to 33kOhms. Below that (tested with 20kOhms) it doesn’t work anymore…  The lower the value, the more reliable the line apparently. Add to this the change in speed to 100kHz instead of 400kHz and here we have a less error prone bus.

I also had to move the board to a different port than the one on which the slave Arduino board is, but more on this later, once I will have investigated the issue…

Software

Here’s the Java / LeJOS class that reads the data from the WiiMote IR Camera.

The only tricky thing worth mentionning regarding porting this code from Arduino to Java, is that in Java bytes are signed, so you need to do the   _dataBuff[i] & 0xFF   trick to unsign them…

You have 3 classes:

1. IRCamera.java – the main one that communicates through I2C with the camera
2. PID.java – a simple implementation of a PID control algorithm
3. FollowBehaviour.java – a behaviour class that gets activated when the camera sees something and tries to track that object…

IRCamera.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB;

import lejos.nxt.I2CPort;
import lejos.nxt.SensorPort;
import lejos.util.Delay;
import trandi.nxt.tigerTankBB.util.Display;
import trandi.nxt.tigerTankBB.util.MyI2CSensor;
import trandi.nxt.tigerTankBB.util.MyThread;

public class IRCamera {
    public static final int X_MIN = 0;
    public static final int X_MAX = 1024;
    public static final int X_MID = (X_MAX + X_MIN) / 2;
    public static final int X_DELTA = X_MAX - X_MIN;

    private static final byte DATA_SIZE = 13;
    private final static byte CAMERA_BOARD_I2C_ADDRESS = 0xB0 >> 1;

    public static final IRCamera instance = new IRCamera(SensorPort.S4, CAMERA_BOARD_I2C_ADDRESS);

    private final MyI2CSensor _i2cSensor;
    private final byte[] _dataBuff = new byte[DATA_SIZE];
    private final int[] _unsignedDataBuff = new int[DATA_SIZE];

    private Blob _blob1 = null;
    private Blob _blob2 = null;
    private Blob _blob3 = null;
    private Blob _blob4 = null;
    private long _lastUpdate = 0;
    private String _error;

    // structure with data from the camera
    public static class Blob {
        public int x;
        public int y;
        public int size;

        @Override
        public String toString() {
            return "X: " + x + " - Y: " + y + " - S: " + size;
        }
    }

    private IRCamera(I2CPort port, byte address){
        _i2cSensor = new MyI2CSensor(port, address);

// INIT the camera sensor
//i2c(new byte[] { 0x30, 0x01 }, true, 10);
//i2c(new byte[] { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90 }, true, 10); // sensitivity part 1
//i2c(new byte[] { 0x07, 0x00, 0x41 }, true, 10); // sensitivity part 2
//i2c(new byte[] { 0x1A, 0x40, 0x00 }, true, 10); // sensitivity part 3
//i2c(new byte[] { 0x33, 0x03 }, true, 10); // sets the mode of Output Format : Short (1) - 9bytes, Medium(3) - 4x3 bytes or Long(5) - 2x8 bytes    http://wiki.wiimoteproject.com/IR_Sensor
//i2c(new byte[] { 0x30, 0x08 }, true, 10);


_i2cSensor.sendData(0x30, (byte)0x01);
_i2cSensor.sendData(0x06, (byte)0x90);
_i2cSensor.sendData(0x08, (byte)0xC0);
_i2cSensor.sendData(0x1A, (byte)0x40);
_i2cSensor.sendData(0x33, (byte)0x03);
_i2cSensor.sendData(0x30, (byte)0x08);

Delay.msDelay(9);


        // start a new thread monitoring the camera board and updating the state
        new MyThread(){
            @Override
            public void run2() throws Exception{
                while(true) {
                    readData();

                    Display.setIRBlob(getBlob1());
                    Display.setInfo(_error);

                    // wait a little before next update
                    Delay.msDelay(50);
                }
            }
        }.start();
    }

    /**
     * Reads data from the I2C Wii IR Camera and transforms it into Blobs structures
     * @return The number of valid blobs read.
     */
    private synchronized int readData(){
// only necessary once in a "perfect" world
// BUT, because of EM interference the IR Camera gets OK when motors run nearby, hence requiring re-init every time (to be on the safe side)
init();

        int bytesRead = _i2cSensor.getData(0x36, _dataBuff, DATA_SIZE);
        Delay.msDelay(1);

        int res = 0;
        _error = null;
        if(bytesRead == DATA_SIZE) {
            // transform into UNSIGNED DATA
            for(byte i=0; i
            // have no idea why the 1st BLOB start at 1 not 0....
            _blob1 = extractBlob(1);
            if(_blob1 != null) res ++;
            _blob2 = extractBlob(4);
            if(_blob2 != null) res ++;
            _blob3 = extractBlob(7);
            if(_blob3 != null) res ++;
            _blob4 = extractBlob(10);
            if(_blob4 != null) res ++;
        }else{
            _error = String.valueOf(bytesRead);
        }

        _lastUpdate = System.currentTimeMillis();
        return res;
    }

    private Blob extractBlob(int offset) {
        Blob result = new Blob();
        result.x = _unsignedDataBuff[offset];
        result.y = _unsignedDataBuff[offset + 1];
        int extra = _unsignedDataBuff[offset + 2];
        result.x += (extra & 0x30) << 4;
        result.y += (extra & 0xC0) << 2;
        result.size = (extra & 0x0F);

        // VALID size < 15         if (result.size >= 15) return null;

        return result;
    }

    public Blob getBlob1() {
        return _blob1;
    }

    public Blob getBlob2() {
        return _blob2;
    }

    public Blob getBlob3() {
        return _blob3;
    }

    public Blob getBlob4() {
        return _blob4;
    }

    public long getLastUpdate(){
        return _lastUpdate;
    }
}

FollowBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.turret;

import trandi.nxt.tigerTankBB.IRCamera;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.Display;
import trandi.nxt.tigerTankBB.util.MyMotor;
import trandi.nxt.tigerTankBB.util.PID;
import trandi.nxt.tigerTankBB.util.Util;

public class FollowBehaviour extends AbstractBehaviour{
    private static final byte TEN_PERC_POWER = Util.PERC_DELTA / 20;

    private final MyMotor _turretMotor;

    public FollowBehaviour(MyMotor turretMotor) {
        super(BodyPart.TURRET, "Fol");

        _turretMotor = turretMotor;
    }

    public boolean takeControl() {
        // whenever we have found something with the IR camera
        return IRCamera.instance.getBlob1() != null;
    }

    @Override
    public void action2() {
        // try to follow the 1st blob found by the camera, such that it remains in the middle (coordinates : )
        long lastUpdate = 0;
        int xPos;
        float dt;

        PID pid = new PID(0.025f, 0.002f, 0.001f, IRCamera.X_DELTA / 2);
        pid.setPoint(IRCamera.X_MID); // keep the IR target in the middle
        while(continu()){
            // 1. wait for the camera to be read
            while(IRCamera.instance.getLastUpdate() == lastUpdate) Thread.yield();
            dt = (IRCamera.instance.getLastUpdate() - lastUpdate) / 1000f; // in seconds
            lastUpdate = IRCamera.instance.getLastUpdate();
            if(IRCamera.instance.getBlob1() != null){
                xPos = IRCamera.instance.getBlob1().x;

                // 2. do a Kalman filter

                // 3. do a PID & adjust the speed of the turret
                _turretMotor.setSpeed(Util.constrainPerc(Util.PERC_MID + pid.doPID(xPos, dt)));

                // 4. check the LIMITS of the turret
            }
        }
    }

    @Override
    public void suppress2() {
        _turretMotor.stop();
    }
}

PID.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.util;

public class PID {
    private final float P, I, D, MAX_ERR;
    private int _setPoint;
    private float _err, _lastErr, _integralErr, _result;

    public PID(float P, float I, float D, float maxErrorAllowed){
        this.P = P;
        this.I = I;
        this.D = D;
        MAX_ERR = Math.abs(maxErrorAllowed);
    }

    public void setPoint(int newSP){
        _setPoint = newSP;
    }

    /**
     * @param procVar
     * @param dt in seconds
     */
    public int doPID(int procVar, float dt){
        _err = Util.constrain(procVar - _setPoint, -MAX_ERR, MAX_ERR);
        _integralErr = Util.constrain(_integralErr + _err * dt, -MAX_ERR, MAX_ERR);
        _result = P * _err + I * _integralErr + D * (_err - _lastErr) / dt;
        _lastErr = _err;

        return Math.round(_result);
    }
}