//*!!CLICK to edit 'wizard' created sensor & motor configuration.                !!*//

//////////////////////////////////////////////////////////////////////////////////////////
//
//               Hitechnic Digit Compass Sensor Example
//               ======================================
//
// This file is a small application program that tests the NXT Compass Sensor
// from HiTechnic.
//
// The program simply configures sensor 1 as a Hitechnic Compass. It then
// reads the value of the compass and displays it on the NXT LCD screen.
//
// This example would typically	 represent the user written part of an application
// using the compass sensor.
//
// The actual "device driver" for the compass sensor would typically be
// supplied (and written) by the custom sensor manufacturer. A user program
// only needs to "#include" this vendor supplied file in their program to
// gain full support for this 3rd party device.
//
//////////////////////////////////////////////////////////////////////////////////////////

//
// the following "#nclude" directive will cause the Compass device driver
// to be 'added' to the user program
//
#include "NXT Compass Sensor Driver.c"

task main()
{
	int nCompassHeading;
	int nCycles = 0;

	//
	// We want the compass to be on port "S1"
	//
	const tSensors kCompass = S1;

	//
	// Initialization Code
	//
	// Program needs to do the following:
	//   1. Configure the compass as a custom sensor. This will tell the NXT firmware
	//      to disable its internal device drivers for the selected sensor so that they
	//      won't conflict with the custom device driver in this program.
	//
	//   2. Cofigure the sensor sub-type to a compass.
	//
	//   3. Start the separate task (found in the 'include' file) that is the device
	//      driver. This task will now run in parallel with this application program.
	//
	// Currently these three steps require manual placemet of this code. A future
	// version of RobotC will automate this functionality as part of the "wizard" used
	// to configure motors and sensors (see menu item "View -> Motors and Sensors Setup"
	//
	SensorType[kCompass]    = sensorI2CCustomStd;
	SensorSubType[kCompass] = subTypeHiTechnicCompass;
	StartTask(taskCompassDeviceDriver);


	//
	// The "program".
	//
	// It simply loops forever reading the value of the compass and displaying the results
	// on the NXT's LCD display. It also displays a count of the number of iterations through
	// the loop.
	//
	nxtDisplayTextLine(3, "Compass Port: %d", kCompass);

	while (true)
	{
		++nCycles;
		nCompassHeading = SensorValue[kCompass];
		nxtDisplayTextLine(5, "Heading: %d", nCompassHeading);
		nxtDisplayTextLine(7, "Cycles: %d", nCycles);
  }
  return;
}