View unanswered posts | View active topics It is currently Thu Sep 18, 2014 12:16 am






Reply to topic  [ 1 post ] 
Sample code - using a Mindsensors DROD-Nx 
Author Message
Rookie

Joined: Sun Jul 22, 2007 6:32 pm
Posts: 16
Location: Ireland
Post Sample code - using a Mindsensors DROD-Nx
Hi all,

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.

You can read more about it at http://mastincrosbie.com/markclego/index.php/?p=39

I hope others find this useful... comments welcome!

Mark

//*!!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();

//
// 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 main()
{
// say hello
sayHello();

initialiseRobot();

StartTask(detectObstacles);

while(1) {
//nxtDisplayTextLine(1, "Obstacle: %d", obstacleLocation);
drawObstacleMap(obstacleLocation);
wait10Msec(30);
}
//motor[DriveMotor] = standardDriveSpeed;
//wait10Msec(200);
//motor[DriveMotor] = 0;

}

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;

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;
}


Mon Feb 11, 2008 1:46 pm
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.