//*!!Sensor,    S1,                  cam, sensorI2CCustomFast,      ,            !!*//
//*!!Motor,  motorA,              r_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!Motor,  motorC,              l_motor, tmotorNxtEncoderClosedLoop,           !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors cam                  = (tSensors) S1;   //sensorI2CCustomFast //*!!!!*//
const tMotor   r_motor              = (tMotor) motorA; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor   l_motor              = (tMotor) motorC; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

#include "nxtcamlib.c"

/************************************************************************************/
// blob_chase.c - fun demo of nxtcam using Robot C. Needs nxtcamlib.c.
// Gordon Wyeth
// 30 October 2007
// Updated 4/12/07 to use new version of nxtcamlib.c that works around Robot-C compiler
// bug.
//
// 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.
// The constants chosen work well with the standard NXT wheelchair robot with the
// camera mounted at the front of the robot looking down at the floor.
//
/************************************************************************************/

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