//////////////////////////////////////////////////////////////////////////////////////////////
//                CMUCAM Operation with VEX and FRC Controllers
//
// Short program to demonstrate operation of the CMUCAM camera.
//////////////////////////////////////////////////////////////////////////////////////////////

// Load the "configuration program" generated camera  settings
#include "CameraConfiguration.h"

task main()
{
  int horizontal;
  int vertical;
	int nTotalNumbOfPackets = 0;

	// Wait for the camera to initialize.
	while (nCameraFields[camTrackingStatus] != camStatusTracking)
	{
		// Wait for the camera to begin tracking
		wait1Msec(10);
	}
	wait1Msec(3000);   // Let the camera stabilize
	time1[T1] = 0;     // Reset timer

	// This is the main loop. It will loop forever polling the camera for new data and acting on it.

	while (true)
	{
		// Same packet that was previously received. No processing required. We should wait until new data is received.
		while (nTotalNumbOfPackets == nCameraFields[camNumbTrackingRecords])
		{
			wait1Msec(5); // Wait a little time so that we don't use all CPU time.
		}
		// Store the coordinates of the blob detected by the camera. The center of the screen is normalized to point (0, 0)
		horizontal = nCameraFields[camHorizontal];  //Assigns the Horizontal value to a variable - Helps with debugging
		vertical   = nCameraFields[camVertical];  //Assings the Vertical value to a variable - Helps with debugging
		nTotalNumbOfPackets = nCameraFields[camNumbTrackingRecords];

		///////////////////////////////////////////////////////////
		//   Place User Code Below to Make the Robot Track an Object
		///////////////////////////////////////////////////////////


		///////////////////////////////////////////////////////////
		//   End User Code
		///////////////////////////////////////////////////////////
	}
}