typedef struct
{
  byte nMsgSize;
  byte nDeviceAddress;
  byte nRegisterIndex;
  byte nData;
} TI2C_Output;

void CalibrateColorSensor(tSensors ColorSensor, int Mode)
{
  TI2C_Output sOutput;

  switch(Mode)
  {
    // white point calibration
    // sensor should be positioned about a half inch above a white surface
     case 1:
      sOutput.nData = 0x43; // white calibration
      break;

    // black point calibration
    // sensor should be pointed in a direction with no near objects
    case 2:
      sOutput.nData = 0x42; // black point calibration
      break;

    default:
      return;
  }
  sOutput.nMsgSize = 3;
  sOutput.nDeviceAddress = 0x02;
  sOutput.nRegisterIndex = 0x41;
  sendI2CMsg(ColorSensor, sOutput.nMsgSize, 0);
  return;
}

#pragma config(Sensor, S1,     HTCS_PORT,           sensorI2CCustomStd)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
    Name: HTCS-calibrate.c
    Author: Xander Soldaat
    Date: 05/02/2009
    Version: 0.2
    Contact: mightor_at_gmail.com

    Address      Type      Contents
      00 - 07H   chars      Sensor version number
      08 - 0FH   chars      Manufacturer
      10 - 17H   chars      Sensor type
      18 - 3DH   bytes      Not used
      3E, 3FH      chars      Reserved
      40H            byte      Not used
      41H            byte      Reserved
      42H            byte      Color number
      43H            byte      Red reading
      44H            byte      Green reading
      45H            byte      Blue reading

    More information is available at www.hitechnic.com or by email at support@hitechnic.com
*/

byte cmdbuff[4];
byte reply[8];
byte _tmp;

task main() {

  while (nI2CBytesReady[HTCS_PORT] > 0)
    readI2CReply(HTCS_PORT, _tmp, 1);

  while(nI2CStatus[HTCS_PORT]== STAT_COMM_PENDING)
  wait1Msec(2);

  cmdbuff[0] = 2;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x10;                        // Set write address to calibration register

  sendI2CMsg(HTCS_PORT, cmdbuff[0], 8);
  while(nI2CStatus[HTCS_PORT]== STAT_COMM_PENDING)
    wait1Msec(2);

  eraseDisplay();
  if (nI2CStatus[HTCS_PORT] != NO_ERR) {
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "talk to sensor.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }

  readI2CReply(HTCS_PORT, reply[0], 8);

  cmdbuff[0] = 3;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x41;                        // Set write address to calibration register
  cmdbuff[3] = 0x43;                        // The data to be written

  eraseDisplay();
  nxtDisplayTextLine(2, "Calibr. %s", reply);
  PlaySound(soundFastUpwardTones);
  while(bSoundActive);
  sendI2CMsg(HTCS_PORT, cmdbuff[0], 0);    // Send command to protoboard
  wait1Msec(1000);
  if (nI2CStatus[S1] == ERR_COMM_BUS_ERR) {
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "calibrate sensor.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  } else {
    nxtDisplayTextLine(4, "Done");
    PlaySound(soundBeepBeep);
    while(bSoundActive);
  }
  wait1Msec(1000);
}