 | Path Following with Compass Example Last Modified: 2006-11-02 | | |
| Acroname Robotics | PDF webpage version  | ||
| | Path Following with Compass Example Last Modified: 2006-11-02 | | |
| Acroname Robotics | PDF webpage version | ||
|  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 Devantech CMPS03 is a small solid-state compass module that is ideal for such a task. It provides both I2C and PWM outputs. This example code makes a robot cover a large rectangular area based on compass input. We used the Rover with a BrainStem Moto 1.0 board to test the program. The robot goes straight for a while, rotates 90 degrees, moves straight a short distance, rotates 90 degrees, then goes straight in the opposite direction from its previous track. By repeating this process over and over again, the robot provides thorough coverage of an area. Such a strategy could be useful in an application such as lawn-mowing or searching for objects in a field. It could also be used for vacuuming, but indoor environments are more likely to interfere with compasses. Source Code The CMPS03 provides an absolute heading measurement in units of 0.1 degrees. The heading value is represented as an integer from 0 to 3599. After rotating beyond 359.9 degrees, the heading wraps back to 0. This causes a wrap-around problem that must be handled in software. 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. The logic for following a compass heading is implemented as a proportional control loop in the track routine. As the robot moves, it continually calculates a steering correction based on the heading error. The error term is railed to +-10 degrees to prevent overflow in the steering calculations. The gain factor provides maximum PWM output when the error is 0. The proper combination of rail and gain was determined by trial and error. This loop keeps the robot pointed in one direction, but it may drift laterally over time. The logic for performing a rotation is just a while loop in the absturn routine. Once the robot starts rotating, it continually checks the error from its desired heading and stops when that error is less than a threshold. The init routine takes four compass readings which are used as reference directions for performing the turns and tracking. The initial orientation of the robot is the first heading. The three other quadrant directions are calculated from the initial heading. A range check for each calculation handles wrap-around. The motion control subroutine calls are specific to the Moto board and Rover base, but the general control techniques may be adapted to a variety of bases and controllers. /* filename: plow.tea */
/* follows a "plowing a field" course based on compass */
/* CW is + rotation */
/* CCW is - rotation */
#include <aCore.tea>
#include <aPrint.tea>
#include <aCompass.tea>
#include <aMotion.tea>
#include <aMulti.tea>
#define RSPEED 32040 /* =QMAX*GAIN */
#define GAIN 534
#define QMAX 60
#define DEGERR 100 /* tracking error railed to 10 degrees */
#define ROWTIME 660
#define ADJTIME 110
#define THRESH 150 /* 15 degree rotational threshold */
int d1, d2, d3, d4; /* quadrant directions */
int abs(int x)
{
if (x<0) return -x;
return x;
}
int getHeadingError(int d0)
{
int d;
int e;
d = aCompass_ReadInt();
e = d - d0;
/* handle wrap-around */
if (e < -1800) e = e + 3600;
if (e > 1800) e = e - 3600;
return e;
}
void init()
{
/* courtesy delay */
aCore_Sleep(30000);
/* initialize 4 relative quadrant directions */
d1 = aCompass_ReadInt();
d2 = d1 + 900;
if (d2 >= 3600) d2 = d2 - 3600;
d3 = d2 + 900;
if (d3 >= 3600) d3 = d3 - 3600;
d4 = d3 + 900;
if (d4 >= 3600) d4 = d4 - 3600;
}
void track(int d, int time)
{
int e;
int v0,v1;
int k = 0;
while (1)
{
e = getHeadingError(d);
/* rail the error */
if (e > DEGERR) e = DEGERR;
if (e < -DEGERR) e = -DEGERR;
/* steer left or right based on error */
if (e>=0)
{
v0 = RSPEED;
v1 = (QMAX - e) * GAIN;
}
else
{
v0 = (QMAX + e) * GAIN;
v1 = RSPEED;
}
aMotion_SetVelocity(v0,v1);
k++;
if (k > time) break;
}
aMotion_SetVelocity(0,0);
aCore_Sleep(5000);
}
void absturn(int dir, int d0, int v)
{
int d;
int e;
/* start rotating */
if (dir<0)
{
aMotion_SetVelocity(v, -v); /* CCW */
}
else
{
aMotion_SetVelocity(-v, v); /* CW */
}
while (1)
{
e = getHeadingError(d0);
if (abs(e) < THRESH) break;
}
aMotion_SetVelocity(0,0);
aCore_Sleep(5000);
}
void go()
{
int i;
for (i=0; i<3; i++)
{
track(d1, ROWTIME);
absturn( 1, d2, RSPEED);
track(d2, ADJTIME);
absturn( 1, d3, RSPEED);
track(d3, ROWTIME);
absturn( -1, d2, RSPEED);
track(d2, ADJTIME);
absturn( -1, d1, RSPEED);
}
}
void main()
{
init();
go();
}
Revision History:
| ||||
Related Examples: Robot Program with Two TEA Programs with Different Behaviors Example | |||||
| voice: 720-564-0373, email: sales@acroname.com, address: 4822 Sterling Dr., Boulder CO, 80301-2350, privacy © Copyright 1994-2008 Acroname, Inc., Boulder, Colorado. All rights reserved. |
