Dinsmore Compass Guidance Example

Related
Products

Product image for Brainstem GP 1.0 Module

Introduction

A common task for a robot is following a pre-determined route. One way to do this is to make the robot follow a compass heading. The Dinsmore 1525 is a small compass module that is ideal for such a task. It provides two sine-cosine analog outputs which can be applied directly to a microcontroller with A2D inputs.

This example code makes a robot follow a straight course. We used the Pond Explorer robot boat with a BrainStem GP 1.0 board to test the program. When turned on, the robot locks onto its current heading and tries to maintain that course.

Note

See the newer boat example for a simpler technique to use with the Dinsmore 1525.

Contents

Theory

The sine-cosine outputs of the Dinsmore 1525 make signal processing a bit tricky. Normally, some floating point operations and trigonometric functions would be required to convert the sine-cosine outputs to an absolute heading. The BrainStem can only do basic integer math. Some observations of the sinusoidal signals provide a method for generating an absolute heading measurement using simple arithmetic.

The sine-cosine signals may be divided into four quadrants based on the sign of each signal. Let A be the sine and B be the cosine. Quadrant 1 is where A and B are positive. Quadrant 2 is where A is positive and B is negative. The Quadrant 3 is where A and B are negative. Quadrant 4 is where A is negative and B is positive. (Since the compass has outputs centered at roughly Vcc/2, a signal will be considered "positive" when it is above its average value and "negative" when it is below its average value.)

Within each quadrant, a sum or difference of the two signals is nearly linear. In Quadrant 1, (A-B) is nearly linear. In Quadrant 2, -(A+B) is nearly linear. In Quadrant 3, (B-A) is nearly linear. In Quadrant 4, (A+B) is nearly linear. That's the trick. Adding an appropriate offset for each quadrant produces a nearly linear absolute heading function throughout 360 degrees.

Calibration is the tedious part. It is necessary to find the average value and scaling factor for each channel experimentally. As shown above, these values can be different for both channels. For the particular compass used in this example, the average for A is 500 and the average for B is 516. A swings +-62 about its average. B swings +-73 about its average. Within each quadrant, the range (difference between max and min) will be the sum of these two scaling factors. The next step is to calculate offsets for each quadrant interpolation function so that the four put together form one continuous line. Some pseudocode for calculating these constants is listed below. (Angles are specified in degrees). The sin and cosine terms cancel out the offsets in the linear approximations. The R terms make everything line up.

CTRA = 500
CTRB = 516
SCALEA = 62
SCALEB = 73
R = SCALEA+SCALEB
K1 = -(SCALEA*sin(0)+CTRA)+(SCALEB*cos(0)+CTRB)
K2 = (SCALEA*sin(90)+CTRA)+(SCALEB*cos(90)+CTRB)+R
K3 = (SCALEA*sin(180)+CTRA)-(SCALEB*cos(180)+CTRB)+2*R
K4 = -(SCALEA*sin(270)+CTRA)-(SCALEB*cos(270)+CTRB)+3*R

The final heading calculation function is plotted below with each quadrant's line in a different color. The result isn't perfect, but it's still a pretty good fit for a controller that can only do integer math.

Contents

Source Code

The robot uses a function called read_dinsmore to calculate the heading based on the interpolation described above. All the work is in deriving the constants so the function itself is quite compact. The units for the heading depend on the calibration constants. In this example, the heading is only used for relative measurements so knowing what heading corresponds to true North is not important. An application that needs true compass points would need an offset to indicate true North.

The robot uses a routine called getHeadingError to take a compass reading and calculate the error between its current heading and a desired heading. This routine handles wrap-around by automatically offsetting the error by 360 degrees when the error exceeds 180 degrees. Suppose the desired heading is 350 degrees and the current heading is 10 degrees. The error should be 20 degrees. However, subtracting 350 from 10 yields an error of -340 degrees. Adding 360 degrees to that value produces the correct error. Units that correspond to 180 degrees and 360 degrees are based on the calibration constants.

The logic for following the compass heading is implemented as a "bang-bang" control loop in the main routine. A bang-bang control system applies on-or-off outputs in an attempt to maintain a setpoint. In this example, as the robot starts to go off course it shuts one motor off in order to steer back to the desired heading. There is a small dead zone built-in to make both motors stay on if the robot is nearly on-course. Bang-bang control loops tend to cause oscillations. On the Pond Explorer, the force of the water on the hull helps to dampen (no pun intended) these oscillations. This loop keeps the robot pointed in one direction, but it may drift laterally over time.

The init routine configures the outputs and performs some tests that indicate whether the status light and motors are working properly or not.

The motion control subroutine calls are specific to the Pond Explorer base, but the general control techniques may be adapted to a variety of bases and controllers.

/* filename: boat.tea */ /* compass guided boat program */ /* uses Dinsmore 1525 2-channel analog compass with */ /* sine-cosine outputs, 2.5V average with +-0.4V swing */ #include <aCore.tea> #include <aDig.tea> #include <aA2D.tea> #define DLIGHT 1 /* motor and light controls */ #define DMOTORR 2 #define DMOTORL 3 #define CCOMPA 3 /* analog inputs A and B from compass */ #define CCOMPB 4 #define CTRA 500 /* quad interpolation constants */ #define CTRB 516 /* (calibrated manually) */ #define K1 89 #define K2 1213 #define K3 327 #define K4 -549 #define KF360 540 #define KF180 270 #define ERRTHR 4 /* heading error threshold */ #define TDELAY 2500 /* delay time for motor tests */ void init() { /* configure outputs */ aDig_Config(DLIGHT,0); aDig_Config(DMOTORL,0); aDig_Config(DMOTORR,0); /* flash light and run motors as hardware test */ aDig_Write(DLIGHT,1); aCore_Sleep(TDELAY); aDig_Write(DMOTORR,1); aCore_Sleep(TDELAY); aDig_Write(DMOTORL,1); aCore_Sleep(TDELAY); aDig_Write(DLIGHT,0); steer(0,0); } void pause_and_blink(char n, int t) { char i; for (i=0; i<n; i++) { aDig_Write(DLIGHT,1); aCore_Sleep(t); aDig_Write(DLIGHT,0); aCore_Sleep(t); } } void steer(char mR, char mL) { aDig_Write(DMOTORR,mR); aDig_Write(DMOTORL,mL); } int abs(int a) { /* sign bit set means value is negative */ if (a & 0x8000) a = -a; return a; } int read_dinsmore() { int h; int ra; int rb; char aflag = 0; char bflag = 0; /* get A and B for current heading */ ra=aA2D_ReadInt(CCOMPA); rb=aA2D_ReadInt(CCOMPB); /* do quad-linear interpolation */ if (ra > CTRA) aflag = 1; if (rb > CTRB) bflag = 1; if (aflag && bflag) { h = K1+(ra-rb); } else if (aflag && !bflag) { h = K2-(ra+rb); } else if (!aflag && !bflag) { h = K3+(rb-ra); } else if (!aflag && bflag) { h = K4+(ra+rb); } return h; } int getHeadingError(int d0) { int d; int e; d = read_dinsmore(); e = d - d0; /* handle wrap-around */ if (e < -KF180) e = e + KF360; if (e > KF180) e = e - KF360; return e; } void track(int d) { int e; int emag; while (1) { e = getHeadingError(d); emag=abs(e); if (emag < ERRTHR) { /* on course, green light on, full speed ahead */ aDig_Write(DLIGHT,1); steer(1,1); } else { /* off course, green light off, steer to correct */ aDig_Write(DLIGHT,0); if (e<0) { /* correct clockwise drift */ steer(1,0); } else { /* correct counter-clockwise drift */ steer(0,1); } } } } void main() { int heading; /* initialize and perform hardware test */ init(); /* give some time to set-up boat */ /* speed of blinking light indicates time left */ pause_and_blink(4,5000); pause_and_blink(4,2500); pause_and_blink(4,1250); /* get initial heading */ heading = read_dinsmore(); /* cruise */ track(heading); }

Revision History:

2003-06-03: Example Created.

Related Examples:

Improved Compass Guidance with the Dinsmore 1525 Example