Tiger 1 BB airsoft RC Tank – V2
May 2, 2011 6 Comments
It’s all about the same project as here, BUT with a different technology: instead of using the FEZ Domino (.NET micro framework based) board, I’ll be using the Lego NXT controller and an Arduino board for low level interaction with the electronics.
NXT + Arduino internals
11May2011
I’ve just received a new LiPo battery to replace the REALLY low quality original one.
It’s not only that it was Ni-MH, 2000mAh, but it would only charge half of it, and if left un-used it would loose its charge in a few days…
The new one is a 4000mAh 2 cells one, it charges much faster, and I hope it will last much longer.
Tipple Loong-Max 4000mAh LiPo - and it's 4mm gold bullet connector to Dean adaptor
Also, here’s the Java code that runs on the Lego NXT Brick, using the LeJOS JVM:
TigerTankBB.java
-------------------------------------------------------
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import trandi.nxt.tigerTankBB.IRCamera;
import trandi.nxt.tigerTankBB.util.Display;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.chassis.ChassisThread;
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) {
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. monitors the RC commands and displays them
SlaveBoard.instance.addCmdListener(new SlaveBoard.RCCmdListener() {
public void cmdReceived(byte speed, byte dir, byte vertical, byte horiz, boolean manual, boolean fire) {
Display.setSpeed(speed);
Display.setDir(dir);
Display.setVertical(vertical);
Display.setHoriz(horiz);
Display.setManualMode(manual);
Display.setFire(fire);
}
public void error(String error) {
Display.setErr(error);
}
});
// 3. START the main threads, each independently controlling one part of the tank
new BatteryMonitorThread().start();
new TurretThread().start();
new ChassisThread().start();
//temp initialises the IRCamera class and starts the monitorin thread....
IRCamera.instance.getBlob1();
}catch(Exception e){
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 RandomMovementsBehaviour(),
new ManualBehaviour()
}).start();
}
}
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
}
}
SlaveBoard.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB;
import trandi.nxt.tigerTankBB.util.Util;
import trandi.nxt.tigerTankBB.util.MyI2CSensor;
import java.util.ArrayList;
import java.util.List;
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 SIGNAL_LOST = (byte)123;
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;
private final List<RCCmdListener> _listeners = new ArrayList<RCCmdListener>();
// state of the commands
volatile private byte _speed;
volatile private byte _dir;
volatile private byte _vertical;
volatile private byte _horiz;
volatile private boolean _manual;
volatile private boolean _fire;
volatile private String _error;
private SlaveBoard(I2CPort port, byte address){
_i2cSensor = new MyI2CSensor(port, address);
// start a new thread monitoring the slave board, updating the state and calling the listeners when new commands arrive
new MyThread(){
@Override
public void run2() throws Exception{
while(true) {
// 1. reads the data and updates the state
readCmd();
// 2. call the listeners if any (lock te list while iterating over it)
synchronized (instance) {
for(RCCmdListener listener : _listeners) {
if(_error == null) listener.cmdReceived(_speed, _dir, _vertical, _horiz, _manual, _fire);
else listener.error(_error);
}
}
// wait a little before next update
Delay.msDelay(50);
}
}
}.start();
}
public synchronized boolean addCmdListener(RCCmdListener listener) {
return _listeners.add(listener);
}
public synchronized boolean removeCmdListener(RCCmdListener listener) {
return _listeners.remove(listener);
}
private void readCmd() {
byte[] data = new byte[CMD_SIZE];
int bytesRead = _i2cSensor.getData(0x00, data, CMD_SIZE);
if(bytesRead == CMD_SIZE) {
_error = null;
_speed = data[0];
_dir = data[1];
_vertical = data[2];
_horiz = data[3];
_manual = Util.percToBool(data[4]);
_fire = Util.percToBool(data[5]);
}else if(bytesRead == 1 && data[0] == SIGNAL_LOST){
_error = "SIG LOST";
}else{
_error = String.valueOf(bytesRead);
}
}
public void sendCmd(int speed, int dir, boolean fire){
byte[] data = new byte[CMD_SIZE];
data[0] = Util.constrainPerc(speed);
data[1] = Util.constrainPerc(dir);
data[2] = Util.boolToPerc(fire);
data[3] = 0;
data[4] = 0;
data[5] = 0; // light
_i2cSensor.sendData(0x00, data, data.length);
}
public static interface RCCmdListener{
public void cmdReceived(byte speed, byte dir, byte vertical, byte horiz, boolean manual, boolean fire);
public void error(String error);
}
public byte getDir() {
return _dir;
}
public boolean isFire() {
return _fire;
}
public byte getHoriz() {
return _horiz;
}
public boolean isManual() {
return _manual;
}
public byte getSpeed() {
return _speed;
}
public byte getVertical() {
return _vertical;
}
public String getError(){
return _error;
}
}
ManualBehaviour.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.turret;
import lejos.util.Delay;
import trandi.nxt.tigerTankBB.SlaveBoard;
import trandi.nxt.tigerTankBB.SlaveBoard.RCCmdListener;
import trandi.nxt.tigerTankBB.util.AbstractBehaviour;
import trandi.nxt.tigerTankBB.util.MyMotor;
public class ManualBehaviour extends AbstractBehaviour{
private final MyMotor _turretMotor;
private final RCCmdListener _cmdListener;
public ManualBehaviour(MyMotor turretMotor) {
super(BodyPart.TURRET, "Man");
_turretMotor = turretMotor;
_cmdListener = new RCCmdListener() {
public void cmdReceived(byte speed, byte dir, byte vertical, byte horiz, boolean manual, boolean fire) {
_turretMotor.setSpeed(SlaveBoard.instance.getHoriz());
}
public void error(String error) {
// do nothing
}
};
}
public boolean takeControl() {
return SlaveBoard.instance.isManual();
}
@Override
public void action2() {
// do not release control until we manually say so...
if(SlaveBoard.instance.isManual()) SlaveBoard.instance.addCmdListener(_cmdListener);
// do not release control until we manually say so...
// do nothing, the listener that we've just added, will do the turret control
while(continu() && SlaveBoard.instance.isManual()) {
Thread.yield();
Delay.msDelay(50);
}
}
@Override
public void suppress2() {
SlaveBoard.instance.removeCmdListener(_cmdListener);
_turretMotor.stop();
}
}
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 {
private final MyMotor _turretMotor = new MyMotor(MotorPort.A);
@Override
public void run2() {
// stays in this method forever...
new MyArbitrator(new Behavior[]{ new SearchBehaviour(_turretMotor),
new FollowBehaviour(_turretMotor),
new ManualBehaviour(_turretMotor)
}).start();
}
}
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();
Display.setBatt(voltage + "mV");
if(voltage < 6500) {
int prevVol = Sound.getVolume();
Sound.setVolume(Sound.VOL_MAX);
Sound.twoBeeps();
Sound.setVolume(prevVol);
}
Thread.yield();
Delay.msDelay(10000);
}
}
}
Display.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.util;
import lejos.nxt.LCD;
import lejos.util.Delay;
import trandi.nxt.tigerTankBB.IRCamera.Blob;
public class Display {
private volatile static boolean _manualMode, _fire;
private volatile static int _speed, _dir, _vertical, _horiz;
private volatile static String _batt, _info, _err, _chassisMode, _turretMode;
private volatile static Exception _exception;
private volatile static Blob _irBlob;
static{
new MyThread(){
@Override
public void run2() throws Exception {
while(true){
refresh();
Delay.msDelay(50);
}
}
}.start();
}
public static void setBatt(String batt){
_batt = batt;
}
public static void setDir(int dir) {
_dir = dir;
}
public static void setManualMode(boolean manualMode) {
_manualMode = manualMode;
}
public static void setFire(boolean fire) {
_fire = fire;
}
public static void setSpeed(int speed) {
_speed = speed;
}
public static void setHoriz(int horiz) {
_horiz = horiz;
}
public static void setVertical(int vertical) {
_vertical = vertical;
}
public 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("Batt: " + _batt, 0, 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){
LCD.drawString("IR x" + _irBlob.x, 0, 4);
LCD.drawString("y" + _irBlob.y, 8, 4);
LCD.drawString("s" + _irBlob.size, 12, 4);
}
LCD.drawString("I:" + _info + ":" + System.currentTimeMillis(), 0, 5);
LCD.drawString("E:" + _err + ":" + System.currentTimeMillis(), 0, 6);
if(_exception != null) LCD.drawString("EX:" + _exception.getMessage(), 0, 7);
LCD.refresh();
}
}
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;
public class MyMotor {
private final static int REAL_MIN_SPEED = 0;
private final static int REAL_MAX_SPEED = 900;
private final Motor _baseMotor;
public MyMotor(TachoMotorPort port){
_baseMotor = new Motor(port);
_baseMotor.smoothAcceleration(true);
//regulateSpeed(true);
}
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 void setSpeed(int speedPerc){
speedPerc = Util.constrainPerc(speedPerc);
if(Math.abs(speedPerc - Util.PERC_MID) < (Util.PERC_DELTA / 10)) _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();
}
}
public void stop(){
_baseMotor.stop();
}
}
MyThread.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.util;
import lejos.nxt.Sound;
public abstract class MyThread {
private final Thread _thread = new Thread(){
@Override
public void run(){
try {
run2();
} catch (Exception ex) {
Display.setException(ex);
Sound.buzz();
}
}
};
public abstract void run2() throws Exception;
public void start(){
_thread.start();
}
}
Util.java
-------------------------------------------------------
package trandi.nxt.tigerTankBB.util;
public final class Util {
public final static byte PERC_MIN = 0;
public final static byte PERC_MAX = 120;
public final static byte PERC_MID = (PERC_MAX + PERC_MIN) / 2;
public final static byte PERC_DELTA = PERC_MAX - PERC_MIN;
public static boolean percToBool(byte percentage){
return (percentage > (PERC_MID + PERC_DELTA / 4));
}
public static byte 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 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 byte constrainPerc(int input){
return (byte)constrain(input, PERC_MIN, PERC_MAX);
}
}
08May2011
Today is my birthday… and this tank is actually my present from one branch of the family… OK, right I bought it slightly in advance, but a BIG THANKS to you guys, you know who you are !
The 2nd best present is that I’ve finally managed to make the V2 FULLY functional ! In manual mode that is, which means that now, I can start working on the “real stuff”, making it autonomous.
Here are a couple of pictures of the finished electronics and wiring:
Details of the NXT brick and the Arduino. Notice the display of vital data on the LCD
The finished V2 internals. All wired up !
Here is the code used on the Arduino slave board
It basically takes care of the low level stuff:
- receiving and decrypting the PPM radio signals
- generating PWM pulses to control the tracks motors
- generating 1 Servo pulses for the fire ESC (that controls the fire motor)
- sending the radio commands to the I2C master when requested
- receiving commands from the I2C master when not in manual mode
TigerTankBB_Slave.pde
-------------------------------------------------------
#include <Wire.h>
// only deal with positive values over I2C and also on only 7 bits (< 127)
#define VALID_PERC_MIN 0
#define VALID_PERC_MAX 120
const byte VALID_PERC_MID = (VALID_PERC_MAX - VALID_PERC_MIN) / 2;
#include "MyServo.h"
#include "Chassis.h"
#include "Turret.h"
#define LED_PIN 13
#define I2C_SLAVE_ADDR 0x08
#define MAX_CHANNELS 6 // Number of radio channels to read. Has to be <= to what the receiver outputs in its PPM signal
#define SIGNAL_LOST 123
Chassis _chassis;
MyServo _fireESC(TURRET_FIRE_PIN);
Turret _turret(&_fireESC);
unsigned long _lastLoop = millis();
boolean _manualMode;
volatile byte _percentages[MAX_CHANNELS];
volatile byte _percentagesMaster[MAX_CHANNELS];
volatile boolean _masterDataReceived = false;
void setup(){
//Serial.begin(115200);
Wire.begin(I2C_SLAVE_ADDR); //join as slave
Wire.onRequest(onI2CRequest);
Wire.onReceive(onI2CReceive);
setup_radio();
pinMode(LED_PIN, OUTPUT);
}
void loop(){
// update te SERVOS pulse generation. This Will NOT work if this LOOP does too many things...
_fireESC.update(micros());
// using a temporized main loop (can't get the radio data much more often that this anyhow...)
if(millis() - _lastLoop >= 60){
_lastLoop = millis();
_turret.clockUpdate(); // the Turret has some internal updates to make
radio_update();
master_update();
//Serial.print("Data from Radio: ");Serial.print(_percentages[0], DEC);Serial.print("/"); Serial.println(_percentages[1], DEC);
//Serial.print("Data from Master: ");Serial.print(_percentagesMaster[0], DEC);Serial.print("/"); Serial.println(_percentagesMaster[1], DEC);
// first thing is to update the mode
_manualMode = isSignalLost() ? true : getChannel(5) > VALID_PERC_MID;
// in MANUAL MODE, the Arduino controls the motors DIRECTLY, ignoring signals from the I2C Master
if(_manualMode){
if(isSignalLost()){
digitalWrite(LED_PIN, HIGH); // signal PROBLEM
_chassis.move(VALID_PERC_MID, VALID_PERC_MID);
}else{
digitalWrite(LED_PIN, LOW); // clear any eventual PROBLEM
_chassis.move(getChannel(1), getChannel(2));
if(getChannel(3) > 100) _turret.fire();
_turret.light(getChannel(6));
}
}else{
master_update();
}
}
}
void master_update(){
// if we can get data from the master and if we're not in manual mode
if(_masterDataReceived){
_chassis.move(_percentagesMaster[0], _percentagesMaster[1]);
if(_percentagesMaster[2] > 100) _turret.fire();
_turret.light(_percentagesMaster[2]);
_masterDataReceived = false;
}
}
// master requests for "stuff" -> we send the percentages calculated from PPM radio data
byte dataToSend[MAX_CHANNELS];
void onI2CRequest(){
if(isSignalLost()) Wire.send(SIGNAL_LOST);
else{
// still transmit data, even in manual mode, for INFO and for the turret horiz movement which the Arduino doesn't have access to
// for some obscure reason the NXT can only accept 7 bits...
for(byte i=0; i<MAX_CHANNELS; i++) dataToSend[i] = _percentages[i];
// This is DIFFERENT than sending them byte by byte, probably this is marked as 1 transaction
Wire.send(dataToSend, MAX_CHANNELS);
}
}
// master sends "stuff" -> we interpret it as COMMANDS for motors
void onI2CReceive(int numBytes){
// ignore messages if in manual mode, and just empty the buffer
if(! _manualMode){
_masterDataReceived = false;
// the first byte is the register, which we don't care about
if(Wire.available()) Wire.receive();
byte bytesCount;
for(bytesCount = 0; bytesCount < MAX_CHANNELS && Wire.available(); bytesCount++){
byte currData = Wire.receive();
if(currData > VALID_PERC_MAX) {
bytesCount = 99;
break;
}
_percentagesMaster[bytesCount] = currData;
}
if(MAX_CHANNELS == bytesCount) _masterDataReceived = true;
}
// IMPORTANT!!! keep reading if there's extra stuff (to empty the buffer)
while(Wire.available()) Wire.receive();
}
Chassis.h
-------------------------------------------------------
#ifndef Chassis_h
#define Chassis_h
#include "Motor.h"
const byte TRACK_LEFT_FORWARD_PIN = 5;
const byte TRACK_LEFT_BACK_PIN = 6;
const byte TRACK_LEFT_ENABLE_PIN = 7;
const byte TRACK_RIGHT_FORWARD_PIN = 9;
const byte TRACK_RIGHT_BACK_PIN = 10;
const byte TRACK_RIGHT_ENABLE_PIN = 8;
class Chassis {
private:
Motor _leftTrack;
Motor _rightTrack;
public:
Chassis() : _leftTrack(TRACK_LEFT_FORWARD_PIN, TRACK_LEFT_BACK_PIN, TRACK_LEFT_ENABLE_PIN),
_rightTrack(TRACK_RIGHT_FORWARD_PIN, TRACK_RIGHT_BACK_PIN, TRACK_RIGHT_ENABLE_PIN){}
void move(int speedPerc, int dirPerc);
};
void Chassis::move(int speedPerc, int dirPerc){
int dirDelta = (dirPerc - VALID_PERC_MID) / 2;
_leftTrack.set(speedPerc + dirDelta);
_rightTrack.set(speedPerc - dirDelta);
}
#endif
Motor.h
-------------------------------------------------------
#ifndef Motor_h
#define Motor_h
#include <WProgram.h>
const byte NO_ENABLE_PIN = 99;
class Motor {
private:
byte _sens1Pin;
byte _sens2Pin;
byte _enablePin;
void init(byte, byte, byte);
public:
Motor(byte sens1Pin, byte sens2Pin) {init(sens1Pin, sens2Pin, NO_ENABLE_PIN);}
Motor(byte sens1Pin, byte sens2Pin, byte enablePin) {init(sens1Pin, sens2Pin, enablePin);}
void set(int);
};
void Motor::init(byte sens1Pin, byte sens2Pin, byte enablePin){
_sens1Pin = sens1Pin;
_sens2Pin = sens2Pin;
_enablePin = enablePin;
if(_enablePin != NO_ENABLE_PIN) pinMode(_enablePin, OUTPUT);
}
void Motor::set(int percentage){
// limit to a valid percentage [0, 120]
byte value = constrain(percentage, VALID_PERC_MIN, VALID_PERC_MAX);
boolean sens1 = value > VALID_PERC_MID;
value = abs(value - VALID_PERC_MID);
// scale to [0..255] which is what analogWrite wants
value = map(value, 0, VALID_PERC_MID, 0, 255);
if(value > 25){
if(_enablePin != NO_ENABLE_PIN) digitalWrite(_enablePin, HIGH);
if(sens1){
analogWrite(_sens2Pin, 0);
analogWrite(_sens1Pin, value);
}else{
analogWrite(_sens1Pin, 0);
analogWrite(_sens2Pin, value);
}
}else{
analogWrite(_sens1Pin, 0);
analogWrite(_sens2Pin, 0);
if(_enablePin != NO_ENABLE_PIN) digitalWrite(_enablePin, LOW);
}
}
#endif
MyServo.h
-------------------------------------------------------
#ifndef MyServo_h
#define MyServo_h
#include <WProgram.h>
const int MIN_PULSE_US = 1100;
const int MAX_PULSE_US = 1900;
const int SERVO_PERIOD_US = 2500; // could be up to 20ms, but the lower the more accuracy
class MyServo {
private:
byte _pin;
boolean _high;
unsigned long _pulseInUS;
unsigned long _startPeriod;
public:
MyServo(uint8_t pin) : _pin(pin), _high(false), _pulseInUS(1500), _startPeriod(0) {
pinMode(_pin, OUTPUT);
}
// 0..120 -> 1000..2000 uS pulses
void setPosition(byte percentage) {
_pulseInUS = map(percentage, VALID_PERC_MIN, VALID_PERC_MAX, MIN_PULSE_US, MAX_PULSE_US);
}
void update(unsigned long timeUS){
if(timeUS - _startPeriod > SERVO_PERIOD_US) {
_startPeriod = timeUS;
if(! _high) {
digitalWrite(_pin, HIGH);
_high = true;
}
}else if((timeUS - _startPeriod >= _pulseInUS) && _high) {
digitalWrite(_pin, LOW);
_high = false;
}
}
};
#endif
radioPPM.pde
-------------------------------------------------------
/*
Setting a bit: byte |= 1 << bit;
Clearing a bit: byte &= ~(1 << bit);
Toggling a bit: byte ^= 1 << bit;
Checking if a bit is set: if (byte & (1 << bit))
Checking if a bit is cleared: if (~byte & (1 << bit)) OR if (!(byte & (1 << bit)))
*/
#define PPM_PIN 2 // External Interrupt 0 - Input Capture Pin (PPM Rx signal reading)
// MicroSecs for each tick = PRESCALER / (F_CPU / 1.000.000) * (OCR + 1)
const int TIMER_MULT = 8;
const int MIN_PULSE = 1100 / TIMER_MULT;
const int MAX_PULSE = 1900 / TIMER_MULT;
const int SYNC_GAP = 5000 / TIMER_MULT; // 5ms means a gap in between 2 PPM signals
const int MIN_VALID_PULSE = 900 / TIMER_MULT; //a valid pulse must be at least 900uS
const int MAX_VALID_PULSE = 2100 / TIMER_MULT; //a valid pulse must be less than 2100uS
const double DELTA_PULSE_PERC = (double)(VALID_PERC_MAX - VALID_PERC_MIN) / (double)(MAX_PULSE - MIN_PULSE);
const byte BAD_VALUE_PERC = 122;
const byte BAD_SIGNALS_FOR_LOST_SIGNAL = 5; //how many bad signad to wait until declaring the signal lost
volatile unsigned int _ticks = 0; //used for counting PPM pulses
volatile int8_t _rx_ch = 0;
volatile int _currentPulses[MAX_CHANNELS + 1]; //pulses width in ticks for performance.
volatile byte _signalLostCount;
void setup_radio(){
// PPM signal initialisation
pinMode(PPM_PIN, INPUT);
// TIMER 2 Pins 3 & 11 will have PWM disabled ! TCNT2 increments every TIMER_MULT uS
TCCR2B = 0; // STOP the timer
TCCR2A = 1 << WGM21; // Mode2 = CTC mode, trigger compare interrupt when greater than OCRA. Automatically RE-INITS TCNT
OCR2A = 1; // call the compare interrupt every 2 ticks (8uS in our case) - when TCNT2 > OCRA2A
TIMSK2 = 1 << OCIE2A; //Compare A interrupt enable
TCCR2B = 1 << CS22; // START the timer with prescaler = 64
// whenever there is a raise in the PPM signal, deal with it
attachInterrupt(0, changeInSignalISR, RISING);
}
// called every time TCNT2 > OCR2A
ISR(TIMER2_COMPA_vect){
_ticks ++;
if(_ticks > SYNC_GAP) _rx_ch = 0;
// _ticksServo ++;
// _fireESC.update(_ticksServo * TIMER_MULT);
}
void changeInSignalISR(){
if(_rx_ch >= 0) _currentPulses[_rx_ch ++] = _ticks; //pulse for one of the channels
// always re-init the times as we always start counting "something" on a rising change
_ticks = 0;
// this should be the last pulse, don't accept other pulses until we get a new SYNC signal
if(_rx_ch > MAX_CHANNELS) _rx_ch = -1;
}
// do periodic things here, we don't want to do them in the ISR as this can have adverse effects on other
// interrupts driven processes (like the Wire receive/request)
void radio_update(){
// "publish" a signal
boolean validData = true;
for(byte i = 0; i < MAX_CHANNELS; i ++){
if((_currentPulses[i+1] > MIN_VALID_PULSE) && (_currentPulses[i+1] < MAX_VALID_PULSE)){
_percentages[i] = constrain(int((_currentPulses[i+1] - MIN_PULSE) * DELTA_PULSE_PERC), VALID_PERC_MIN, VALID_PERC_MAX);
}else{
_percentages[i] = BAD_VALUE_PERC; //ERROR
validData = false;
}
}
if(validData) _signalLostCount = 0;
else _signalLostCount++;
}
byte getChannel(byte channelNo){
return _percentages[channelNo - 1];
}
boolean isSignalLost(){
return (_signalLostCount >= BAD_SIGNALS_FOR_LOST_SIGNAL);
}
Turret.h
-------------------------------------------------------
#ifndef Turret_h
#define Turret_h
#include "Motor.h"
#include "MyServo.h"
const byte TURRET_FIRE_PIN = 11;
const byte TURRET_LIGHT_PIN = 12;
const int FIRE_TIME = 2000; // milliseconds
class Turret {
private:
// the horizontal motor is managed by somebody else (right now, the I2C master -
MyServo* _fireESC;
byte _lightPin;
unsigned long _fireStartTime;
public:
Turret(MyServo* fireESC) : _fireESC(fireESC),
_lightPin(TURRET_LIGHT_PIN),
_fireStartTime(0)
{
_fireESC->setPosition(VALID_PERC_MIN);
pinMode(_lightPin, OUTPUT);
digitalWrite(_lightPin, HIGH);
}
void clockUpdate();
boolean fire();
void light(byte intensityPerc);
};
void Turret::clockUpdate(){
// we are in the process of firing and the necessary delay lapsed
if(_fireStartTime != 0 && (millis() - _fireStartTime >= FIRE_TIME)){
_fireESC->setPosition(VALID_PERC_MIN); // stop the canon
_fireStartTime = 0; // we are not firing anymore
}
}
boolean Turret::fire(){
if(_fireStartTime == 0){
// the canon is actually NOT firing
_fireESC->setPosition(VALID_PERC_MAX); // start the canon
_fireStartTime = millis(); // record when we started, to know when to stop !
return true; // we started firing
}else{
return false; // couldn't fire, another fire operation in progress !
}
}
void Turret::light(byte intensityPerc){
// percentage is [0..120] and analogWrite wants [0..255]
analogWrite(_lightPin, map(constrain(intensityPerc, VALID_PERC_MIN, VALID_PERC_MAX), VALID_PERC_MIN, VALID_PERC_MAX, 0, 255));
}
#endif
01May2011
After the FEZ Domino board broke, I quite successfully moved to a 2 Arduino boards based solution : 1 the main brain, and another (which wasn’t actually Arduino, but simply a Atmega160 based board) for the PPM radio signals.
The communication was through a I2C bus.
However the “problem” appeared quite quickly: in order to do anything useful with the tank and be able to track and shoot any target, I needed to have some feedback on the position of the turret… One bright idea was to use a nifty optical encoder salvaged a couple of weeks ago from a printer.
However, mechanically it proved to be a pain… also, the noise made by the cheap / crappy turret motor was driving me crazy… that’s when this great idea came to me:
Lego NXT based turret motor control. Do notice the "polymorph" plastic used to attach it to the chassis.
The Lego NXT motors have an encoder included, so you can bet feedback and achieve precise movements !
Now, this means either a NXT motor multiplexer (which can actually be controlled by any MCU that can do I2C) OR simply using a NXT Brick…
Miscellaneous
Careful with I2C transactions…
On the Arduino / slave side you need to send the data at once with
Wire.send(buffer, bytesCount);
when the NXT / master asks for it, otherwise (if you send the bytes one by one) it will be considered several transactions and won’t arrive properly…
Synchronized send and getData()
I’ve just found out the hard way (more than half a day of wasted work… lol 🙂 ) that you can’t send and receive data from the master on the I2C serial bus at the same time… 🙂
I was calling both getData() and sendData() on a I2CSensor object, at the same time from 2 different threads and was getting an -3 error : “I2C error”.
Now here’s a better version of the I2CSensor class, that solves this and improves other details:
package trandi.nxt.tigerTankBB;
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...
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;
}
}
Robotics / Electronics / Physical Computing 
Hello,
Very interesting. I guess I have the same tank only i quess mine is a Leopard. I bought a motordriver with an L298 dual H bridge(before i saw this article) But i guess this won’t work with the NXT because of the bipolar motor output? I have to make pin 1a or 1b high to go forward or backward. Could i use the optocoupler (optoisolator)from the mosfetbridge suggested in the NXT Extreme book?
Hi Hans,
First of all, make sure you check
this post which is about the latest version of the tank and also has some nice videos ! (here is also the 1st post about this tank…)
I’m finally using exactly 2 L298 to drive the tracks and they work pretty well.
However you can’t control them directly with the NXT, so in my case I have a “slave” Arduino board, which does the “low level” control of the L298 and accepts serial commands (over I2C) from the NXT.
I have no idea what optocoupler from the NXT Extreme book you’re talking about, I’ve never looked at that book… is it any good ?
Hope this helps,
Dan
Thanks for reply Dan
I could send you if you have an adress Also some subjects with I2C in there..
thanks, I’ll send you my e-mail address. Also, do let me know if you want to go the “Arduino in the middle ” way, and if you need more help…?
Dan
Pingback: Tiger 1 BB airsoft RC Tank – V3 « Robotics / Electronics / Physical Computing
Pingback: Tiger 1 BB airsoft RC Tank « Robotics / Electronics / Physical Computing