Programming NXT camera in C

-

As I mentioned in previously you need to download a new library called nxtcamlib in order for RobotC to be able to control the camera. This library has many functions that are useful when trying to write a program for the camera in C. “nxtcamlib.c” library was created by Gordon Wyeth to simplify the use of nxtcam and provide an easy to use interface. The two key functions are:

void init_camera (tSensors camera) - Initialises the camera so it is ready to find blobs. Assumes that the color map has been set up using nxtcamview or similar.

void get_blobs(tSensors camera, int &nblobs, int_array &color, int_array &left, int_array &top, int_array &right, int_array &bottom) - Fills the variables nblobs and bounding box coordinate arrays with current values from the camera. nblobs is loaded with the number of visible blobs. The arrays carry the color, leftmost pixel, topmost pixel, rightmost pixel, and bottommost pixel in each blob. Only the first nblobs in each array are valid. Old data will persist in the rest.

The functions camera_cmd() and camera_flush() might also be useful, but were more intended to make writing the above functions easier.

After downloading the library you need to add it to your RobotC directory in the include file. The path for mine is “C:\Program Files\Robotics Academy\ROBOTC for Mindstorms\Includes”. In case you cannot find it on the internet all you have to do is create a .c file and past this in it:

#define MAX_BLOBS 8

// I2C Constants associated with camera

const int I2C_addr = 0x02;

const int cmd_reg = 0x41;

const int count_reg = 0x42;

const int data_reg = 0x43;

// Data structure for storing blob information

typedef int int_array[MAX_BLOBS];

// void camera_flush() - flushes any pending reply bytes from camera.

void camera_flush(tSensors camera)

{

int n;

byte dump[8];

while (nI2CStatus[camera] == STAT_COMM_PENDING) ;

n = nI2CBytesReady[camera];

while (n > 0) {

while (nI2CStatus[camera] == STAT_COMM_PENDING);

readI2CReply(camera, dump[0], n);

while (nI2CStatus[camera] == STAT_COMM_PENDING);

n = nI2CBytesReady[camera];

}

}

// void camera_cmd(byte cmd) - sends cmd to the camera over I2C.

void camera_cmd(tSensors camera, byte cmd) {

const byte msg[] = {3, I2C_addr, cmd_reg, cmd};

while (nI2CStatus[camera] == STAT_COMM_PENDING);

sendI2CMsg(camera, msg[0], 0);

camera_flush(camera);

}

// void init_camera() - Initialises camera ready to find blobs.

void init_camera(tSensors camera)

{

SensorType[camera] = sensorI2CCustomFast;

camera_cmd(camera, 'A'); // Sort by size

camera_cmd(camera,'E'); // Start finding

}

// void get_blobs() - loads the current blobs into the global data structure over I2C

void get_blobs(tSensors camera, int &nblobs, int_array &color, int_array &left, int_array &top, int_array &right, int_array &bottom)

{

byte msg[3];

byte reply[5];

int i;

camera_flush(camera);

// Request number of blobs from the count register

msg[0] = 2;

msg[1] = I2C_addr;

msg[2] = count_reg;

while (nI2CStatus[camera] == STAT_COMM_PENDING);

sendI2CMsg(camera, msg[0], 1);

// Get the reply and put into nblobs global

while (nI2CStatus[camera] == STAT_COMM_PENDING);

while (nI2CBytesReady[camera] != 1);

readI2CReply(camera, reply[0], 1);

nblobs = reply[0];

camera_flush(camera);

// Get nblobs of blob data from the camera

for (i = 0; i <>

// Request blob data

msg[0] = 2;

msg[1] = I2C_addr;

msg[2] = data_reg + i * 5;

while (nI2CStatus[camera] == STAT_COMM_PENDING);

sendI2CMsg(camera, msg[0], 5);

// Get blob data reply

while (nI2CStatus[camera] == STAT_COMM_PENDING);

while (nI2CBytesReady[camera] != 5);

readI2CReply(camera, reply[0], 5);

// Put data into global variables.

// Casting to unsigned int throws a warning BUT casting a byte to int is quite

// different to casting to unsigned int, as the former performs sign extension

// (copies first bit of byte to higher bits to make it negative) and latter

// does not. Not sure what Robot C is doing now, but leave it for when they

// fix it. :)

color[i] = ((unsigned int)reply[0]) & 0x00FF;

left[i] = ((unsigned int)reply[1]) & 0x00FF;

top[i] = ((unsigned int)reply[2]) & 0x00FF;

right[i] = ((unsigned int)reply[3]) & 0x00FF;

bottom[i] = ((unsigned int)reply[4]) & 0x00FF;

camera_flush(camera);

}

}

Time: 7.5 hrs.