Obstacle display using DROD

May 2008

 

I recently order two DROD-Nx proximity sensors from mindsensors.com and wanted to see how they performed on my Synchro drive robot platform. I thought that the best way to test them would be to write a sample program to draw an “obstacle map” on the NXT screen that represented what the sensors detected. Next step is to write code for the robot to back itself out of corners and avoid obstacles.

Interfacing to the DROD-Nx is very simple; it returns one of three values depending on whether the obstacle is detected to the left, front or right of the sensors. I did some testing and found that the sensors often return a front reading if even a part of the obstacle is in front of the sensor. For example, you could hold your hand to the right but get an “obstacle detected in front of the sensor” value returned. This will probably call for some code to remember the past N readings to build a map of the obstacles around the robot.

Below you can see my Robot C code sample for reading the values returned and drawing a graphic on the screen that shows which of the six “zones” around the robot have a reading. Because I use two sensors I represent the readings as a bit-field in an integer. Currently a maximum of two bits can be set to one indicating a reading returned, but I could change sensors in the future and this display code will still work fine.

Pictures


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.

Programming

You can download my SynchoDrive v5.c for RobotC.

//*!!Sensor,    S1,            frontDROD, sensorLightActive,      ,              !!*//

//*!!Sensor,    S2,             rearDROD, sensorLightActive,      ,              !!*//

//*!!Motor,  motorA,           DriveMotor, tmotorNxtEncoderClosedLoop,           !!*//

//*!!Motor,  motorB,        SteeringMotor, tmotorNxtEncoderClosedLoop,           !!*//

//*!!                                                                            !!*//

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

const tSensors frontDROD            = (tSensors) S1;   //sensorLightActive  //*!!!!*//

const tSensors rearDROD             = (tSensors) S2;   //sensorLightActive  //*!!!!*//

const tMotor   DriveMotor           = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//

const tMotor   SteeringMotor        = (tMotor) motorB; //tmotorNxtEncoderClosedLoop //*!!!!*//

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


//

// Synchro Drive robot v4

//

// Copyright 2008 Mark Crosbie mark@mastincrosbie.com

// 3/2/2008

//

// Uses mindsensors.com DROD-Nx on "front" and "rear" to detect obstacles

//

// Motor A:steering motor

// Motor B: drive motor

//

// Sensor 1: "Forward" facing DROD-Nx

// Sensor 2: "Rear" facing DROD-Nx

//

// Even though the robot has no real front or rear as it can drive in any

// direction we need to nominate a "forward" direction to keep coding

// simple. So the NXT is pointing "forward" and hence that side of the

// robot is the front for our purposes.

//



// Constants


#define ORANGE_BUTTON 3


// DROD-Nx readings for obstacle location

const int kLeftObstacle = 33;

const int kFrontObstacle = 78;

const int kRightObstacle = 67;

const int DRODerror = 3; // readings from DROD may vary by this much


// display related constants for drawing the obstacle map

const int kDisplayWidth = 100;

const int kDisplayHeight = 64;


// dimensions of the central "box" that represents the robot

const int t = 12;// distance from box corners of "obstacle" line endpoints

const int ax=30, ay=48; // top left of central box

const int w=25, h=32; // width and height of box


const int standardDriveSpeed = 50;

const int warpDriveSpeed = 100;

const int slowDriveSpeed = 15;


// Bitmask defining which segments around the vehicle perimeter an

// obstacle was detected in. Obstacles can be located all around

// the robot so we need to know every obstacle in one reading

// to plan a route away from them

typedef enum {

kNoObstacles = 0,

kFrontLeft = 1,

kFront = 2,

kFrontRight = 4,

kRearLeft = 8,

kRear = 16,

kRearRight = 32

} tObstacleLocation;


// Global variables

int line = 1;

tObstacleLocation obstacleLocation;


// This array contains the start and endpoints of each line

// segment we draw around the robot graphic on the screen

// Each segment is represented as four coordiantes

// 0: start x

// 1: start y

// 2: end x

// 3: end y

//

// The contents are computed once based on the dimensions specified

// in ax, ay, h, t during initialisation

int obstacleMap[6][4];


//

// Function prototypes

//

void initialiseRobot();

void sayHello();

tObstacleLocation computeObstacleLocation(int reading);

void drawObstacleMap(tObstacleLocation obstacles);

void precomputeObstacleMap();

void checkBTLinkConnected();


//

// Monitor the front and rear DROD sensors and update the global

// variable obstacleLocation when an obstacle is detected

task detectObstacles() {


int front, rear;


while(1) {


obstacleLocation = kNoObstacles; // clear previous reading


// compute the distance between the front DROD reading and each

// return value. The one with the smallest error is the location

// of the obstacle

front = SensorValue[frontDROD];

rear = SensorValue[rearDROD];


obstacleLocation |= computeObstacleLocation(front);

obstacleLocation |= (computeObstacleLocation(rear)*8); // left shift three bits


wait10Msec(1); // don't hog CPU

}

}


task recvMessages() {

static int nLastMessage = 0;

int nMessage;


while (true)

{

  nMessage = message;

  if (nMessage != 0)

  {

  motor[DriveMotor] = messageParm[1];

  //motor[SteeringMotor] = messageParm[2];

  ClearMessage();

}

wait1Msec(1);

}

}


task main()

{

// say hello

sayHello();


initialiseRobot();


StartTask(detectObstacles);


StartTask(recvMessages);


while(1) {

//nxtDisplayTextLine(1, "Obstacle: %d", obstacleLocation);

drawObstacleMap(obstacleLocation);

wait10Msec(30);

}


}


void sayHello() {


eraseDisplay();

nxtDisplayTextLine(line++, "Synchro Drive");

nxtDisplayTextLine(line++, "DriveMotor: A");

nxtDisplayTextLine(line++, "SteeringMotor: B");

nxtDisplayTextLine(line++, "DROD-Nx 1: S1");

nxtDisplayTextLine(line++, "DROD-Nx 2: S2");

nxtDisplayTextLine(line++, "Press orange...");


// wait for keypress

while(nNxtButtonPressed != ORANGE_BUTTON) {

wait10Msec(10);

}


wait10Msec(10);

eraseDisplay();

}



void initialiseRobot()

{

// clear the encoders on each motor

nMotorEncoder[DriveMotor] = 0;

nMotorEncoder[SteeringMotor] = 0;


checkBTLinkConnected();


obstacleLocation = kNoObstacles;

precomputeObstacleMap();


}


// Drive around and avoid hitting anything

void driveAround()

{


nxtDisplayTextLine(line++, "Driving.");


}


tObstacleLocation computeObstacleLocation(int reading) {


bool errorLeft, errorFront, errorRight;

tObstacleLocation o;


errorLeft = ( abs(kLeftObstacle - reading) <= DRODerror);

errorFront = ( abs(kFrontObstacle - reading) <= DRODerror);

errorRight = ( abs(kRightObstacle - reading) <= DRODerror);


o = kNoObstacles;


if(errorLeft) {

o |= kFrontLeft;

}

if(errorRight) {

o |= kFrontRight;

}

if(errorFront) {

o |= kFront;

}


return o;

}


// Draw an line map of the obstacles around the robot

// using segments to display the location of obstacles detected


void drawObstacleMap(tObstacleLocation obstacles) {


int i;


// first erase the screen

eraseDisplay();


// draw the central box that represents the robot

nxtDrawRect(ax, ay, ax+w, ay-h);


// now see which bits are set in the obstacle map and draw the line

// segment corresponding to those bits set to one

if(obstacles == kNoObstacles) {

return;

}


if(obstacles & kFrontLeft) {

nxtDrawLine(obstacleMap[0][0],

obstacleMap[0][1],

obstacleMap[0][2],

obstacleMap[0][3]);

}


if(obstacles & kFront) {

nxtDrawLine(obstacleMap[1][0],

obstacleMap[1][1],

obstacleMap[1][2],

obstacleMap[1][3]);

}


if(obstacles & kFrontRight) {

nxtDrawLine(obstacleMap[2][0],

obstacleMap[2][1],

obstacleMap[2][2],

obstacleMap[2][3]);

}


if(obstacles & kRearLeft) {

nxtDrawLine(obstacleMap[3][0],

obstacleMap[3][1],

obstacleMap[3][2],

obstacleMap[3][3]);

}


if(obstacles & kRear) {

nxtDrawLine(obstacleMap[4][0],

obstacleMap[4][1],

obstacleMap[4][2],

obstacleMap[4][3]);

}


if(obstacles & kRearRight) {

nxtDrawLine(obstacleMap[5][0],

obstacleMap[5][1],

obstacleMap[5][2],

obstacleMap[5][3]);

}


}


void precomputeObstacleMap() {


int i, j;



for(i=0; i < 6; i++) {

for(j=0; j < 4; j++) {

obstacleMap[i][j] = 0;

}

}


// obstacle segment 1

obstacleMap[0][0] = ax - t;

obstacleMap[0][1] = ay;

obstacleMap[0][2] = ax;

obstacleMap[0][3] = ay + t;


// obstacle segment 2

obstacleMap[1][0] = ax;

obstacleMap[1][1] = ay + t;

obstacleMap[1][2] = ax + w;

obstacleMap[1][3] = ay + t;


// obstacle segment 3

obstacleMap[2][0] = ax + w;

obstacleMap[2][1] = ay + t;

obstacleMap[2][2] = ax + w + t;

obstacleMap[2][3] = ay;


// obstacle segment 4

obstacleMap[3][0] = ax + w;

obstacleMap[3][1] = ay - h - t;

obstacleMap[3][2] = ax + w + t;

obstacleMap[3][3] = ay - h;


// obstacle segment 5

obstacleMap[4][0] = ax;

obstacleMap[4][1] = ay - h - t;

obstacleMap[4][2] = ax + w;

obstacleMap[4][3] = ay - h - t;


// obstacle segment 6

obstacleMap[5][0] = ax - t;

obstacleMap[5][1] = ay - h;

obstacleMap[5][2] = ax;

obstacleMap[5][3] = ay - h - t;

}


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


void checkBTLinkConnected()

{

if (nBTCurrentStreamIndex >= 0)

  return;


PlaySound(soundLowBuzz);

PlaySound(soundLowBuzz);

eraseDisplay();

nxtDisplayCenteredTextLine(3, "BT not");

nxtDisplayCenteredTextLine(4, "Connected");

wait1Msec(3000);

StopAllTasks();

}


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

//

//                                        Receive Messages

//

// Far end device is periodically sending message. Here we loop continuously absorbing/processing

// the message.

//

// Far end increments the message / mailbox by one over the last message sent. we can use this to

// validate that the messages arrive in order and are not dropped.

//

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


void readMessages()

{

static int nLastMessage = 0;


while (true)

{

  nMessage = message;

  if (nMessage != 0)

  {

  ++nNearEndRead;// Keep a running count of the number of messages successfully read

  if (nMessage != (nLastMessage + 1))

    ++nReadOutOfSequence;

  nLastMessage = nMessage;

  nFarEndSent  = messageParm[1];

  nFarEndRead  = messageParm[2];

  ClearMessage();

nElapsedTime  = nPgmTime;

}

else

  ++nRcxNoMsg;

wait1Msec(1);

}

}