Track Rover
Track Rover
April 2008
I built this low-slung tracked rover using the LEGO medium power-functions motor. It avoids obstacles using two LEGO ultrasonic sensors mounted on the front of the robot. The power-function motors are controlled using the Mindsensors NRLink-Nx unit and the code I developed previously.
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
•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
All content © 2008 Mark Crosbie mark@mastincrosbie.com
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.