Robot Using Sonars Example

Related
Products

Product image for BrainStem Moto 1.0 Module

Product image for Devantech SRF08 Range Finder

Introduction
Source Code - Moto Configuration
Source Code - TEA File

Introduction

This example shows how to use the built-in motion control features of a Moto board along with its I2C capability to control a roaming robot. The robot uses two SRF08 sonars with I2C interfaces.

This program was developed on a prototype of the Garcia robot. The sonars were temporarily mounted on the front of the robot pointing slightly off to the sides. A Lynxmotion Dual Sonar Housing or a Devantech Sonar Housing is an ideal accessory for making a permanent mounting for the sonars.

The basic behavior of the robot is to check its two sonars and steer in the direction that tends to balance the two readings. This is an effective strategy for making the robot follow a path down the middle of a narrow hallway. Since the sonars have wide beam widths, the robot can steer away from most of the clutter in its way. The velocity damping feature of the Moto board makes the motion reasonably smooth and natural-looking. Unfortunately, this strategy can cause situations where the robot may steer itself directly into a wall. To prevent this, the robot checks to see if it is getting too close to oncoming obstacles. If it is about to crash, it will stop and turn away from the closest obstacle in order to find a clear path.

The logic isn't perfect. The robot may crash into low-lying obstacles. The robot may also get into tight spaces and have difficulty finding its way out. Apart from these two difficulties, the program is a good starting point for making a robot that can navigate rooms and hallways.

Contents

Source Code - Moto Configuration

The first file is a batch file of commands. This batch file only needs to be run once for this example. These commands are used to initialize and save all the motion control parameters for the two motors. The file is a series of cmdMO_CFG commands followed by a cmdMO_SAV command. The cmdMO_SAV action stores the settings in the Moto EEPROM. This makes it so when the power is cycled on the Moto, the configuration settings are set up how we want them. Alternatively, one could configure the Moto settings by using the Moto application.

/* roamcfg.bag */ 4 63 0 0 7 1 /* cmdMO_CFG, CH0, ENCVEL mode, invert PID input */ 4 63 0 1 2 0 /* cmdMO_CFG, CH0, PID P term, value=2.0 */ 4 63 0 2 0 0 /* cmdMO_CFG, CH0, PID I term, value=0.0 */ 4 63 0 3 4 0 /* cmdMO_CFG, CH0, PID D term, value=4.0 */ 4 63 0 4 0 0 /* cmdMO_CFG, CH0, COFFSET, value=0 */ 4 63 0 5 127 255 /* cmdMO_CFG, CH0, PWMRAIL, value=32767 */ 4 63 0 6 0 50 /* cmdMO_CFG, CH0, PERIOD, value=5usec */ 4 63 0 7 0 0 /* cmdMO_CFG, CH0, LATENCY, value=0 */ 4 63 0 8 0 255 /* cmdMO_CFG, CH0, PWMFREQ, 10-bit, 40kHz */ 4 63 1 0 7 2 /* cmdMO_CFG, CH1, ENCVEL mode, invert motor output */ 4 63 1 1 2 0 /* cmdMO_CFG, CH1, PID P term, value=2.0 */ 4 63 1 2 0 0 /* cmdMO_CFG, CH1, PID I term, value=0.0 */ 4 63 1 3 4 0 /* cmdMO_CFG, CH1, PID D term, value=4.0 */ 4 63 1 4 0 0 /* cmdMO_CFG, CH1, COFFSET, value=0 */ 4 63 1 5 127 255 /* cmdMO_CFG, CH1, PWMRAIL, value=32767 */ 4 63 1 6 0 50 /* cmdMO_CFG, CH1, PERIOD, value=5usec */ 4 63 1 7 0 0 /* cmdMO_CFG, CH1, LATENCY, value=0 */ 4 63 1 8 0 255 /* cmdMO_CFG, CH1, PWMFREQ, 10-bit, 40kHz */ 4 64 /* cmdMO_SAV */

Contents

Source Code - TEA File

The final file is the TEA code for the routine. The main loop has an inner proportional control loop that calculates a steering correction to keep the robot moving in a direction that balances the sonar readings. The aMotion routines are documented in the TEA section of the online BrainStem Reference.

/* roam.tea */ /* Roaming Garcia */ #include <aCore.tea> #include <aMotion.tea> #include <aSRF08.tea> #define THR 1 #define SRMAX 108 #define UNITS aSRF08_INCH #define CRASH_THR 10 #define ROTATE_THR 12 void init_velocity_damping(int nACCT) { aMotion_SetRampVel(0,0); aMotion_SetRampVel(1,0); aMotion_SetRampAccStepTime(0,nACCT); aMotion_SetRampAccStepTime(1,nACCT); aMotion_SetRampFlags(0,1); aMotion_SetRampFlags(1,1); aMotion_RampEnable(aMOTION_CHANNEL_ALL,1); } void rotate_away(int v, unsigned char sonarx) { int s; aMotion_SetRampVelocity(v,-v); while (1) { s = aSRF08_RangeInt(sonarx, UNITS); if (s > ROTATE_THR) break; } } void roam(int vmax) { int e; int vmin = vmax/4; int atten = 2; int s0 = 0; int s1 = 0; while (1) { // inner loop to follow "middle" of area // (tries to keep sonar readings balanced) while (1) { s1=aSRF08_RangeInt((unsigned char)0xE2, UNITS); s0=aSRF08_RangeInt((unsigned char)0xE0, UNITS); // check for impending crash if ((s1 < CRASH_THR) || (s0 < CRASH_THR)) break; // calculate steering correction // (has a small dead zone) e = s1-s0; if ((e <= THR) && (e >= -THR)) { e = 0; } else if (e > THR) { e = (e - THR) / atten; } else if (e < -THR) { e = (e + THR) / atten; } // apply steering correction aMotion_RampVelSteer(vmax,vmin,e); } // if here then robot was about to crash // back up a wee bit aMotion_SetRampVelocity(-3,-3); aCore_Sleep(5000); // rotate away from sonar with closest reading if (s1<s0) { rotate_away(-3,(unsigned char)0xE2); } else { rotate_away(3,(unsigned char)0xE0); } } } void main() { // brief start delay for more convenient handling aCore_Sleep(30000); // increase velocity damping step time to make motion less jerky init_velocity_damping(15); roam(20); }

Revision History:

2006-10-31: Added comments for clairity in the "roamcfg.bag" file.
2003-02-26: Example Created.

Related Examples:

Shows a simple program using the C Development Download