View unanswered posts | View active topics It is currently Fri Aug 22, 2014 11:46 am






Reply to topic  [ 1 post ] 
Mindsensors Compass Driver 
Author Message
Moderator
Moderator
User avatar

Joined: Wed Jan 31, 2007 3:39 am
Posts: 299
Location: San Diego, California. USA
Post Mindsensors Compass Driver
Hello Everyone,

Here is my modified drivers for use with the mindsensors compass. It includes an error function that will be activated when the compass is not being communicated to. I also incorporated some code found in a RobotC sample programs to the display heading test, it will draw a compass to the screen and will change direction as the compass is moved. The original code was used for a Hitechnic compass.

Mindsensors Compass Driver and Examples.zip 3.10 Kb

Enjoy B-)
Scott

Driver Code:
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);
}


_________________
Mmmm Legos B-)

My Robot Projects:
http://www.freewebs.com/robotprojects/


Thu Nov 13, 2008 7:16 pm
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.