Code: /*
Utility program to use with mindsensors.com CMPS-Nx V2.0 compass sensor.
For use with RobotC 1.05 or higher.
(c) mindsensors.com 2006, 2007
Revision History
Who When Why
-------------------------------------------------------------
Dr. Nitin Patil Sep-2006 Initial Authoring
Deepak Dec-08-2006 Compatibility related changes
Scott Briscoe Mar-10-2007 Port Input for Functions
Initialization Void
Brett Jun-14-2007 1.05 I2C Protical Update
Scott Briscoe Oct-21-2008 Error Void
-------------------------------------------------------------
for more info visit www.mindsensors.com
NXT I2C register storage:
location Rd contents Wr contents
--------------------------------------------------
0-7 v2.0 -
8-f mndsnsrs -
10-18 CMPS -
0x41 command
0x42 lsb heading data -
0x43 msb heading data -
0x44 X_offset lo X_offset lo
0x45 X_offset hi X_offset hi
0x46 Y_offset lo Y_offset lo
0x47 Y_offset hi Y_offset hi
0x48 X_range byte0 X_range byte0
0x49 X_range byte1 X_range byte1
0x4A Y_range byte0 Y_range byte0
0x4B Y_range byte1 Y_range byte1
0x4C X_raw byte0
0x4D X_raw byte1
0x4E Y_raw byte0
0x4F Y_raw byte1
Commands Action
ASCII Hex
A 0x41 0x02 Set AutoTrig mode On (default)
S 0x53 0x01 Set AutoTrig mode Off
B 0x42 Compassing Mode: Result in Byte (0-360 mapped to 0-255)
(default for Lego Sonar compatible mode)
I 0x49 Compassing Mode: Result in Integer (0-360 mapped to 0-36000)
(default for advanced mode)
E 0x45 Compassing Mode: Set Sampling frequency to 50Hz
(Europe standard)
U 0x55 Compassing Mode: Set Sampling frequency to 60Hz
(USA standard)
N 0x4E Set ADPA mode On
O 0x4F Set ADPA mode Off (default)
C 0x43 Begin Calibration mode
D 0x44 Calibration Done
L 0x4C Load user Calibration Value
The I2C Bus address on CMPS-Nx can be changed.
To set an address different from the default address:
Send sequence of folowing commands on the command register:
0xA0, 0xAA, 0xA5, <new I2C address>
Note: Send commands with no Read operation in between.
The new address is effective immediately.
*********************************************************************************************************/
#define CMPS_ID 0xc2 // Change this to match your compass address //
#define CMPS_COMMAND_REG 0x41
#define CMPS_READ_RESULT 0x42
#define CMPS_READ_HEADING 0x49
#define CMPS_CAL 0x43
#define CMPS_END_CAL 0x44
#define CMPS_SAMPLE_US 0x55
#define CMPS_SAMPLE_EU 0x45
#define CMPS_AUTO 0x41
#define CMPS_APA_ON 0x4E
#define CMPS_APA_OFF 0x4F
// Prototypes of functions in this file //
void CmpsError(const tSensors CmpsPort);
void CmpsCommand(const tSensors CmpsPort, byte cmpsCommand);
void InitializeCmps(const tSensors CmpsPort);
int CmpsHeading(const tSensors CmpsPort);
void TurnToHeading(const tSensors CmpsPort, int TargetHeading, int Accuracy,
tMotor LeftMotorHere, tMotor RightMotorHere);
// Setup Compass on a port
void InitializeCmps(const tSensors CmpsPort)
{
int test;
SensorType[CmpsPort] = sensorI2CMindsensorsCompass;
CmpsCommand(CmpsPort, CMPS_APA_ON);
// Make sure compass is plugged in
// If not, CmpsHeading will trigger CmpsError
test = CmpsHeading(CmpsPort);
}
// Displays compass error
void CmpsError(const tSensors CmpsPort)
{
nxtDisplayTextLine(4,"Cmps Error");
nxtDisplayTextLine(5,"Port: S%d ", ((int)CmpsPort) +1);
nxtDisplayTextLine(6,"Cmps Adr: 0x%x", CMPS_ID);
nxtDisplayTextLine(7,"Restart Program");
while (true) // Loop Forever, must restart application
{
PlaySound(soundException);
wait1Msec(10000);
}
}
// Send a command to the compass
void CmpsCommand(const tSensors CmpsPort, byte cmpsCommand)
{
byte cmpsMsg[5];
const byte MsgSize = 0;
const byte CmpsAddress = 1;
const byte CommandAddress = 2;
const byte Command = 3;
// Build the I2C message
cmpsMsg[MsgSize] = 3;
cmpsMsg[CmpsAddress] = CMPS_ID;
cmpsMsg[CommandAddress] = CMPS_COMMAND_REG ;
cmpsMsg[Command] = cmpsCommand;
while (nI2CStatus[CmpsPort] == STAT_COMM_PENDING)
{
wait1Msec(2); // Wait for I2C bus to be ready
}
sendI2CMsg(CmpsPort, cmpsMsg[0], 0); // Send the message
}
// get compass heading and return value
int CmpsHeading(const tSensors CmpsPort)
{
int Heading;
byte replyMsg[2];
byte cmpsMsg[5];
const byte MsgSize = 0;
const byte CmpsAddress = 1;
const byte ReadAddress = 2;
const byte CommandAddress = 2;
const byte Command = 3;
// tell sensor to update heading
cmpsMsg[MsgSize] = 3;
cmpsMsg[CmpsAddress] = CMPS_ID ;
cmpsMsg[CommandAddress] = CMPS_COMMAND_REG ;
cmpsMsg[Command] = CMPS_READ_HEADING;
while (nI2CStatus[CmpsPort] == STAT_COMM_PENDING)
{
wait1Msec(2); // Wait for I2C bus to be ready
}
sendI2CMsg(CmpsPort, cmpsMsg[0], 0); // Send the message
// tell sensor to reply back with heading value
cmpsMsg[MsgSize] = 2;
cmpsMsg[CmpsAddress] = CMPS_ID ;
cmpsMsg[ReadAddress] = CMPS_READ_RESULT ;
while (nI2CStatus[CmpsPort] == STAT_COMM_PENDING)
{
wait1Msec(2); // Wait for I2C bus to be ready
}
sendI2CMsg(CmpsPort, cmpsMsg[0], 2); // Send the message
//read heading value replied back
while (nI2CStatus[CmpsPort] == STAT_COMM_PENDING)
{
wait1Msec(2); // Wait for I2C bus to be ready
}
if (nI2CStatus[CmpsPort] != NO_ERR) // no reply, probably sensor is missing.
{
CmpsError(CmpsPort); // Display Error
}
readI2CReply(CmpsPort, replyMsg[0], 2); // Read reply
// Interpret heading value returned
Heading = (0x00FF & replyMsg[0]);
Heading += ((0x00FF & replyMsg[1])<<8);
return (Heading);
}
| 
Mindsensors Compass Driver
