Leader-Follower Algorithm using NXTCam

To start the Leader-Follower algorithm first we need to see the object and then move the motors so the object is in the center and then follow it. Finding the object was explained in previous blogs, the code below starts off by centering the object in the middle. I will be modifying this code to complete the mission.

const tSensors cam = (tSensors) S1; //sensorI2CCustomFast

const tMotor r_motor = (tMotor) motorA;

const tMotor l_motor = (tMotor) motorC;

#include "nxtcamlib.c"

task main ()

{

int nblobs; // Number of blobs

int_array bc, bl, bt, br, bb;

int x_centre, x_error;

int y_centre, y_error;

bool erased = false;

int i, sq, n, width, height;

// Initialise the camera

init_camera(cam);

while (true) {

// Get the blobs from the camera into the array

get_blobs(cam, nblobs, bc, bl, bt, br, bb);

if (nblobs == 1) {

if (!erased) {

nxtDisplayTextLine(0,"Tracking ...");

erased = TRUE;

}

// Find the centre of the blob using double resolution of camera

x_centre = bl[0] + br[0];

y_centre = bt[0] + bb[0];

// Compute the error from the desired position of the blob (using double resolution)

x_error = 176 - x_centre;

y_error = 143 - y_centre;

// Drive the motors proportional to the error

motor[l_motor] = (y_error - x_error) / 5;

motor[r_motor] = (y_error + x_error) / 5;

} else {

motor[l_motor] = 0;

motor[r_motor] = 0;

nxtDisplayTextLine(0,"Found %d blobs.",nblobs);

erased = FALSE;

}

}

}

Time: 6.5 hrs