Tiger 1 BB airsoft RC Tank – V3

This is the sequel of 2 other posts describing previous work on this project:

• version 2 of the tank attempt
• and the initial work which dealt more with the low level building.

It also builds on this previous post, which is all about interfacing the WiiMote IR Camera with the NXT and tracking an IR LED with a Lego motor.

11Jun2011 – FINALLY !!!

Everything seems to work nicely together now it not only tracks and locks on the target, but it finally shoots PLASTIC PELLETS !

10Jun2011

Eureka !!!

After having bough a new motor controller to replace my own home made one (remember I had an issue whereby once I fired the gun, the IR Camera won’t work anymore … lol…) I realised that the controller was NOT the problem.

SparkFun TB6612FNG Motor Controller - unused as finally that was NOT the issue...

I’ve played with the pullup resistors on the I2C bus, reducing them from 100kOhms to 33kOhms (less than that the NXT won’t work !) and reduced the speed of the bus from 400kHz to 100kHz… it seemed to have improved things, as in the IR Camera would stop working after a few dozens seconds of running the motor as oposed to immediately before hand…

But still, the final solution came when I realised that due to electro magnetic interference the camera must reset itself, and it needed re-initialisation. So now, the IRCamera.java class, initialises the camera before EACH readData() !

This has required an optimisation of the init code, so that it takes no more than 10ms, which was quite straight forward (thanks Nintento, the Wiimote is really a good quality piece of kit !!!).

05Jun2011

Final videos, with improved target and some fixes in the code:

Here is the Java code running onthe Lego NXT brick.  There are new behavious classes, PID and some improvements since V2.

TigerTankBB.java
-------------------------------------------------------
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.comm.RConsole;
import trandi.nxt.tigerTankBB.IRCamera;
import trandi.nxt.tigerTankBB.util.Display;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.SlaveBoard.SlaveBoardThread;
import trandi.nxt.tigerTankBB.chassis.ChassisThread;
import trandi.nxt.tigerTankBB.turret.FireMonitorThread;
import trandi.nxt.tigerTankBB.turret.TurretThread;
import trandi.nxt.tigerTankBB.util.BatteryMonitorThread;

public class TigerTankBB {

/**
* Entry point for the whole project.
*/
public static void main(String[] args) {
//RConsole.openBluetooth(0);

try{
// 1. ALWAYS listen for ESCAPE !
Button.ESCAPE.addButtonListener(
new ButtonListener() {
public void buttonPressed(Button b) {
System.exit(0);
}

public void buttonReleased(Button b) {
throw new UnsupportedOperationException("Not supported yet.");
}
});

// 2. START the main threads, each independently controlling / monitoring one part of the tank
new Display.DisplayThread().start();
SlaveBoard.instance.new SlaveBoardThread().start();
IRCamera.instance.new IRCameraThread().start();

new BatteryMonitorThread().start();
new TurretThread().start();
new ChassisThread().start();
new FireMonitorThread().start();

}catch(Exception e){
Display.setInfo("Main");
Display.setException(e);
}
}
}

ChassisThread.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.chassis;

import lejos.robotics.subsumption.Behavior;
import trandi.nxt.tigerTankBB.util.MyArbitrator;
import trandi.nxt.tigerTankBB.util.MyThread;

public class ChassisThread extends MyThread {
@Override
public void run2() {
// stays in this method forever...
new MyArbitrator(new Behavior[]{ new StraightMovementsBehaviour(),
new ObstacleDetectedBehaviour(),
new FollowTargetBehaviour(),

new ManualBehaviour()
}).start();
}

@Override
public String id() {
return "CT";
}
}

FollowTargetBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.chassis;

import trandi.nxt.tigerTankBB.IRCamera;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.turret.TurretThread;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.PID;
import trandi.nxt.tigerTankBB.util.Util;

public class FollowTargetBehaviour extends AbstractBehaviour{
private static final int DELTA_DEG_ALIGNED = 60; //degrees withing which the gun is considered aligned
private static final int TARGET_DISTANCE_GOAL = 80; //distance in cm of the target at which the tank will try to get

public FollowTargetBehaviour(){
super(BodyPart.CHASSIS, "Flw");
}

public boolean takeControl() {
return IRCamera.instance.getBlob() != null;
}

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

PID pid = new PID(0.5f, 0.5f, 0f, IRCamera.X_DELTA / 2);
pid.setPoint(0); // keep gun aligned with the chassis
while(continu()){
if(System.currentTimeMillis() > nextUpdateDue){
dt = (System.currentTimeMillis() - nextUpdateDue) / 1000f + 0.010f; // in seconds
nextUpdateDue = System.currentTimeMillis() + 10; // do this loop every 10ms
pidValue = pid.doPID(TurretThread.turretMotor.getTachoCount(), dt);

// are we ALIGNED ?
if(Math.abs(TurretThread.turretMotor.getTachoCount()) < DELTA_DEG_ALIGNED){
// Try to keep the chassis at TARGET_DISTANCE_GOAL cm, so TARGET_DISTANCE_GOAL should get Util.PERC_MID. ONLY if it's aligned with the gun !
// the distance goes exactly from [Util.PERC_MIN .. Util.PERC_MAX].
speed = SlaveBoard.instance.getUSDist() + Util.PERC_MID - TARGET_DISTANCE_GOAL;

dir = Util.PERC_MID;
}else{
speed = Util.PERC_MID;

// set the rotation of the chassis according to the position of the gun
// make the dir start at 10%
dir = Util.PERC_MID + pidValue + (pidValue < 0 ? -Util.TEN_PERC_POWER : Util.TEN_PERC_POWER);
// make the dir stop at 70%
dir = Util.constrain(dir, Util.PERC_MID - 9*Util.TEN_PERC_POWER, Util.PERC_MID + 9*Util.TEN_PERC_POWER);
}

SlaveBoard.instance.sendCmd(speed, dir, false);
}

Thread.yield();
}
}

@Override
public void suppress2() {
// stop motors
SlaveBoard.instance.sendCmd(Util.PERC_MID, Util.PERC_MID, false);
}
}

ManualBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.chassis;

import lejos.util.Delay;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;

public class ManualBehaviour extends AbstractBehaviour{

public ManualBehaviour(){
super(BodyPart.CHASSIS, "Man");
}

public boolean takeControl() {
return SlaveBoard.instance.isManual();
}

@Override
public void action2() {
// do not release control until we manually say so...
// do absolutely NOTHING, as the slave board controls the motors directly !
while(continu() && SlaveBoard.instance.isManual()) {
Thread.yield();
Delay.msDelay(50);
}
}

@Override
public void suppress2() {
// nothing to do
}
}

ObstacleDetectedBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.chassis;

import lejos.util.Delay;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.Util;

public class ObstacleDetectedBehaviour extends AbstractBehaviour{

public ObstacleDetectedBehaviour(){
super(BodyPart.CHASSIS, "Obs");
}

public boolean takeControl() {
return SlaveBoard.instance.getUSDist() < 60;
}

@Override
public void action2() {
int dist;

while(continu()) {
dist = SlaveBoard.instance.getUSDist();
if(dist < 40){
// dangerously cloase to something -> go in reverse
SlaveBoard.instance.sendCmd(Util.PERC_MID - Util.PERC_DELTA / 5,
Util.map(dist, 0, 40, Util.PERC_MAX, Util.PERC_MID), false);
}else {
// getting closer, but still enough room to turn
SlaveBoard.instance.sendCmd(Util.PERC_MID + Util.PERC_DELTA / 5,
Util.map(dist, 40, 60, Util.PERC_MAX, Util.PERC_MID), false);
}

Thread.yield();
Delay.msDelay(20);
}
}

@Override
public void suppress2() {
// stop motors
SlaveBoard.instance.sendCmd(Util.PERC_MID, Util.PERC_MID, false);
}
}

StraightMovementsBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.chassis;

import lejos.util.Delay;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.Util;

public class StraightMovementsBehaviour extends AbstractBehaviour{

public StraightMovementsBehaviour(){
super(BodyPart.CHASSIS, "Str");
}

public boolean takeControl() {
return true;
}

@Override
public void action2() {
while(continu()) {
// just go straight at 20% of max throttle
SlaveBoard.instance.sendCmd(Util.PERC_MID + Util.PERC_DELTA / 5, Util.PERC_MID, false);

Thread.yield();
Delay.msDelay(20);
}
}

@Override
public void suppress2() {
// stop motors
SlaveBoard.instance.sendCmd(Util.PERC_MID, Util.PERC_MID, false);
}
}

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;
import trandi.nxt.tigerTankBB.util.Util;

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;
// when a blog dissapears, report the latest valid data for this number of readings
// this "inertia" is usefull, to stop behaviours jumping to conclusions about the IR target having dissapeared, when it can be just a temporary problem
private final static byte KEEP_ALIVE_COUNT = 30;

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;

// used internally to keep the latest valid Blob "alive" for another few readings
private byte age = 0;

@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);
Delay.msDelay(10);
_i2cSensor.sendData(0x30, (byte)0x08);
Delay.msDelay(10);
_i2cSensor.sendData(0x06, (byte)0x90);
Delay.msDelay(10);
_i2cSensor.sendData(0x08, (byte)0xC0);
Delay.msDelay(10);
_i2cSensor.sendData(0x1A, (byte)0x40);
Delay.msDelay(10);
_i2cSensor.sendData(0x33, (byte)0x03);
Delay.msDelay(10);
_i2cSensor.sendData(0x30, (byte)0x08);
Delay.msDelay(10);

Delay.msDelay(100);
}

/**
* 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(){
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<DATA_SIZE; i++) _unsignedDataBuff[i] = Util.unsign(_dataBuff[i]);

// have no idea why the 1st BLOB start at 1 not 0....
_blob1 = extractBlob(1, _blob1);
if(_blob1 != null) res ++;
_blob2 = extractBlob(4, _blob2);
if(_blob2 != null) res ++;
_blob3 = extractBlob(7, _blob3);
if(_blob3 != null) res ++;
_blob4 = extractBlob(10, _blob4);
if(_blob4 != null) res ++;
}else{
_error = String.valueOf(bytesRead);
}

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

private Blob extractBlob(int offset, Blob oldBlob) {
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) {
if(oldBlob != null && oldBlob.age++ > KEEP_ALIVE_COUNT) return null; // enough with assisted life, it's time to tell the truth !
else return oldBlob; // keep returning the latest valid blob for a while
}

return result;
}

/**
* Returns the 1st available Blob. For now we're interested in only 1 !
*/
public Blob getBlob() {
if(_blob1 != null) return _blob1;
else if(_blob2 != null) return _blob2;
else if(_blob3 != null) return _blob3;
else if(_blob4 != null) return _blob4;

return null;
}

public long getLastUpdate(){
return _lastUpdate;
}

/**
* Thread monitoring the camera board and updating the state
*/
public class IRCameraThread extends MyThread {
@Override
public void run2() throws Exception{
while(true) {
readData();

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

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

@Override
public String id() {
return "ICMT";
}
}
}

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

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

/**
* Singleton.
* @author trandi
*/
public class SlaveBoard {
private static final byte CMD_SIZE = 6;
private static final byte DATA_SIZE = CMD_SIZE + 1; //1 last byte for the US sensor distance
private static final byte SIGNAL_LOST = (byte)253;
private final static byte SLAVE_BOARD_I2C_ADDRESS = 0x08;

public static final SlaveBoard instance = new SlaveBoard(SensorPort.S1, SLAVE_BOARD_I2C_ADDRESS);

private final MyI2CSensor _i2cSensor;

// state of the commands
volatile private int _speed;
volatile private int _dir;
volatile private int _vertical;
volatile private int _horiz;
volatile private boolean _manual;
volatile private boolean _fire;
volatile private String _error;
volatile private int _usDist;

private byte[] _cmdBuff = new byte[CMD_SIZE];
private byte[] _dataBuff = new byte[DATA_SIZE];
private final int[] _unsignedDataBuff = new int[DATA_SIZE];

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

private void readCmd() {
int bytesRead = _i2cSensor.getData(0x00, _dataBuff, DATA_SIZE);

if(bytesRead == DATA_SIZE){
// transform into UNSIGNED DATA
for(byte i=0; i<DATA_SIZE; i++) _unsignedDataBuff[i] = Util.unsign(_dataBuff[i]);

if(_dataBuff[0] == SIGNAL_LOST) {
_error = "SIG LOST";
} else {
_error = null;

_speed = _unsignedDataBuff[0];
_dir = _unsignedDataBuff[1];
_vertical = _unsignedDataBuff[2];
_horiz = _unsignedDataBuff[3];
_manual = Util.percToBool(_unsignedDataBuff[4]);
_fire = Util.percToBool(_unsignedDataBuff[5]);
}

// the US sensor is independent of the Radio data
_usDist = _unsignedDataBuff[6];
}else{
_error = String.valueOf(bytesRead);
}
}

public void sendCmd(int speed, int dir, boolean fire){
_cmdBuff[0] = Util.sign(Util.constrainPerc(speed));
_cmdBuff[1] = Util.sign(Util.constrainPerc(dir));
_cmdBuff[2] = Util.sign(Util.boolToPerc(fire));
_cmdBuff[3] = 0;
_cmdBuff[4] = 0;
_cmdBuff[5] = 0; // light
_i2cSensor.sendData(0x00, _cmdBuff, _cmdBuff.length);
}

public int getDir() {
return _dir;
}

public boolean isFire() {
return _fire;
}

public int getHoriz() {
return _horiz;
}

public boolean isManual() {
return _manual;
}

public int getSpeed() {
return _speed;
}

public int getVertical() {
return _vertical;
}

public int getUSDist() {
return _usDist;
}

public String getError(){
return _error;
}

/**
* Thread monitoring the slave board, updating the state when new commands arrive
*/
public class SlaveBoardThread extends MyThread {
@Override
public void run2() throws Exception{
while(true) {
// reads the data and updates the state
readCmd();

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

@Override
public String id() {
return "SBMT";
}
}
}

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

import trandi.nxt.tigerTankBB.util.*;
import lejos.nxt.Sound;
import lejos.util.Delay;
import trandi.nxt.tigerTankBB.IRCamera;
import trandi.nxt.tigerTankBB.IRCamera.Blob;

public class FireMonitorThread extends MyThread {
private Blob _irBlob;
private long _lockStartTime = 0;
private int _prevVol;

@Override
public void run2() throws Exception {
while(true){
_irBlob = IRCamera.instance.getBlob();
if(_irBlob != null){
// if the target is within 10% of the center, then it's locked
if(Math.abs(_irBlob.x - IRCamera.X_MID) > IRCamera.X_DELTA / 10) _lockStartTime = System.currentTimeMillis();

// if the target has been locked for long enough, can FIRE
if(System.currentTimeMillis() - _lockStartTime > 3000) {
_prevVol = Sound.getVolume();
Sound.setVolume(Sound.VOL_MAX);
Sound.buzz();
Sound.setVolume(_prevVol);
_lockStartTime = System.currentTimeMillis();
}
}

Thread.yield();
Delay.msDelay(50);
}
}

@Override
public String id() {
return "FMT";
}
}

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

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

public class FollowBehaviour extends AbstractBehaviour{
public FollowBehaviour() {
super(BodyPart.TURRET, "Fol");
}

public boolean takeControl() {
// whenever we have found something with the IR camera
return IRCamera.instance.getBlob() != 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.1f, 0.02f, 0.01f, 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.getBlob() != null){
xPos = IRCamera.instance.getBlob().x;

// do a PID & adjust the speed of the turret
TurretThread.turretMotor.setSpeed(Util.constrainPerc(Util.PERC_MID - pid.doPID(xPos, dt)));
}
}
}

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

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

import lejos.util.Delay;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;

public class ManualBehaviour extends AbstractBehaviour{
public ManualBehaviour() {
super(BodyPart.TURRET, "Man");
}

public boolean takeControl() {
return SlaveBoard.instance.isManual();
}

@Override
public void action2() {
// do not release control until we manually say so...
// do the turret control
while(continu() && SlaveBoard.instance.isManual()) {
TurretThread.turretMotor.setSpeed(SlaveBoard.instance.getHoriz());
Thread.yield();
Delay.msDelay(50);
}
}

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

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

import lejos.util.Delay;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.Util;

public class SearchBehaviour extends AbstractBehaviour{
private static final int SPEED_DELTA = 5 * Util.TEN_PERC_POWER;
private int _speed = Util.PERC_MID - SPEED_DELTA;

public SearchBehaviour() {
super(BodyPart.TURRET, "Src");
}

public boolean takeControl() {
// by default this is the behaviour that becomes active if nothing else special happens
return true;
}

@Override
public void action2() {
// keeps moving the turrent left and right
while(continu()){
if(! TurretThread.turretMotor.setSpeed(_speed)){
// if we can't set the speed -> we've reached the max angle, go back now...
if(_speed < Util.PERC_MID) _speed = Util.PERC_MID + SPEED_DELTA;
else _speed = Util.PERC_MID - SPEED_DELTA;
}

Thread.yield();
Delay.msDelay(50);
}
}

@Override
public void suppress2() {
TurretThread.turretMotor.setSpeed(Util.PERC_MID);
}
}

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

import lejos.nxt.MotorPort;
import lejos.robotics.subsumption.Behavior;
import trandi.nxt.tigerTankBB.util.MyArbitrator;
import trandi.nxt.tigerTankBB.util.MyMotor;
import trandi.nxt.tigerTankBB.util.MyThread;

public class TurretThread extends MyThread {
// After -700 or 700 degrees the turret reaches its back and HAS to stop.
// When powering on the NXT, the Turret's gun HAS to be facing FORWARD (that's where the 0 will be)
private static final int MAX_FREEDOM_DEG = 600;

public static final MyMotor turretMotor = new MyMotor(MotorPort.A, MAX_FREEDOM_DEG);

@Override
public void run2() {
// stays in this method forever...
new MyArbitrator(new Behavior[]{ new SearchBehaviour(),
new FollowBehaviour(),
new ManualBehaviour()
}).start();
}

@Override
public String id() {
return "TT";
}
}

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

import lejos.robotics.subsumption.Behavior;

public abstract class AbstractBehaviour implements Behavior{
protected enum BodyPart {CHASSIS, TURRET}

private boolean _continue = false;
private final BodyPart _bodyPart;
private final String _modeName;

protected abstract void action2();
protected abstract void suppress2();

protected AbstractBehaviour(BodyPart bodyPart, String modeName){
_bodyPart = bodyPart;
_modeName = modeName;
}

public void action() {
_continue = true;
switch(_bodyPart){
case CHASSIS: Display.setChassisMode(_modeName); break;
case TURRET: Display.setTurretMode(_modeName); break;
default: break;
}

action2();
}

public void suppress() {
_continue = false;
suppress2();
}

protected boolean continu(){
return _continue;
}
}

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

import lejos.nxt.Battery;
import lejos.nxt.Sound;
import lejos.util.Delay;

public class BatteryMonitorThread extends MyThread {
@Override
public void run2() throws Exception {
int voltage;
while(true){
voltage = Battery.getVoltageMilliVolt() / 100;
Display.setBatt(voltage + "dV");
if(voltage < 65) {
int prevVol = Sound.getVolume();
Sound.setVolume(Sound.VOL_MAX);
Sound.twoBeeps();
Sound.setVolume(prevVol);
}
Thread.yield();
Delay.msDelay(10000);
}
}

@Override
public String id() {
return "BMT";
}
}

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

import lejos.nxt.LCD;
import lejos.nxt.Sound;
import lejos.util.Delay;
import trandi.nxt.tigerTankBB.IRCamera.Blob;
import trandi.nxt.tigerTankBB.SlaveBoard;

public class Display {
private volatile static boolean _manualMode, _fire;
private volatile static int _speed, _dir, _vertical, _horiz, _usDist;
private volatile static String _batt, _info, _err, _chassisMode, _turretMode;
private volatile static Exception _exception;
private volatile static Blob _irBlob;

public static void setBatt(String batt){
_batt = batt;
}

private static void setDir(int dir) {
_dir = dir;
}

private static void setManualMode(boolean manualMode) {
_manualMode = manualMode;
}

private static void setFire(boolean fire) {
_fire = fire;
}

private static void setSpeed(int speed) {
_speed = speed;
}

private static void setHoriz(int horiz) {
_horiz = horiz;
}

private static void setVertical(int vertical) {
_vertical = vertical;
}

private static void setUSDist(int dist) {
_usDist = dist;
}

private static void setErr(String err) {
_err = err;
}

public static void setInfo(String info) {
_info = info;
}

public static void setException(Exception ex) {
_exception = ex;
refresh();
}

public static void setChassisMode(String chassisMode) {
_chassisMode = chassisMode;
}

public static void setTurretMode(String turretMode) {
_turretMode = turretMode;
}

public static void setIRBlob(Blob blob) {
_irBlob = blob;
}

private static synchronized void refresh() {
LCD.clear();
LCD.drawString("Bt:" + _batt, 0, 0);
LCD.drawString("Di" + _usDist, 9, 0);

LCD.drawString("M:" + _manualMode, 0, 1);
LCD.drawString("F:" + _fire, 9, 1);
LCD.drawString("S" + _speed, 0, 2);
LCD.drawString("D" + _dir, 4, 2);
LCD.drawString("V" + _vertical, 8, 2);
LCD.drawString("H" + _horiz, 12, 2);

LCD.drawString("Cm:" + _chassisMode, 0, 3);
LCD.drawString("Tm:" + _turretMode, 9, 3);

if(_irBlob != null){
try{
LCD.drawString("IR x" + _irBlob.x, 0, 4);
LCD.drawString("y" + _irBlob.y, 8, 4);
LCD.drawString("s" + _irBlob.size, 13, 4);
}catch(NullPointerException e){
// Can still have another thread updating and nullifying the blog, do nothing
}
}

if(_info != null) LCD.drawString("I:" + _info + ":" + System.currentTimeMillis(), 0, 5);
if( _err != null) LCD.drawString("E:" + _err + ":" + System.currentTimeMillis(), 0, 6);
if(_exception != null) {
try{
LCD.drawString("EX:" + _exception.getMessage() + Util.className(_exception.getClass()), 0, 7);
Sound.buzz();
}catch(NullPointerException e){
// Can still have another thread updating and nullifying the blog, do nothing
}
}

LCD.refresh();
}

/**
* monitors the RC commands and displays them
*/
public static class DisplayThread extends MyThread {
@Override
public void run2() throws Exception {
while(true){
Display.setSpeed(SlaveBoard.instance.getSpeed());
Display.setDir(SlaveBoard.instance.getDir());
Display.setVertical(SlaveBoard.instance.getVertical());
Display.setHoriz(SlaveBoard.instance.getHoriz());
Display.setManualMode(SlaveBoard.instance.isManual());
Display.setFire(SlaveBoard.instance.isFire());
Display.setUSDist(SlaveBoard.instance.getUSDist());
Display.setErr(SlaveBoard.instance.getError());
refresh();

Delay.msDelay(50);
}
}

@Override
public String id() {
return "DMT";
}
}

}

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

import lejos.robotics.subsumption.Behavior;

/**
* Arbitrator controls which behavior should become active in
* a behavior control system. Make sure to call start() after the
* Arbitrator is instantiated.<br>
*  This class has three major responsibilities: <br>
* 1. Determine the highest priority  behavior that returns <b> true </b> to takeControl()<br>
* 2. Suppress the active behavior if its priority is less than highest
* priority. <br>
* 3. When the action() method exits, it calls action() on the Behavior of highest priority.
* This Behavior becomes active.
* Chances since release 0.7:
* <br> 1. It assumes that a Behavior is no longer active when action() exits.
* <br> 2. Therefore it will only call suppress() on the Behavior whose action() method is running.
* <br> 3. It can make consecutives calls of action() on the same Behavior.
* <br> 4. Requirements for a Behavior:
* <br>    When suppress() is called, terminate  action() immediately.
* @see Behavior
*
*
* Need to change / FIX a few things...
*/
public class MyArbitrator {

private final int NONE = -1;
private Behavior[] _behavior;
// highest priority behavior that wants control ; set by start() usec by monitor
private int _highestPriority = NONE;
private int _active = NONE; //  active behavior; set by montior, used by start();
private boolean _returnWhenInactive;
/**
* Monitor is an inner class.  It polls the behavior array to find the behavior of hightst
* priority.  If higher than the active behavior, it calls active.suppress()
*/
private Monitor monitor;

/**
* Allocates an Arbitrator object and initializes it with an array of
* Behavior objects. The index of a behavior in this array is its priority level, so
* the behavior of the largest index has the highest the priority level.
* The behaviors in an Arbitrator can not
* be changed once the arbitrator is initialized.<BR>
* <B>NOTE:</B> Once the Arbitrator is initialized, the method start() must be
* called to begin the arbitration.
* @param behaviorList an array of Behavior objects.
* @param returnWhenInactive if <B>true</B>, the <B>start()</B> method returns when no Behavior is active.
*/
public MyArbitrator(Behavior[] behaviorList, boolean returnWhenInactive) {
_behavior = behaviorList;
_returnWhenInactive = returnWhenInactive;
monitor = new Monitor();
monitor.setDaemon(true);
}

/**
* Same as Arbitrator(behaviorList, false) Arbitrator start() never exits
* @param behaviorList An array of Behavior objects.
*/
public MyArbitrator(Behavior[] behaviorList) {
this(behaviorList, false);
}

/**
* This method starts the arbitration of Behaviors and runs an endless loop.  <BR>
* Note: Arbitrator does not run in a separate thread. The start()
* method will never return unless <br>1.  no action() method is running  and
* <br>2. no behavior  takeControl()
* returns <B> true </B>  and  <br> 3. the <i>returnWhenInacative </i> flag is true,
*/
public void start() {
monitor.start();
while (_highestPriority == NONE) {
Thread.yield();//wait for some behavior to take contro
}
while (true) {
synchronized (monitor) {
if (_highestPriority != NONE) {
_active = _highestPriority;

} else if (_returnWhenInactive) {// no behavior wants to run
monitor.more = false;//9 shut down monitor thread
return;
}
}// monotor released before action is called
if (_active != NONE) //_highestPrioirty could be NONE
{
_behavior[_active].action();
_active = NONE;  // no active behavior at the moment
}
Thread.yield();
}
}

/**
* Finds the highest priority behavior that returns <B>true </B> to takeControl();
* If this priority is DIFFERENT than the active behavior, it calls active.suppress().
* If there is no active behavior, calls suppress() on the most recently acrive behavior.
*/
private class Monitor extends Thread {

boolean more = true;
int maxPriority = _behavior.length - 1;

public void run() {
while (more) {
//FIND HIGHEST PRIORITY BEHAVIOR THAT WANTS CONTROL
synchronized (this) {
_highestPriority = NONE;
for (int i = maxPriority; i >= 0; i--) {
if (_behavior[i].takeControl()) {
_highestPriority = i;
break;
}
}
int active = _active;// local copy: avoid out of bounds error in 134
if (active != NONE && _highestPriority != active)
{
_behavior[active].suppress();
}
}// end synchronize block - main thread can run now
Thread.yield();
}
}
}
}

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

import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import lejos.nxt.SensorConstants;
import lejos.util.Delay;

/**
* Improvements:
* - default config
* - clearer and more complete return codes for sendData
* - synchronize send and get Data, as you can't do both at the same time !
*
* @author trandi
*/
public class MyI2CSensor extends I2CSensor{
private final I2CPort _port;
private int _address;

public MyI2CSensor(I2CPort port, int addr){
super(port, I2CPort.LEGO_MODE);
port.setType(SensorConstants.TYPE_HISPEED);
_port = port;

super.setAddress(addr);
_address = addr;
}

@Override
synchronized public int getData(int register, byte[] buf, int len) {
// ??? somehow it gets disabled....
_port.i2cEnable(I2CPort.LEGO_MODE);

int ret = _port.i2cStart(_address, register, 1, null, len, 0);

if (ret != 0) return  ret - 10;

while (_port.i2cBusy() != 0) Thread.yield();

ret = _port.i2cComplete(buf, len);
Delay.msDelay(1);//need to ensure at least a minimum break...

// don't ask WHY!!!!  But when it reports an error, most often it's not...
//if(ret == -3) ret = len;

return ret;
}

@Override
synchronized public int sendData(int register, byte[] buf, int len) {
int ret = _port.i2cStart(_address, register, 1, buf, len, 1);

if (ret != 0) return ret - 10;

while (_port.i2cBusy() != 0) Thread.yield();

ret = _port.i2cComplete(null, 0);
Delay.msDelay(1);//need to ensure at least a minimum break...

return ret;
}
}

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

import lejos.nxt.Motor;
import lejos.nxt.TachoMotorPort;
import lejos.util.Delay;

public class MyMotor {
private final static int REAL_MIN_SPEED = 0;
private final static int REAL_MAX_SPEED = 900;

private final Motor _baseMotor;
private final int   _maxRotationDeg;

public MyMotor(TachoMotorPort port, int maxRotationDeg){
_baseMotor = new Motor(port);
_baseMotor.smoothAcceleration(true);
//regulateSpeed(true);
_maxRotationDeg = maxRotationDeg;

// start a thread monitoring that the motor hasn't gone past the max
if(_maxRotationDeg != 0){
new MyThread() {
@Override
public String id() {
return "MMT";
}

@Override
public void run2() throws Exception {
while(true){
// if the motor is going int an invalid state stop now. If it is already but trying to get out, then let it continue...
if(! isValidSpeedForPosition(getSpeedPerc())) stop();

Thread.yield();
Delay.msDelay(10);
}
}
}.start();
}
}

public int getSpeedPerc(){
byte absSpeed = (byte)Util.map(_baseMotor.getSpeed(), REAL_MIN_SPEED, REAL_MAX_SPEED, 0, Util.PERC_DELTA / 2);
if(_baseMotor.isForward()) return Util.constrainPerc(Util.PERC_MID + absSpeed);
else return Util.constrainPerc(Util.PERC_MID - absSpeed);
}

public synchronized boolean setSpeed(int speedPerc){
if(! isValidSpeedForPosition(speedPerc)) return false;

speedPerc = Util.constrainPerc(speedPerc);
if(speedPerc == Util.PERC_MID) _baseMotor.flt();
else {
boolean forward = speedPerc > Util.PERC_MID;
int internalSpeedTarget = Util.map(Math.abs(speedPerc - Util.PERC_MID), 0, Util.PERC_DELTA / 2, REAL_MIN_SPEED, REAL_MAX_SPEED);

_baseMotor.setSpeed(internalSpeedTarget);

if(forward) _baseMotor.forward();
else _baseMotor.backward();
}

return true;
}

private boolean isValidSpeedForPosition(int speedPerc){
// if we are at the allowed limit, check that we're not requested to go beyond...
if(getTachoCount() <= -_maxRotationDeg && speedPerc < Util.PERC_MID) return false;
if(getTachoCount() >= _maxRotationDeg && speedPerc > Util.PERC_MID) return false;
return true;
}

public synchronized void stop(){
_baseMotor.stop();
}

public synchronized void flt(){
_baseMotor.flt();
}

public int getTachoCount(){
return _baseMotor.getTachoCount();
}
}

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

public abstract class MyThread {
private final Thread _thread = new Thread(){
@Override
public void run(){
try {
run2();
} catch (Exception ex) {
Display.setInfo("MyTh:" + id());
Display.setException(ex);
}
}
};

public abstract String id();
public abstract void run2() throws Exception;

public void start(){
_thread.start();
}
}

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);
}
}

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

public final class Util {
public final static int PERC_MIN = 0;
public final static int PERC_MAX = 200;
public final static int PERC_MID = (PERC_MAX + PERC_MIN) / 2;
public final static int PERC_DELTA = PERC_MAX - PERC_MIN;
public static final int TEN_PERC_POWER = Util.PERC_DELTA / 20;

public static boolean percToBool(int percentage){
return (percentage > (PERC_MID + PERC_DELTA / 4));
}

public static int boolToPerc(boolean bool){
return (bool ? PERC_MAX : PERC_MIN);
}

public static int map(int input, int inMin, int inMax, int outMin, int outMax) {
return Math.round(((float)(input - inMin) / (float)(inMax - inMin)) * (outMax - outMin) + outMin);
}

public static float constrain(float input, float min, float max){
float result = input;
if(input < min) result = min;
else if(input > max) result = max;

return result;
}

public static int constrain(int input, int min, int max){
int result = input;
if(input < min) result = min;
else if(input > max) result = max;

return result;
}

public static int constrainPerc(int input){
return constrain(input, PERC_MIN, PERC_MAX);
}

public static int unsign(byte val){
return val & 0xFF;
}

public static byte sign(int val){
byte result = (byte)(constrain(val, 0, 255) & 0xFF);
return result;
}

public static String className(Class<? extends Throwable> clazz){
// clazz.toString will give "class xx"
try{
int value = Integer.valueOf(clazz.toString().substring(6));

switch(value){
case 0 : return "JL_OBJECT";
case 1 : return "JL_THROWABLE";
case 2 : return "JL__ER";
case 3 : return "JL_OOMEM_ER";
case 4 : return "BOOLEAN";
case 5 : return "CHAR";
case 6 : return "FLOAT";
case 7 : return "DOUBLE";
case 8 : return "BYTE";
case 9 : return "SHORT";
case 10 : return "INT";
case 11 : return "LONG";
case 12 : return "VOID";
case 13 : return "ALJL_OBJECT";
case 14 : return "JL_NOSUCHMETHOD_ER";
case 15 : return "JL_STACKOVERFLOW_ER";
case 16 : return "JL_NULLPTR_EX";
case 17 : return "AZ";
case 18 : return "AC";
case 19 : return "AF";
case 20 : return "AD";
case 21 : return "AB";
case 22 : return "AS";
case 23 : return "AI";
case 24 : return "AJ";
case 25 : return "AV";
case 26 : return "JL_CLASSCAST_EX";
case 27 : return "JL_ARITHMETIC_EX";
case 28 : return "JL_ARRIDXOOBOUNDS_EX";
case 29 : return "JL_ILLEGALARGUMENT_EX";
case 30 : return "JL_INTERRUPTED_EX";
case 31 : return "JL_ILLEGALSTATE_EX";
case 32 : return "JL_ILLEGALMONITORSTATE_EX";
case 33 : return "JL_THREADDEATH";
case 34 : return "JL_ARRAYSTORE_EX";
case 35 : return "JL_NEGATIVEARRAYSIZE_EX";
case 36 : return "JL_CLASS";
case 37 : return "JL_STRING";
case 38 : return "JL_CLONEABLE";
case 39 : return "JL_THREAD";
default : return String.valueOf(value);
}
}catch(Throwable e){
return "BAD";
}
}
}

04Jun2011 – improve the IR target

New, triple IR "target"

New, triple IR target, with the battery holder and 4 20 Ohms resistors in parallel

During the previous tests I realised that there were 2 problems with the IR LED (VISHAY TSAL5300) I was using as a “target”:

1. the beam is quite narrow
2. the power is not that great (the wiimote IR camera can detect it from no more than 1m away)

I’ve never really cared about this, until NOW, when the whole thing starts to work nicely together and I realised while doing the previous 2 videos that it would be nicer if the tank could find the target quicker and not be so fussy about the angle, etc. …

The 1st thing I tried was to sand the end of the LED to widen it’s beam angle, but to no avail: the angle seems to be the same, and worse, the power seems to have diminished (probably the sanded surface absorbs some of the emission…)

I’ve also had a search on the Internet to find which exact IR LEDs are recommended for use with the Wiimote camera, but after finding 2 contradictory statements on 2 different blogs (one stating it’s best to use a 950 nm LED, the other 880 nm :)) I decided to continue with whatever I had in stock at the moment: VISHAY TSAL5300

• Peak wavelength: λp = 940 nm
• Angle of half intensity: ϕ = ± 22°
• Forward voltage 1.35V at 100mA

TSAL5300 Radiant Intensity VS Angular Displacement

I want to make the “target” self contained, to power it up from a small AAA rechargeable battery, and get the maximum rated emitting power.

So to obtain 100mA under 1.2V, the resistor to add should be 1200/100 = 12 Ω. The only ones I have right now are 20 Ω, so I’ve put 2 of these in parallel, giving me a 10 Ω resistance and hence a 120mA current.

• After some testing it turned out that at 100 mA the LEDs were not powerful enough, and the IR camera would only see them at 1-1.5 meters
• I’ve therefore added 2 extra resistors, to lower the total resistence to 5 Ω and hence have a current of around 240 mA
• This increased the “range” to 1.5 – 2 meters

Marginally higher than the rating from the documentation, might shorten the life of the LED, but I really don’t think it will make any difference for my limited usage.

Also, an obvious choice is also to use more than 1 LED in parallel, so I decided to put 3 of them to increase both the total power and the width of the beam.

04Jun2011 – the tank is FULLY autonomous:

• the CHASSIS
1. is just going forward a 20% speed
2. when it finds an obstacle (uses the ultrasonic ranger) it turns right until it avoids it
3. when the turret has found an IR target then the chassis tries to align itself with the gun
4. once it’s aligned it tries to go to 80cm from the target

• the TURRET
1. starts in “looking” mode going from left to right
2. when it finds an IR target it “locks” on it and follows it
3. if it’s been locked for more than 3 senconds then it fires (the buzz for now, will be replaced by actual firing !)

Another video of the same behaviour:

30May2011 – ultrasound range finder

I’ve now mounted this SRF05 ultra sound range finder to the chassis. The goal is to make the chassis able to independently and autonomously (both from a human control but also from the turret) roam around the terrain, while the turret is searching for targets or firing.

Ultra sond range finder (SRF05) installed

14May2011 – “locking” on a target

Here’s the tank “locking” on a target (simply an IR LED) and then keeping its gun (or at least trying) aligned in that direction.  The chassis is manually controlled whereas the turret is in automatic mode: it does nothing until / unless it finds an IR target and then it starts to track it.