/*
Utility program to use with mindsensors.com CMPS-Nx V2.0 compass sensor.
For use with RobotC 1.0.08 or higher.
This Demo uses the address of 0x02 for the CMPS-Nx
assuming NxT screen with 0.1 degree resolution

(c) mindsensors.com 2006, 2007
Revision History
Who                When           Why
-------------------------------------------------------------
Dr. Nitin Patil    Sep-2006       Initial Authoring
Deepak             Dec-08-2006    Compatibility related changes
-------------------------------------------------------------
*/
/*
for more info visit www.mindsensors.com
Watch Demo Video:
http://www.mindsensors.com/images/NXT-with-mindsensors-compass.mpg
*/

/*

NXT I2C register storage

*/


#define    CmpsID               0xa8
#define    CmpsCommandReg       0x41
#define    CmpsReadResult       0x42
#define    CmpsReadHeading      0x49
#define    CmpsCAL              0x43
#define    CmpsEndCAL           0x44
#define    CmpsSampleUS         0x55
#define    CmpsSampleEU         0x45
#define    CmpsAuto             0x41

#define    CmpsPort             S1    // Connect CMPS sensor to this port!!
#define       touchPort            S1


//////////////////////////////////////////////////////////////////////////////
//
//      Read the heading  data from Compass sensor
//
//
/////////////////////////////////////////////////////////////////////////////

int  cmpsHeading()
{
  byte replyMsg[2];
    int Heading;
  byte cmpsMsg[5];
  const byte MsgSize        = 0;
  const byte CmpsAddress    = 1;
  const byte ReadAddress    = 2;
  const byte CommandAddress = 2;
  const byte Command        = 3;

  // Build the I2C message
  cmpsMsg[MsgSize]          = 3;
  cmpsMsg[CmpsAddress]      = CmpsID ;
  cmpsMsg[CommandAddress]   = CmpsCommandReg ;
  cmpsMsg[Command]          = CmpsReadHeading;

  while (nI2CStatus[S1] == STAT_COMM_PENDING);   // Wait for I2C bus to be ready

  sendI2CMsg(S1, cmpsMsg[0], 0);                    // Send the message

  cmpsMsg[MsgSize]        = 2;
  cmpsMsg[CmpsAddress]    = CmpsID ;
  cmpsMsg[ReadAddress]    = CmpsReadResult ;

  while (nI2CStatus[S1] == STAT_COMM_PENDING);    // Wait for I2C bus to be ready
  sendI2CMsg(S1, cmpsMsg[0], 2);                    // Send the message

  while (nI2CStatus[S1] == STAT_COMM_PENDING);     // Wait for I2C bus to be ready
  if ( nI2CStatus[S1] != NO_ERR ) {  // probably sensor is missing.
    return (-1);
  }
  readI2CReply(S1, replyMsg[0], 2);

   if ( replyMsg[0] == -1 ) { // -1 is reserved to report errors, so
   Heading = 0; // change it to zero.
   }
   else {
   Heading = ( 0x00FF & replyMsg[0] );
   }
   Heading += ( (0x00FF & replyMsg[1]) <<8 );
   return (Heading);
}

//////////////////////////////////////////////////////////////////////////////
//
//     send command to Compass sensor
//
/////////////////////////////////////////////////////////////////////////////

void cmpsCommand(byte cmpsCommand)
{
  byte cmpsMsg[5];
  const byte MsgSize        = 0;
  const byte CmpsAddress    = 1;
  const byte ReadAddress    = 2;
  const byte CommandAddress = 2;
  const byte Command        = 3;

  // Build the I2C message
  cmpsMsg[MsgSize]          = 3;
  cmpsMsg[CmpsAddress]      = CmpsID;
  cmpsMsg[CommandAddress]   = CmpsCommandReg ;
  cmpsMsg[Command]          = cmpsCommand;

  while (nI2CStatus[S1] == STAT_COMM_PENDING);   // Wait for I2C bus to be ready
  sendI2CMsg(S1, cmpsMsg[0], 0);                    // Send the message
}

//////////////////////////////////////////////////////////////////////////////
//
//      Display the heading and touch state on Nxt screen
//
/////////////////////////////////////////////////////////////////////////////

task main()
{

  int currheading;
  bool in = true;

   nI2CBytesReady[S1] = 0;




  while(true)   //just keep running
  {
      SensorType[S1] = sensorTouch;

     if(!SensorValue(touchPort) && in){

       nxtDisplayTextLine(2,"Touch: Out");
       in = false;

     }else if(SensorValue(touchPort) && !in){

       nxtDisplayTextLine(2,"Touch: In");
       in = true;

     }


     SensorType[S1] = sensorI2CCustomFast9V;

     currheading = cmpsHeading();
     if ( currheading == -1 ) {
         nxtDisplayTextLine(5,"Error: Check connections");
     } else {
        nxtDisplayTextLine(5,"Read OK");
     }
     nxtDisplayTextLine(6,"Heading = %d", currheading / 10);

  }
  StopAllTasks();
}

// End of Program.