Tracked Rover

In the first video you see the rover in action, making some attempts to avoid hitting the walls.

But it is easy for him to get stuck in an endless loop in a corner. I can add some randomization code to get out of these situations.

Soft fabrics (such as the shopping bag) don’t reflect well from the Ultrasonic sensor, so the rover happily drives straight into them!

Pictures

Video

Building Instructions

•Instructions in PDF format TrackRover.pdf
•Step-by-step instructions as an image gallery
•Download TrackRover_v0.1.ldr file to view the design in LDView.

Programming

I used RobotC to program the obstacle-avoidance behaviour. You can download TrackRover.c or read the code below. This code builds on my work to upload new macros into the NRLink-Nx device and requires a macro to power both motors simultaneously for the robot to drive.

I used a very simple obstacle avoidance algorithm in this code, it could stand to be improved! For example, it is possible to use “ping mode” on the Ultrasonic sensor to do a one-shot measurement of distance in the hope of gaining greater accuracy. I modified the source code into TrackRover_ping.c to try this out.

//*!!Sensor, S1, NRLINK, sensorI2CCustomStd9V, , !!*//

//*!!Sensor, S2, frontSonar, sensorSONAR9V, , !!*//

//*!!Sensor, S3, rearSonar, sensorSONAR9V, , !!*//

//*!! !!*//

//*!!Start automatically generated configuration code. !!*//

const tSensors NRLINK = (tSensors) S1; //sensorI2CCustomStd9V //*!!!!*//

const tSensors leftSonar = (tSensors) S2; //sensorSONAR9V //*!!!!*//

const tSensors rightSonar = (tSensors) S3; //sensorSONAR9V //*!!!!*//

//*!!CLICK to edit 'wizard' created sensor & motor configuration. !!*//

/*

*This is sample program to use with mindsensors.com NRLink interface.

*V1.1

*

* Written by Dr. Nitin Patil

* Copyright (c) mindsensors.com 2006, 2007

* for more info visit www.mindsensors.com

*

* Modified by Mark Crosbie mark@mastincrosbie.com 1 April 2008

* To install macros that power both motors simultaneously

* For more information visit http://markclego.mastincrosbie.com

*

* This sample has been updated to demonstrate

* the use of NRLink to drive PF Motors

*

*/

/*

Changeable macro definitions of NRLink

address definition RCX opcode

0x50 Motor Ch1 A Float, 02, 01, 00,

0x53 Motor Ch1 A Forward, 02, 01, 10,

0x56 Motor Ch1 A Reversed, 02, 01, 20,

0x59 Motor Ch1 A Brake, 02, 01, 30,

0x5C Motor Ch1 B Float, 02, 01, 00,

0x5F Motor Ch1 B Forward, 02, 01, 40,

0x62 Motor Ch1 B Reversed, 02, 01, 80,

0x65 Motor Ch1 B Brake, 02, 01, c0,

0x68 Motor Ch2 A Float, 02, 11, 00,

0x6B Motor Ch2 A Forward, 02, 11, 10,

0x6E Motor Ch2 A Reversed, 02, 11, 20,

0x71 Motor Ch2 A Brake, 02, 11, 30,

0x74 Motor Ch2 B Float, 02, 11, 00,

0x77 Motor Ch2 B Forward, 02, 11, 40,

0x7A Motor Ch2 B Reversed, 02, 11, 80,

0x7D Motor Ch2 B Brake, 02, 11, c0,

0x80 Motor Ch3 A Float, 02, 21, 00,

0x83 Motor Ch3 A Forward, 02, 21, 10,

0x86 Motor Ch3 A Reversed, 02, 21, 20,

0x89 Motor Ch3 A Brake, 02, 21, 30,

0x8C Motor Ch3 B Float, 02, 21, 00,

0x8F Motor Ch3 B Forward, 02, 21, 40,

0x92 Motor Ch3 B Reversed, 02, 21, 80,

0x95 Motor Ch3 B Brake, 02, 21, c0,

0x98 Motor Ch4 A Float, 02, 31, 00,

0x9B Motor Ch4 A Forward, 02, 31, 10,

0x9E Motor Ch4 A Reversed, 02, 31, 20,

0xA1 Motor Ch4 A Brake, 02, 31, 30,

0xA4 Motor Ch4 B Float, 02, 31, 00,

0xA7 Motor Ch4 B Forward, 02, 31, 40,

0xAA Motor Ch4 B Reversed, 02, 31, 80,

0xAD Motor Ch4 B Brake, 02, 31, C0,

Motor Ch1 A Forw B Forw, B0, 02, 01, 50,

Motor Ch1 A Forw B Rev, B3, 02, 01, 90,

Motor Ch1 A Rev B Forw, B6, 02, 01, 60,

Motor Ch1 A Rev B Rev, B9, 02, 01, a0,

Motor Ch2 A Forw B Forw, BC, 02, 11, 50,

Motor Ch2 A Forw B Rev, BF, 02, 11, 90,

Motor Ch2 A Rev B Forw, C2, 02, 11, 60,

Motor Ch2 A Rev B Rev, C5, 02, 11, a0,

Motor Ch3 A Forw B Forw, C8, 02, 21, 50,

Motor Ch3 A Forw B Rev, CB, 02, 21, 90,

Motor Ch3 A Rev B Forw, CE, 02, 21, 60,

Motor Ch3 A Rev B Rev, D1, 02, 21, a0,

Motor Ch4 A Forw B Forw, D4, 02, 31, 50,

Motor Ch4 A Forw B Rev, D7, 02, 31, 90,

Motor Ch4 A Rev B Forw, DA, 02, 31, 60,

Motor Ch4 A Rev B Rev, DD, 02, 31, a0

*/

//definations for NRLink

const ubyte NRLinkID = 0x02;

const ubyte NRLinkDataBytes = 0x40;

const ubyte NRLinkCommandReg = 0x41;

const ubyte NRLinkReadResult = 0x42;

const ubyte NRLinkWriteData = 0x42;

const tSensors NRLinkPort = NRLINK; // Connect NRLink sensor to this port!!

const ubyte NRLinkDefault = 0x44;

const ubyte NRLinkFlush = 0x46;

const ubyte NRLinkHighSpeed = 0x48;

const ubyte NRLinkLongRange = 0x4C;

const ubyte NRLinkShortRange = 0x53;

const ubyte NRLinkSetADPAON = 0x4E;

const ubyte NRLinkSETADPAOFF = 0x4F;

const ubyte NRLinkTxUnassembled = 0x55;

const ubyte NRLinkSelectRCX = 0x58;

const ubyte NRLinkSelectTRAIN = 0x54;

const ubyte NRLinkSelectPF = 0x50;

const ubyte NRLinkMacro = 0x52;

const ubyte Macro_Short_range = 0x01;

const ubyte Macro_Long_Range = 0x04;

const ubyte Motor_Ch1_A_Float = 0x50;

const ubyte Motor_Ch1_A_FWD = 0x53;

const ubyte Motor_Ch1_A_REV = 0x56;

const ubyte Motor_Ch1_A_Brake = 0x59;

const ubyte Motor_Ch1_B_Float = 0x5C;

const ubyte Motor_Ch1_B_FWD = 0x5F;

const ubyte Motor_Ch1_B_REV = 0x62;

const ubyte Motor_Ch1_B_Brake = 0x65;

const ubyte Motor_Ch2_A_Float = 0x68;

const ubyte Motor_Ch2_A_FWD = 0x6B;

const ubyte Motor_Ch2_A_REV = 0x6E;

const ubyte Motor_Ch2_A_Brake = 0x71;

const ubyte Motor_Ch2_B_Float = 0x74;

const ubyte Motor_Ch2_B_FWD = 0x77;

const ubyte Motor_Ch2_B_REV = 0x7A;

const ubyte Motor_Ch2_B_Brake = 0x7D;

const ubyte Motor_Ch3_A_Float = 0x80;

const ubyte Motor_Ch3_A_FWD = 0x83;

const ubyte Motor_Ch3_A_REV = 0x86;

const ubyte Motor_Ch3_A_Brake = 0x89;

const ubyte Motor_Ch3_B_Float = 0x8C;

const ubyte Motor_Ch3_B_FWD = 0x8F;

const ubyte Motor_Ch3_B_REV = 0x92;

const ubyte Motor_Ch3_B_Brake = 0x95;

const ubyte Motor_Ch4_A_Float = 0x98;

const ubyte Motor_Ch4_A_FWD = 0x9B;

const ubyte Motor_Ch4_A_REV = 0x9E;

const ubyte Motor_Ch4_A_Brake = 0xA1;

const ubyte Motor_Ch4_B_Float = 0xA4;

const ubyte Motor_Ch4_B_FWD = 0xA7;

const ubyte Motor_Ch4_B_REV = 0xAA;

const ubyte Motor_Ch4_B_Brake = 0xAD;

const ubyte Motor_Ch1_A_Forw_B_Forw = 0xB0;

const ubyte Motor_Ch1_A_Forw_B_Rev = 0xB3;

const ubyte Motor_Ch1_A_Rev_B_Forw = 0xB6;

const ubyte Motor_Ch1_A_Rev_B_Rev = 0xB9;

const int kThreshold = 20; // threshold for sonar sensor

// extensions to the default macros loaded into the NRlink to

// allow for control of two motors simultaneously

const ubyte powerFunctionsMacros[] = {

0xB0, 0x02, 0x01, 0x50, // Motor Ch1 A Forw B Forw

0xB3, 0x02, 0x01, 0x90, // Motor Ch1 A Forw B Rev

0xB6, 0x02, 0x01, 0x60, // Motor Ch1 A Rev B Forw

0xB9, 0x02, 0x01, 0xa0, // Motor Ch1 A Rev B Rev

0xBC, 0x02, 0x11, 0x50, // Motor Ch2 A Forw B Forw

0xBF, 0x02, 0x11, 0x90, // Motor Ch2 A Forw B Rev

0xC2, 0x02, 0x11, 0x60, // Motor Ch2 A Rev B Forw

0xC5, 0x02, 0x11, 0xa0, // Motor Ch2 A Rev B Rev

0xC8, 0x02, 0x21, 0x50, // Motor Ch3 A Forw B Forw

0xCB, 0x02, 0x21, 0x90, // Motor Ch3 A Forw B Rev

0xCE, 0x02, 0x21, 0x60, // Motor Ch3 A Rev B Forw

0xD1, 0x02, 0x21, 0xa0, // Motor Ch3 A Rev B Rev

0xD4, 0x02, 0x31, 0x50, // Motor Ch4 A Forw B Forw

0xD7, 0x02, 0x31, 0x90, // Motor Ch4 A Forw B Rev

0xDA, 0x02, 0x31, 0x60, // Motor Ch4 A Rev B Forw

0xDD, 0x02, 0x31, 0xa0 // Motor Ch4 A Rev B Rev

};

/////////////////////////////////////////////////////////////////////////////

//

// send Command to NrLink interface

//

/////////////////////////////////////////////////////////////////////////////

void NRLinkCommand(byte NRLinkCommand)

{

byte NRLinkMsg[5];

const byte MsgSize = 0;

const byte Address = 1;

const byte CommandAddress = 2;

const byte Command = 3;

// Build the I2C message

NRLinkMsg[MsgSize] = 3;

NRLinkMsg[Address] = NRLinkID;

NRLinkMsg[CommandAddress] = NRLinkCommandReg ;

NRLinkMsg[Command] = NRLinkCommand;

while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING)

{

// Wait for I2C bus to be ready

}

// when the I2C bus is ready, send the message you built

sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0);

}

/////////////////////////////////////////////////////////////////////////////

//

// send macro and run it from NRLink interface

//

/////////////////////////////////////////////////////////////////////////////

void NRLinkRunMacro(byte NRLinkMacroAdd) {

byte NRLinkMsg[5];

const byte MsgSize = 0;

const byte Address = 1;

const byte CommandAddress = 2;

const byte Command = 3;

const byte MacroAddress = 4;

// Build the I2C message

NRLinkMsg[MsgSize] = 4;

NRLinkMsg[Address] = NRLinkID;

NRLinkMsg[CommandAddress] = NRLinkCommandReg;

NRLinkMsg[Command] = NRLinkMacro;

NRLinkMsg[MacroAddress] = NRLinkMacroAdd;

while (nI2CStatus[NRLinkPort] == STAT_COMM_PENDING) {

// Wait for I2C bus to be ready

}

// when the I2C bus is ready, send the message you built

sendI2CMsg(NRLinkPort, NRLinkMsg[0], 0);

}

//////////////////////////////////////////////////////////////////////////////////////////

//

// Send a Message via I2C

//

// Sends an arbitrary 2-byte message over an I2C port. It would be easy to modify for

// messages of different length. Simply adjust the function parameters and the initialization

// of the 'nMsg' array.

//

// Usually when writing to device the reply length will be zero.

//

//////////////////////////////////////////////////////////////////////////////////////////

bool sendI2CMessage(tSensors nPortIndex, ubyte registerIndex, ubyte nByte1, ubyte nByte2, ubyte nByte3)

{

const int kI2CAddress = 0x02; // You may want to make this a function parameter

const ubyte nMsg[] =

{

2 + 3, // This is length field for transmitted message.

kI2CAddress, // The I2C address of the device. Almost all devices use value '0x02'

registerIndex, // The internal register index within the sensor to start writing at.

nByte1,

nByte2,

nByte3

};

// wait for I2C bus to be available

while (nI2CStatus[nPortIndex] == STAT_COMM_PENDING) { wait1Msec(1); }

sendI2CMsg(nPortIndex, nMsg[0], 0);

return true;

}

//////////////////////////////////////////////////////////////////////////////////////////

//

// Install additional power function macros into the NRLink that allow for

// simultaneously control of both attached motors

//

/////////////////////////////////////////////////////////////////////////////////////////

void installPFMacros() {

int i, numPFmacros;

nxtDisplayTextLine(2, "Writing macro");

numPFmacros = sizeof(powerFunctionsMacros);

for(i=0; i < numPFmacros; i+=4) {

// install macro into address

sendI2CMessage(NRLinkPort, // port the NRLink is connected to

powerFunctionsMacros[i], // register address to write the macro into

powerFunctionsMacros[i+1], // number of bytes in the macro

powerFunctionsMacros[i+2], // macro command

powerFunctionsMacros[i+3]);// macro command

}

nxtDisplayTextLine(3, "Done");

wait10Msec(100);

}

void brakeMotors() {

NRLinkRunMacro(Motor_Ch1_A_Brake);

wait10Msec(10);

NRLinkRunMacro(Motor_Ch1_B_Brake);

wait10Msec(10);

}

/////////////////////////////////////////////////////////////////////////////

//

// Run some commands and macro to control PF Motors using NRLink.

//

/////////////////////////////////////////////////////////////////////////////

task main()

{

int leftReading, rightReading;

nI2CBytesReady[NRLinkPort] = 0;

SensorType[NRLinkPort] = sensorI2CCustom9V;

NRLinkCommand(NRLinkFlush);

NRLinkCommand(NRLinkDefault);

NRLinkCommand(NRLinkLongRange);

NRLinkCommand(NRLinkSelectPF);

//installPFMacros();

nxtDisplayTextLine(4, "Driving");

while(1) {

// start driving forwards

leftReading = SensorValue[leftSonar];

rightReading = SensorValue[rightSonar];

while( (leftReading > kThreshold) && (rightReading > kThreshold) ) {

NRLinkRunMacro(Motor_Ch1_A_Forw_B_Rev); // motors are wired in opposite directions

wait10Msec(10);

leftReading = SensorValue[leftSonar];

rightReading = SensorValue[rightSonar];

}

brakeMotors();

// obstacle on the right, reverse turn to the left

while( (leftReading > kThreshold) && (rightReading <= kThreshold) ) {

NRLinkRunMacro(Motor_Ch1_A_Forw_B_Forw);

wait10Msec(10);

leftReading = SensorValue[leftSonar];

rightReading = SensorValue[rightSonar];

}

brakeMotors();

// obstacle on the left, reverse turn to the right

while( (leftReading <= kThreshold) && (rightReading > kThreshold) ) {

NRLinkRunMacro(Motor_Ch1_A_Rev_B_Rev);

wait10Msec(10);

leftReading = SensorValue[leftSonar];

rightReading = SensorValue[rightSonar];

}

brakeMotors();

// obstacle dead ahead, reverse and turn

while( (leftReading <= kThreshold) && (rightReading <= kThreshold) ) {

NRLinkRunMacro(Motor_Ch1_A_Rev_B_Forw);

wait10Msec(100);

NRLinkRunMacro(Motor_Ch1_A_Forw_B_Forw);

wait10Msec(10);

leftReading = SensorValue[leftSonar];

rightReading = SensorValue[rightSonar];

}

brakeMotors();

}

Last updated 23 May, 2008

LEGO® is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this site. This site, its owner and contents are in no way affiliated with or endorsed by the LEGO Group. For more please read the LEGO Fair Play policy.