I’m not sure off-hand how well the Heading sketch will work without z info. Maybe you should just try playing around with the program I wrote to test the compass. I just went through and commented it some, so I hope it will be easier to follow. When you push the user button, the Zumo will beep a few times, then spin in place for two seconds while calibrating the compass. After that, the main loop has it turn to face the four cardinal directions in order (pausing for a second after each one), turning the user LED on whenever it is trying to point north. It sets the compass to the highest sensitivity but ignores the z axis.
Some caveats:
- I wrote this before the Zumo Arduino libraries were released, so I made my own functions for setting the motor speeds and controlling the buzzer. Feel free to replace those with library functions. (I’m pretty sure my simple buzzer code will need to be modified if you are using a Leonardo.)
- I wrote this for a Zumo that was assembled with (I believe) 30:1 HP or 50:1 HP micro metal gearmotors. If your Zumo has different motors than mine, you might need to modify the speeds and/or PID parameters used in my code. For example, if the robot doesn’t rotate well (or at all) during the calibration phase, you should try changing the setMotors(150, -150) line of compassCalibrate() to something like setMotors(200, -200), and you will likely have to tweak the turnToHeading() function as well.
- I cleaned it up in a text editor and did not try to compile it after. If it doesn’t compile, that means I made some typos while cleaning it up!
- You need to have our LSM303 Arduino library installed to run this sketch.
Please let me know how it works for you, and don’t hesitate to ask if you have any questions about it. I’d like to use parts of it as a basis for a Zumo compass example down the road.
#include <Wire.h>
#include <LSM303.h>
#define CRA_REG_M 0x00 // magnetometer control register location
#define CRA_REG_M_75HZ 0x18 // reg value for 75 Hz update rate for magnetometer (default is 15 Hz)
#define CRA_REG_M_220HZ 0x1C // reg value 220 Hz update rate for magnetometer
const unsigned char MAX_SPEED = 255;
const unsigned char M1PWM = 9;
const unsigned char M1DIR = 7;
const unsigned char M2PWM = 10;
const unsigned char M2DIR = 8;
const unsigned char BUTTON = 12;
const unsigned char BUZZER = 11;
const unsigned char LED = 13;
LSM303 compass;
// variables used for compass calibration
int xmax[10] = {-4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097};
int xmin[10] = {4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097};
int ymax[10] = {-4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097, -4097};
int ymin[10] = {4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097, 4097};
int xminavg = 0;
int xmaxavg = 0;
int yminavg = 0;
int ymaxavg = 0;
// the setup routine runs once when you press reset:
void setup() {
pinMode(LED, OUTPUT);
pinMode(BUTTON, INPUT_PULLUP);
pinMode(M1PWM, OUTPUT);
pinMode(M1DIR, OUTPUT);
pinMode(M2PWM, OUTPUT);
pinMode(M2DIR, OUTPUT);
pinMode(BUZZER, OUTPUT);
Serial.begin(9600);
Wire.begin();
compass.init();
compass.enableDefault();
compass.setMagGain(compass.magGain_13); // highest sensitivity: +/- 1.3 gauss range (this is the default)
compass.writeMagReg(CRA_REG_M, CRA_REG_M_220HZ); // 220 Hz compass update rate (default is 15 Hz)
delay(5);
// wait for a debounced button press
while (digitalRead(BUTTON)) // loop while button is not pressed
{
while (digitalRead(BUTTON)); // loop while button is not pressed
delay(10); // debounce
}
// make the buzzer beep a few times
delay(500);
analogWrite(BUZZER, 128);
delay(100);
analogWrite(BUZZER, 0);
delay(250);
analogWrite(BUZZER, 128);
delay(100);
analogWrite(BUZZER, 0);
delay(250);
analogWrite(BUZZER, 128);
delay(250);
analogWrite(BUZZER, 0);
delay(250);
compassCalibrate(); // makes Zumo spin in place while calibrating the compass
delay(100);
}
// the loop routine runs over and over again forever:
void loop() {
digitalWrite(LED, HIGH); // turn LED on to signal north
turnToHeading(0); // turn to face north
delay(1000);
digitalWrite(LED, LOW); // turn LED off
turnToHeading(90); // turn to face east
delay(1000);
turnToHeading(180); // turn to face south
delay(1000);
turnToHeading(270); // turn to face west
delay(1000);
}
// turns the Zumo to face the requested magnetic heading
void turnToHeading(int angle)
{
// make sure angle is a number from 0 to 360
while (angle < 0)
angle += 360;
while (angle >= 360)
angle -= 360;
int currentAngle;
long diffSum = 0;
int lastDiff = 1000; // dummy value outside allowed -180 to 180 range indicates uninitialized
unsigned char count = 0;
while (1)
{
currentAngle = getHeading();
int diff = currentAngle - angle;
if (diff < 0)
diff += 360;
if (diff > 180)
diff -= 360;
if (lastDiff == 1000);
lastDiff = diff;
if (diff < 10 && diff > -10)
{
// if we are within 10 degrees of our target heading
count++;
if (count > 6)
{
// if we have been within 10 degrees of our target for 6 consecutive readings
// we have reached our target; stop motors and return
setMotors(0, 0);
return;
}
}
else
count = 0;
// use PID on the heading error (diff) to determine what the motor speed should be
// proportional constant is 5/2, derivative constant is 30, integral constant is 1/7
int motor_speed = diff*5/2 + (diff - lastDiff)*30 + diffSum/7;
setMotors(-motor_speed, motor_speed); // update speed of motors
lastDiff = diff;
// only include small errors in error sum (integral term only grows when Zumo is close to its target)
if (diff > -40 && diff < 40)
diffSum += diff;
// cap the magnitude of the error sum at 100,000
if (diffSum > 100000)
diffSum == 100000;
if (diffSum < -100000)
diffSum = -100000;
/*
// the following code is an alternate approach that doesn't use PID
// it doesn't work as well, but it's easier to understand
if (diff < -40)
setMotors(150, -150);
else if (diff < -30)
setMotors(100, -100);
else if (diff < -20)
setMotors(80, -80);
if (diff > 40)
setMotors(-150, 150);
else if (diff > 30)
setMotors(-100, 100);
else if (diff > 20)
setMotors(-80, 80);
*/
delay(5);
}
}
// inserts value into array at the appropriate location and reorders the array
// elements so that "array" always contains the largest values and is ordered
// from smallest to largest. Examples:
// if array is {3, 5, 7} before checkMax and value is 2, array is {3, 5, 7} after
// if array is {3, 5, 7} before checkMax and value is 4, array is {4, 5, 7} after
// if array is {3, 5, 7} before checkMax and value is 6, array is {5, 6, 7} after
// if array is {3, 5, 7} before checkMax and value is 9, array is {5, 7, 9} after
// This function makes it easy to fill an array with the largest n numbers from a set
// of data.
// Note: array must already be sorted from smallest to largest or else this function
// will not work. It is fine if all elements have the same value.
void checkMax(int value, int *array, unsigned int arraySize)
{
if (value <= array[0])
return;
unsigned int i = 0;
while (value > array[i])
{
if (++i >= arraySize)
break;
}
unsigned int c;
for (c = 1; c < i; c++)
array[c-1] = array`;
array[i-1] = value;
}
// inserts value into array at the appropriate location and reorders the array
// elements so that "array" always contains the smallest values and is ordered
// from largest to smallest. Examples:
// if array is {7, 5, 3} before checkMax and value is 2, array is {5, 3, 2} after
// if array is {7, 5, 3} before checkMax and value is 4, array is {5, 4, 3} after
// if array is {7, 5, 3} before checkMax and value is 6, array is {6, 5, 3} after
// if array is {7, 5, 3} before checkMax and value is 9, array is {7, 5, 3} after
// This function makes it easy to fill an array with the largest n numbers from a set
// of data.
// Note: array must already be sorted from largest to smallest or else this function
// will not work. It is fine if all elements have the same value.
void checkMin(int value, int *array, unsigned int arraySize)
{
if (value >= array[0])
return;
unsigned int i = 0;
while (value < array[i])
{
if (++i >= arraySize)
break;
}
unsigned int c;
for (c = 1; c < i; c++)
array[c-1] = array`;
array[i-1] = value;
}
// Spins the Zumo around in place for two seconds while reading the magnetometer x and y values
// every ten milliseconds. The ten highest and ten lowest values for the x and y axes are
// stored in respective max and min arrays. Once the Zumo stops, the middle six values from
// each array are averaged to produce final calibration coefficients.
// (This level of calibration is probably overkill.)
void compassCalibrate()
{
setMotors(150, -150); // make the Zumo spin (might need to adjust speeds for your motors)
delay(200); // give it time to start moving before we begin reading the magnetometer
unsigned int i;
for (i = 0; i < 180; i++) // takes 180 readings over 1.8 seconds
{
compass.read();
checkMax((int)compass.m.x, xmax, 10);
checkMin((int)compass.m.x, xmin, 10);
checkMax((int)compass.m.y, ymax, 10);
checkMin((int)compass.m.y, ymin, 10);
delay(10);
}
setMotors(0, 0);
// simple approach to reject potential outliers: only average the middle array values
for (i = 2; i < 8; i++)
{
xminavg += xmin[i];
xmaxavg += xmax[i];
yminavg += ymin[i];
ymaxavg += ymax[i];
}
xminavg /= 6; // typical minimum reading we can expect from the x axis of the magnetometer
xmaxavg /= 6; // typical maximum reading we can expect from the x axis of the magnetometer
yminavg /= 6; // typical minimum reading we can expect from the y axis of the magnetometer
ymaxavg /= 6; // typical maximum reading we can expect from the y axis of the magnetometer
}
// Gets x and y readings from the magnetometer, scales them based on calibration coefficients,
// and computes a heading angle in degrees from them. z magnetometer axis is ignored.
// The returned angle is always a number from 0 to 360.
int getHeading()
{
compass.read();
int x = (int)compass.m.x;
int y = (int)compass.m.y;
float xScaled = 2.0*(float)(x - xminavg) / (xmaxavg - xminavg) - 1.0;
float yScaled = 2.0*(float)(y - yminavg) / (ymaxavg - yminavg) - 1.0;
int angle = round(atan2(yScaled, xScaled)*180 / M_PI);
if (angle < 0)
angle += 360;
return angle;
}
// Sets the Zumo motor speeds. (I wrote this before the Zumo Arduino libraries were released,
// so I made my own basic function for interfacing with the motor drivers.)
void setMotors(int m2Speed, int m1Speed)
{
if (m1Speed > 255)
m1Speed = 255;
if (m1Speed < -255)
m1Speed = -255;
if (m1Speed <= 0)
{
digitalWrite(M1DIR, HIGH);
analogWrite(M1PWM, -m1Speed);
}
else
{
digitalWrite(M1DIR, LOW);
analogWrite(M1PWM, m1Speed);
}
if (m2Speed > 255)
m2Speed = 255;
if (m2Speed < -255)
m2Speed = -255;
if (m2Speed <= 0)
{
digitalWrite(M2DIR, HIGH);
analogWrite(M2PWM, -m2Speed);
}
else
{
digitalWrite(M2DIR, LOW);
analogWrite(M2PWM, m2Speed);
}
}
- Ben
