Implementing obstacle avoidance

I worked on implementing obstacle avoidance using an ultrasonic sensor mounted on the front of the NXT. I updated my code to show the implantation. After testing, the problem of directing the robot when there is an obstacle because complicated with only one sonar sensor.

#pragma config(Sensor, S4, sonarSensor, sensorSONAR)

#pragma config(Motor, motorA, r_motor, tmotorNormal, PIDControl, )

#pragma config(Motor, motorC, l_motor, tmotorNormal, PIDControl, )

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

#include "nxtcamlib.c"

/************************************************************************************/

// When a single blob is found in the image the robot will try to centre the blob by moving

// the motors. When more the one blob is found the robot halts and displays a message.

// Tracked image resolution of 88 x 144 pixels at 30 frames/second

// Perform full-resolution (176 x 144) pixels.

/************************************************************************************/

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;

// Initialise the camera

init_camera(S1);

while (true) {

// Get the blobs from the camera into the array

get_blobs(S1, 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 = 144 - y_centre;

// Drive the motors proportional to the error

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

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

}

else {

motor[l_motor] = 20;

motor[r_motor] = -20;

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

erased = false;

}

while (SensorValue[sonarSensor] <30>

motor[l_motor] = -5;

motor[r_motor] = -5;

wait10Msec(60);}

}

}

Time : 6 hrs.