Magnetometer Working

A project log for Stubby the (Teaching) Hexapod

100% open source robot platform with accessability and affordability in mind: teaching children of all ages about robots & programming

The Big OneThe Big One 09/28/2014 at 05:280 Comments

The magnetometer arrived on Thursday (yay for Adafruit and their speedy shipping!); today I spent most of the day getting it working.

There were a few issues which I needed to overcome.

First, I had to read the raw values over i2c.  This is easy enough; the only gotcha was that I didn't read the data sheet close enough, and I failed to notice that the axis are read in X, Z, Y order rather than X, Y, Z. (Really?!  I wonder if there is a technical reason for this ordering, or what ).

After I got the raw values, I used atan2 to convert the x,y co-ordinates into an angle (heading).  The problem here was that (probably due to the magnetic fields of the servo motors) the Y axis was shifted very far down.  This meant that all the angles were being read as negative.  See a graph of the raw values below:

By adding a constant offset to x and y, we got the readings to be properly centered around the origin.

Next challenge was that I am operating in a magnetically noisy environment (there are 18 servos within centimeters of the magnetometer).  Implementing a low pass filter against headings is actually much harder than you would think.  (You have to remember the 'wrap around' at +/- PI).  In the end, I apply a low pass filter on the raw x, y co-ordinates.  A timer polls the magnetometer multiple times a second to ensure that the filtered heading is always accurate and up to date.

 Finally, once I had accurate, filtered headings available at any time, I was able to implement a simple PID algorithm which compares the actual heading to the desired heading and makes any required corrections to keep the robot on course.  This works very well: see for instance the video below, demonstrating the same programmed walk with and without using the magnetometer for course correction.

Some of my next steps include: