Improved 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.

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. However the BrainStem can only do basic integer math. The aMath.tea library from the math example has an integer approximation of the atan2 function found in C math libraries. This function is called aMath_Atan2. It accepts x-y values in the range from -1000 to 1000 and returns an angle in the range from 0 to 359. The compass outputs have a fairly large magnitude so this function can convert the sine-cosine outputs of the compass to a heading without any additional scaling.

The trick with this technique is to find the average value of each channel. This average must be subtracted from each channel in order to make them symmetric about the 0 axis. This is necessary for the aMath_Atan2 function to work properly.

Note

An older boat example used some linear interpolation functions to get a heading. This updated example is easier to calibrate and provides better results throughout the range of possible heading values.

Contents

Source Code

The robot uses a function called read_dinsmore to calculate the heading. A routine called getHeadingError calculates the error between the robot's 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.

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: newboat.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> #include <aMath.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 /* compass center points */ #define CTRB 516 /* (determined manually) */ #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 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); /* convert to heading */ h = aMath_Atan2(ra - CTRA, rb - CTRB); return h; } int getHeadingError(int dir0, int dirC) { int e; e = dirC - dir0; /* handle wrap-around */ if (e < -180) e = e + 360; if (e > 180) e = e - 360; return e; } void track(int d) { int e; int h; int emag; while (1) { h = read_dinsmore(); e = getHeadingError(d, h); emag = aMath_Absval(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 in water */ /* 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:

2004-07-26: Example Created.

Related Examples:

Using the Dinsmore 1525 Compass with a BrainStem GP Example