SparkFun Forums 

Where electronics enthusiasts find answers.

Have questions about a SparkFun product or board? This is the place to be.
Hi all, Matt here--

I am in the Authentic Science Research Program at my high school and one of my projects is creating a scale AGV and I need a little help debuging my code to make a servo change direction based on the orientation of an on board compass. I have written a simple program to attempt to do this, only to have mixed results. When I have the compass oriented at 0 degrees, the wheels should straighten (controlled by the servo). When the compass is anywhere between 1 and 180, the wheels should turn right. When the compass is between 181 and 359, the servo should turn the wheels to the left.

Turning to the right works fine, and straightening is ok, but I might make the range between 355 and 5. When it should turn left, it does until it is rotated to a little more than 270 degrees and it turns right again. Can anyone help me debug the program below? Thanks!!

And by the way, I am using an Arduino Mega 2560.
Code: Select all
/*Code to make the Arduino turn the servo in the "target" direction
based on the current orientation

Created on July 17th, 2012


//Begining code is to initialize the compass and find the current orientation

#include <Wire.h>  //includes the wire library for compass
#include <Servo.h>  //includes the servo library to PWM the servo

int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41; //"A" in hex, A command is:

float GOAL = 90; //Taget of the compass (where to orient to)

Servo Steering; //defines the servo to be used to steer the car

int headingValue;

void setup(){
  Steering.attach(10);  //attaches the servo to pin 10
  // "The Wire library uses 7 bit addresses throughout.
  //If you have a datasheet or sample code that uses 8 bit address,
  //you'll want to drop the low bit (i.e. shift the value one bit to the right),
  //yielding an address between 0 and 127."
  HMC6352SlaveAddress = HMC6352SlaveAddress >> 1; // I know 0x42 is less than 127, but this is still required


void loop(){
  //"Get Data. Compensate and Calculate New Heading"
  Wire.write(HMC6352ReadAddress);              // The "Get Data" command

  //time delays required by HMC6352 upon receipt of the command
  //Get Data. Compensate and Calculate New Heading : 6ms

  Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB

  //"The heading output data will be the value in tenths of degrees
  //from zero to 3599 and provided in binary format over the two bytes."
  byte MSB =;
  byte LSB =;

  float headingSum = (MSB << smiley-cool + LSB; //(MSB / LSB sum)
  float headingInt = headingSum / 10;

  //Serial.println(" degrees");

  float OrientationDiff = GOAL - headingInt;
  //defining OrientationDiff as the target minus the current orientaion
  Serial.println(" degrees");
  if (OrientationDiff == 0)            //if the difference equals 0
    Steering.write(90);                //go straight
  else if ((OrientationDiff < 180) && (OrientationDiff > 0))  //if the difference is greater than 0 and less than 180
    Steering.write(180);               //turn right
  else                                 //anything else (if the difference is less than 0 and greater than 180
     Steering.write(0);                //turn left
Before looking at the code, make sure that the HMC6352 is outputting a heading that looks roughly correct.

If not, it needs calibration. The sensor has onboard calibration which you can try. If you can't get that to work, you can do your own calibration. Search for "quick and dirty compass calibration" and you will find that some clown (me) wrote a little how to out there :D

Hopefully smiley-cool == 8 ?

The OrientationDiff will almost never be 0 when you're dealing with a sensor that operates in tenths with a lot of noise.
When I have the compass oriented at 0 degrees, the wheels should straighten (controlled by the servo). When the compass is anywhere between 1 and 180, the wheels should turn right. When the compass is between 181 and 359, the servo should turn the wheels to the left.
Your code has it set up to go "straight" when heading is 90° -- is the compass oriented 90° off?

Your if-then-else logic may need a little refinement. To double-check, what is the value of OrientationDiff when heading is...

350, 0, 10, 80, 90, 100, 170, 180, 190, 260, 270, 280

What's the range of possible values for OrientationDiff given that heading ranges from 0 to 359.9 ?

Does the logic work like you want in those cases?
Thank you very much for the response!!

Quick question for you; there is an arrow with a "n" pointing towards the compass on one side--I am not sure if this means that that specific side is where the reading gets taken from or if the direction the arrow is pointing is where the reading comes you know which is true?

and also, we are using two different compasses, so would your calibration code still work with the HMC6352 rather than the HMC6352?

Lastly, I attempted to make a range rather than have OrientationDiff set at zero and then I noticed two things:

1)The difference can be negative, which pains me dearly because I sort of messed up the last "else if"

2) The compass (when no delay between each reading) can sway 40 degrees in each direction...sometimes more--do you think this is a calibration problem? And if so, how do I fix it?

Thank you very much!!

Does the compass swing +- 40 when the robot is still and the servos are NOT plugged in? If it does not, the chances are very good that the magnetic field generated by the motors in the servos are causing the swing. A magnetic compass works just like a boy scout compass. If you pas a magnet near it, that field is MUCH stronger than the earth's field and the compass points towards the magnet.

Moving the compass as far away from the servos as possible (feet if not yards or miles) would be good. If not possible, only take sensor readings when the motors are off and not turning. If that is not possible, isolate the servos in a MuMetal box.
Thanks for the suggestion, because you are right!!!

the servos are the problem.....and they are placed pretty far away......

and I'm not even running the much larger motor behind the compass....could a dc motor that isn't moving create a large enough magnetic field too?

I will look into that box!!!

Thank you so much!!
I have a hundred horror stories about magnetometers including the metal in clothing (zippers, buttons, ...) getting magnetized and screwing up the heading. And we will not talk about the guy with those arthritis magnet bracelets...

Buying some MuMetal foil and wrapping the servos in it will help. Just don't wrap the sensor in MuMetal or it will not see the earth's magnetic field!
Moving them apart as much as possible will help. This is often why you see a sensor mast on some robots.
sora62896 wrote:If I put the servo in the mumetal foil, would I have to ground the servo to it as well?
No, but the MuMetal must completely surround the servo. This is a 'magnetic, not electrostatic, shield which its intent is to contain the magnetic field produced by the servo.
My experience is that this is not a 'cure all' and in most cases does not cure the problem.
The other solutions recommended work much better.
That would help and may completely solve the problem.
Magnetic fields, like all EM fields, decrease be the inverse square of the distance (1/(d^2)). So the further away the weaker the field.

You really need to experiment to determine how far away for the servos the compass needs to be.
It depends on too many factors to easily calculate.
Connect the compass to a processor that continually takes a reading then sends it to a display (local LCD or PC term program). Then turn the servo (motor) on/off and see if the compass readings remain steady.
Move the compass closer or farther to determine the distance it will work at. Also move the compass to different locations, above, behind, in front, etc of the servo. There could be a 'sweet spot' where the servo doesn't affect the compass.
Repeat with any shielding of the servo. If the shielding does work then the compass should work when it is much closer to the servo.
I mounted the compass 18 inches above the platform where I have all of my electronics. That's like 20 inches above the servo for steering and the rear dc motor I have. If I completely covered both motors with mumetal foil, how much noise will be canceled out? And what is the difference between that foil and tin foil? thickness? To the eye, it looks like metal foil...
MuMetal is like a sponge to magnetic fields. The permeability of the metal captures much of the field and minimizes its escape. Using aluminum, copper, or tin foil for magnetic is a complete waste of time. A ferrous foil would help some but not as much as a high permeability MuMetal (or similar) container/shield.

How much help the MuMetal shield will be will depend on many factors including proper alignment of the shield with the magnetic field, proper aspect ratio of the container to the field, lack or minimized sharp corners (a sphere would be best, a tube second best), and number of concentric shields.

How much you can attenuate the field will also depend on the strength of the motor. A tiny pager motor and a high performance stepper will generate very different results.

There is no way for us to provide a quantitative measure of how much your system will improve. I assume that at 20 inches, you still are getting lots of interference? If so, then a higher sensor mask, or properly designed MuMetal tubes around your servos will be needed. Visit the MuShield company for design information.
Tin (sn) does not 'conduct' magnetic fields. MuMetal, like ferrous (iron) metals does 'conduct' magnetic fields.
As I stated earlier, it is very difficult to calculate the effectiveness of magnetic shielding. It is best to determine the effectiveness by experimentation.

Does the compass get inference from the servo's when 20 inches away?
What about at 10 inches? or 30 inches?

Looks like fll-freak answered at the same time.
Thanks both of you for responding! The compass' data accuracy has definitely increased by a lot, while running the servo motor. I have yet to attempt to run the DC motor with the compass.....I'm afraid of the results. But I will try it soon. And I tried uploading code the other day to make the servo turn straight in between a certain orientation and I'm still getting odd results where it thinks two places are 90 degrees.......I'm not sure why it does this......but then a day earlier it worked perfectly for the first time......does the scl on the compass go to the scl pin or the sda on the Arduino?