Let's Make Robots!

Rover 5: how to determine direction with a compass (LSM303)

I recently bought a Rover 5 and an Arduino Uno and made my first sketches. My goal is to let the Rover drive standalone in our livingroom, avoiding objects, childrens and pets ;-).

I gave the Rover a pan/tilt-system with an ultrasonic sensor to measure “free distance”. I also installed a compass (on the pan/tilt-system) to determine the “direction of the free space” (https://www.sparkfun.com/products/10703). The idea is to direct the Rover to the found free space using the compass information (and encoder outputs of the motors).

The use and programming of the compass however is complex (underestimated that ;-) and the examples that I found are complicated. I would appreciate your help here. How to determine directions in a horizontal pane with my compass (X, Y coordinates only)?

Thanks in advance. Regards, Ko

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Solved. Chris's remark about calibrating put me on the right track. Google led me to https://forum.sparkfun.com/viewtopic.php?f=14&t=32575.

Following the instructions resulted in correct headings:

·         LSM303 library installed

·         running sketch Calibrate + manually robot rotating => min / max  ​​X-Y-Z-values at serial monitor

·         Found values put in compass calibration lines @ void setup () of sketch Heading

·         Run sketch Heading => correct headings

Remark: the library was originally developed for "Pololu LSM303 implementations" but also does the trick for the Sparkfun implementation. 

One last question, though. The used I2C addresses are defined at the libraries header file. One of the registers has the same address as my LCD display:

·         http://www.dfrobot.com/wiki/index.php?title=I2C/TWI_LCD1602_Module_ (SKU: _DFR0063

·         Compass h-file: # define LSM303_STATUS_REG_A 0x27.

Everything seems to work fine but this looks to me as a potential conflict. I can set the address of the LCD display – but to what? What is the best way to deal with these kind of address issues , any rules of thumb?

Regards, Ko

You are now firmly on the correct path, my friend. The way you are approching this problem now --sitting down, making a chart and "guage", recording numbers and writing them down. Perfect! This is EXACTLY how you program robots. You got it.

As a matter of fact, think about what a mess it would be if you were trying to mix your motors in with all this. The robot would not work and you would spend hours tweaking the motors and trying to figure out why it keeps overshooting or undershooting. Now, you are armed with solid data. Your compass ain't workin' --and knowing is 1/2 the battle.

You get this compass issue fixed and your "turning to a heading" project will probably just work "automatically".

Good job figuring out this problem, dude.

Double check the manual on this one, but if I remember correctly, there is a calibration mode that needs to be done. It involves sending the compass a character (I think it is a "C" or maybe "!C") and the calibration mode is started. At that point, you rotate the compass around (by hand) a few times and boom, it works again.

I remember having to do this in the past, I also remember it solving a bunch of problems after I did it.

Also --double check the magnets in the room --My big stereo speakers threw my compass off quite a bit.

 

Hi,

I tried to follow Chris his suggestions (@ his reply Now we’re getting somewhere”). First I put the robot with its front towards south and rotated it 360 degrees. On the monitor, I saw no linear relation between the headings and I decided to do an experiment.

I drew a “degree distribution grid” on a sheet of paper and placed the robot in the centre heading south (heading = 180).
Then I rotated the robot 45 degrees clockwise and noted the headings in the “degree grid”.
I repeated that (4 times) until the robot was back at its starting position.

See picture for results. 
It seems that turning "above the center line" per quarter results in a "heading difference" of 60 degrees. And "below the center line" of 120 degrees.

How can this be explained?
And how to get a linear degree distribution?

For the sake of completeness, I added the executed code below

Thanks in advance for your reactions.

Puzzled regards, Ko

 

 

/* LSM303DLH Example Code
   by: Jim Lindblom
   SparkFun Electronics
   date: 9/6/11
   license: Creative commons share-alike v3.0
  
   Summary:
   Show how to calculate level and tilt-compensated heading using
   the snazzy LSM303DLH 3-axis magnetometer/3-axis accelerometer.
  
   Firmware:
   You can set the accelerometer's full-scale range by setting
   the SCALE constant to either 2, 4, or 8. This value is used
   in the initLSM303() function. For the most part, all other
   registers in the LSM303 will be at their default value.
  
   Use the LSM303_write() and LSM303_read() functions to write
   to and read from the LSM303's internal registers.
  
   Use getLSM303_accel() and getLSM303_mag() to get the acceleration
   and magneto values from the LSM303. You'll need to pass each of
   those functions an array, where the data will be stored upon
   return from the void.
  
   getHeading() calculates a heading assuming the sensor is level.
   A float between 0 and 360 is returned. You need to pass it a
   array with magneto values.
  
   getTiltHeading() calculates a tilt-compensated heading.
   A float between 0 and 360 degrees is returned. You need
   to pass this function both a magneto and acceleration array.
  
   Headings are calculated as specified in AN3192:
   http://www.sparkfun.com/datasheets/Sensors/Magneto/Tilt%20Compensated%20Compass.pdf
  
   Hardware:
   I'm using SparkFun's LSM303 breakout. Only power and the two
   I2C lines are connected:
   LSM303 Breakout ---------- Arduino
         Vin                   5V
         GND                   GND
         SDA                   A4
         SCL                   A5
*/
#include <Wire.h>
#include <math.h>

#define SCALE 2  // accel full-scale, should be 2, 4, or 8

/* LSM303 Address definitions */
#define LSM303_MAG  0x1E  // assuming SA0 grounded
#define LSM303_ACC  0x18  // assuming SA0 grounded

#define X 0
#define Y 1
#define Z 2

/* LSM303 Register definitions */
#define CTRL_REG1_A 0x20
#define CTRL_REG2_A 0x21
#define CTRL_REG3_A 0x22
#define CTRL_REG4_A 0x23
#define CTRL_REG5_A 0x24
#define HP_FILTER_RESET_A 0x25
#define REFERENCE_A 0x26
#define STATUS_REG_A 0x27
#define OUT_X_L_A 0x28
#define OUT_X_H_A 0x29
#define OUT_Y_L_A 0x2A
#define OUT_Y_H_A 0x2B
#define OUT_Z_L_A 0x2C
#define OUT_Z_H_A 0x2D
#define INT1_CFG_A 0x30
#define INT1_SOURCE_A 0x31
#define INT1_THS_A 0x32
#define INT1_DURATION_A 0x33
#define CRA_REG_M 0x00
#define CRB_REG_M 0x01
#define MR_REG_M 0x02
#define OUT_X_H_M 0x03
#define OUT_X_L_M 0x04
#define OUT_Y_H_M 0x05
#define OUT_Y_L_M 0x06
#define OUT_Z_H_M 0x07
#define OUT_Z_L_M 0x08
#define SR_REG_M 0x09
#define IRA_REG_M 0x0A
#define IRB_REG_M 0x0B
#define IRC_REG_M 0x0C

/* Global variables */
int accel[3];  // we'll store the raw acceleration values here
int mag[3];  // raw magnetometer values stored here
float realAccel[3];  // calculated acceleration values here

void setup()
{
  Serial.begin(9600);  // Serial is used for debugging
  Wire.begin();  // Start up I2C, required for LSM303 communication
  initLSM303(SCALE);  // Initialize the LSM303, using a SCALE full-scale range
}

void loop()
{
  getLSM303_accel(accel);  // get the acceleration values and store them in the accel array
  while(!(LSM303_read(SR_REG_M) & 0x01))
    ;  // wait for the magnetometer readings to be ready
  getLSM303_mag(mag);  // get the magnetometer values, store them in mag
  //printValues(mag, accel);  // print the raw accel and mag values, good debugging
 
  for (int i=0; i<3; i++)
    realAccel[i] = accel[i] / pow(2, 15) * SCALE;  // calculate real acceleration values, in units of g
 
  /* print both the level, and tilt-compensated headings below to compare */
  Serial.print(getHeading(mag), 3);  // this only works if the sensor is level
  Serial.print("\t\t");  // print some tabs
  Serial.println(getTiltHeading(mag, realAccel), 3);  // see how awesome tilt compensation is?!
  delay(100);  // delay for serial readability
}

void initLSM303(int fs)
{
  LSM303_write(0x27, CTRL_REG1_A);  // 0x27 = normal power mode, all accel axes on
  if ((fs==8)||(fs==4))
    LSM303_write((0x00 | (fs-fs/2-1)<<4), CTRL_REG4_A);  // set full-scale
  else
    LSM303_write(0x00, CTRL_REG4_A);
  LSM303_write(0x14, CRA_REG_M);  // 0x14 = mag 30Hz output rate
  LSM303_write(0x00, MR_REG_M);  // 0x00 = continouous conversion mode
}

void printValues(int * magArray, int * accelArray)
{
  /* print out mag and accel arrays all pretty-like */
  Serial.print(accelArray[X], DEC);
  Serial.print("\t");
  Serial.print(accelArray[Y], DEC);
  Serial.print("\t");
  Serial.print(accelArray[Z], DEC);
  Serial.print("\t\t");
 
  Serial.print(magArray[X], DEC);
  Serial.print("\t");
  Serial.print(magArray[Y], DEC);
  Serial.print("\t");
  Serial.print(magArray[Z], DEC);
  Serial.println();
}

float getHeading(int * magValue)
{
  // see section 1.2 in app note AN3192
  float heading = 180*atan2(magValue[Y], magValue[X])/PI;  // assume pitch, roll are 0
 
  if (heading <0)
    heading += 360;
 
  return heading;
}

float getTiltHeading(int * magValue, float * accelValue)
{
  // see appendix A in app note AN3192
  float pitch = asin(-accelValue[X]);
  float roll = asin(accelValue[Y]/cos(pitch));
 
  float xh = magValue[X] * cos(pitch) + magValue[Z] * sin(pitch);
  float yh = magValue[X] * sin(roll) * sin(pitch) + magValue[Y] * cos(roll) - magValue[Z] * sin(roll) * cos(pitch);
  float zh = -magValue[X] * cos(roll) * sin(pitch) + magValue[Y] * sin(roll) + magValue[Z] * cos(roll) * cos(pitch);

  float heading = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
    return heading;
  else
    return (360 + heading);
}

void getLSM303_mag(int * rawValues)
{
  Wire.beginTransmission(LSM303_MAG);
  Wire.send(OUT_X_H_M);
  Wire.endTransmission();
  Wire.requestFrom(LSM303_MAG, 6);
  for (int i=0; i<3; i++)
    rawValues[i] = (Wire.receive() << 8) | Wire.receive();
}

void getLSM303_accel(int * rawValues)
{
  rawValues[Z] = ((int)LSM303_read(OUT_X_L_A) << 8) | (LSM303_read(OUT_X_H_A));
  rawValues[X] = ((int)LSM303_read(OUT_Y_L_A) << 8) | (LSM303_read(OUT_Y_H_A));
  rawValues[Y] = ((int)LSM303_read(OUT_Z_L_A) << 8) | (LSM303_read(OUT_Z_H_A)); 
  // had to swap those to right the data with the proper axis
}

byte LSM303_read(byte address)
{
  byte temp;
 
  if (address >= 0x20)
    Wire.beginTransmission(LSM303_ACC);
  else
    Wire.beginTransmission(LSM303_MAG);
   
  Wire.send(address);
 
  if (address >= 0x20)
    Wire.requestFrom(LSM303_ACC, 1);
  else
    Wire.requestFrom(LSM303_MAG, 1);
  while(!Wire.available())
    ;
  temp = Wire.receive();
  Wire.endTransmission();
 
  return temp;
}

void LSM303_write(byte data, byte address)
{
  if (address >= 0x20)
    Wire.beginTransmission(LSM303_ACC);
  else
    Wire.beginTransmission(LSM303_MAG);
   
  Wire.send(address);
  Wire.send(data);
  Wire.endTransmission();
}

 

If the output is x and y magentometer readings, you can get the heading by:

rawH = (atan2(y, x)) ;
 // Correct for when signs are reversed.
 if (rawH < 0) {
   rawH += 2*PI;
 }
This will give you a heading, in radians, between -pi and +pi.
If your output is noisy, e.g., the bearing jumps around a lot, rather than staying steady, I just posted a short tutorial on a simple noise filter that I use on my compass output: http://www.mcgurrin.com/robots/?p=154
By the way, it turns out that a compass is not a good sensor for headings in the short run (too prone to both noise and external errors).  I gather that it's good to correct for drifts in the output of gyros found in inertial measurement units.  I haven't had first hand experience with those, though.  To start, I used wheel encoders for both distance and heading, then added the compass for a more accurate and precise heading.

 

 

Well, the good news is that you have a working compass and there are numbers coming out of it. Really, you are just looking at good 'ol fashoned code. As basic as one could go, it would be:

  • Read the compass
  • If the compass heading is greater than where we want to be, turn one direction
  • if the compass heading is less than where we want to be, turn the other direction
  • During the turn, keep checking the compass and stop when you get to your number.

I would start there. Physically stick the robot facing south (heading 180) and then have it turn itself north and stop. You may need to stop early to prevent an over-shoot. If you can get this basic move down, you can then add code to calculate the distance from your target --taking into effect that you might cross the 359/0 mark and have your bot turn accordingly. You can add PID routines etc and get as complicated as you want, but again, I would start with a simple, "turn to a heading and stop" code before you try anything else.

 

Thank for your reactions. I relocated the compass -put it on a stick horizontally 25 cm above the rover platform.

The compass is connected (via I2C) with the UNO. I have the Sparkfun example code running but don’t know what to do with the raw data (on my serial monitor ;-) in order to determine the needed directions for “guiding” the Rover motors to the free space in the room (direction the Rover was driving AND direction of the greatest free space I want the Rover tot direct to).

Bajdi, the behavior you described is indeed what I am striving at. Is it possible to have a look at your code?

Apart from that I would like to understand the possibilities of my compass with respect to direction determination on a horizontal plan. Suggestions and tips are more than welcome.

Tomorrow when it is light ;-) I shall add some pictures of my robot to this post.

  1. Regards, Ko

I actually have a sketch for my Rover that does something like you want. The Rover stops when the distance measured is smaller then 30cm. It then very slowly rotates 360° and scans the area. After the 360° turning it will drive in the direction with the biggest distance measured. Was actually very easy to program. I have 4 compass sensors and have used 2 (MAG3110 and HMC5883L) on my Rover. Still have to try out the CMPS10 and GY-26.

I just downloaded the example code from SparkFun, and yes, to a newbie it does have a lot of sorta funny stuff in it. That said, I didn't have to change any of it. It just compiled and started spitting numbers out on the serial monitor (I have the same compas BTW). 

Confusing code aside, did it compile and run? Did it spit out numbers to the serial monitor?

First off, get the compass off of the pan and tilit and away from motors. Second, even if it has tilt compensation, keep it level. Why would you want to tilt a compass? Actually, why would you want to rotate (pan) it either? 

Search LMR for other folks using a compas and you will quickly find that everyone has them up on sticks, high and away from the chassis/ motors.

Finally, what have you done so far? Have you comunicated with it? Have you searched for any of the Arduino Wire (i2c) stuff? Do you know what i2c communication is? Do you have it wired? What part specifically are you having problems with in terms of the examples? What examples are you using? Have you searched for other examples? Do the examples not compile? Do they compile and not work?

C'mon, dude. GIGO