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

Building Instructions


  1. Instructions in PDF format TrackRover.pdf

  2. Step-by-step instructions as an image gallery

  3. 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[] = {

  0xB00x020x010x50, // Motor Ch1 A Forw B Forw

  0xB30x020x010x90, // Motor Ch1 A Forw B Rev

  0xB60x020x010x60, // Motor Ch1 A Rev B Forw

  0xB90x020x010xa0, // Motor Ch1 A Rev B Rev

  0xBC0x020x110x50, // Motor Ch2 A Forw B Forw

  0xBF0x020x110x90, // Motor Ch2 A Forw B Rev

  0xC20x020x110x60, // Motor Ch2 A Rev B Forw

  0xC50x020x110xa0, // Motor Ch2 A Rev B Rev

  0xC80x020x210x50, // Motor Ch3 A Forw B Forw

  0xCB0x020x210x90, // Motor Ch3 A Forw B Rev

  0xCE0x020x210x60, // Motor Ch3 A Rev B Forw

  0xD10x020x210xa0, // Motor Ch3 A Rev B Rev

  0xD40x020x310x50, // Motor Ch4 A Forw B Forw

  0xD70x020x310x90, // Motor Ch4 A Forw B Rev

  0xDA0x020x310x60, // Motor Ch4 A Rev B Forw

  0xDD0x020x310xa0  // 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.