/*******************************************************/
/* HiTechnic I2C Compass Sensor - Simple Test Program
/* by Eric Salinas
/* nxtasy.org | a LEGO Mindstorms Community
/***************************************************/


/* Administrative *//////////////////////////

//Assign port
const tSensors kCmpsPort			= S1;

//Compass Address
const ubyte kCmpsAddr		 		= 0x02;

//Memory location for bearing/2
const ubyte kCmpsBearing1			= 0x42;

//Memory location for odd/even
const ubyte kCmpsBearing2 			= 0x43;


/* Retrieve Bearing *///////////////////////

int ReadBearing()
{
  int n = 0;
  int bearing[1];
  int cmpsRply[1];
  int cmpsMsg[3];

  const int kMsgSize 			= 0;
  const int kMsgAddr 			= 1;
  const int kMsgReadAddr 		= 2;

  //Construct I2C message for bearing
  cmpsMsg[kMsgSize] 			= 2;
  cmpsMsg[kMsgAddr]			= kCmpsAddr;

  //For loop iteration
  int i;

  for (i = 0; i < 2; i++)
  {
    //Decide which byte to retrieve
    if (i == 0)
    	cmpsMsg[kMsgReadAddr] = kCmpsBearing1;
    else
    	cmpsMsg[kMsgReadAddr] = kCmpsBearing2;

    //Wait
    while (nI2CStatus[kCmpsPort] == STAT_COMM_PENDING);

    //Send I2C message
    sendI2CMsg(kCmpsPort, cmpsMsg[0], 1);

    //Wait
    while (nI2CStatus[kCmpsPort] == STAT_COMM_PENDING);

    //Receive I2C reply
    readI2CReply(kCmpsPort, cmpsRply[0],1);

    //Store results in array
    bearing[i] = cmpsRply[0];
  }

  //Calculate bearing
  if (bearing[1] == 0)
  	n = (2*bearing[0]);
  else
  	n = (2*bearing[0])+1;

  return(n);
}


/* Main program *///////////////////////////

task main()
{
  //Declare sensor as type I2C custom
  //Thanks to Dick Swan for reminding me to do this!!
  SensorType[kCmpsPort] = sensorI2CCustom;

  int reading = 0;

  //Display header
  nxtDisplayTextLine(1, "HT Compass Test");
  nxtDisplayTextLine(3, " Connect to S1");

  //Continuously retrieve bearing
  while(1)
  {
  	//Call read function
  	reading = ReadBearing();

  	//Display results
  	nxtDisplayTextLine(5,"   Bearing: %d", reading);
  }

  StopAllTasks();
}