Robot with Multiple Behaviors Example

Related
Products

Brainstem GP 1.0 Module

Devantech Compass

Parallax Cont Rotation Servo

Contents

Introduction
Source Code
Compile and Load Programs

Introduction

This example shows how a robot with two different behaviors may be implemented. For this example, the default behavior is compass-following. The robot switches to an obstacle-avoidance mode when it is about to run into something. A BrainStem GP 1.0 module is the robot's brain. Two modified servos propel the robot. A button starts the robot. A Devantech CMPS03 compass provides compass input. Three Sharp GP2D02 sensors look for obstacles in front of the robot. A fourth GP2D02 looks for objects above the robot. The robot stops when something is above it.

This example was written for an "exploded" robot. All the parts were connected on a workbench, but there was no actual robot rolling around. Verifying the code was a process of hand-waving in front of sensors to simulate obstacles, turning the compass, and viewing print statements to monitor sensor data and program flow. On a real robot, the servo motions for obstacle avoidance would probably need improvement.

This example uses code and techniques from the multiple TEA files , compass guidance , and multiple GP2D02 examples. Refer to these pages for more detailed discussions of these topics.

Contents

Source Code

The upward-looking GP2D02 is connected to the BrainStem GP 1.0 module's built-in GP2D02 port so it is read with the standard aGP_ReadGP2D02 routine. The other three GP2D02 sensors have their outputs connected to digital IO lines and share a common digital clock line. The start button takes up the last of the five digital IO lines on the GP module. A simple go routine controls both servos. Servos normally require values in the 0-255 range, but it's handy to use signed values to indicate direction. The go routine takes care of this conversion.

The programming for this example consists of two source files. The "mdemo0.tea" file initializes IO, waits for the start button to be pushed, then begins a compass following loop and is shown below.

/* filename: mdemo0.tea */ /* With GP application, the two servos must be configured so they - stop when slider position is 128 - go forward when slider position is 255 - go backward when slider position is 0 If using Parallax servos with adjustment holes near the wires, insert a screwdriver and adjust internal pot to center the servos so that they stop when the slider position is 128. */ #include <aCore.tea> #include <aMulti.tea> #include <aPrint.tea> #include <aServo.tea> #include <aCompass.tea> #include <aDig.tea> #include <aGP.tea> #define LEFTMOTOR 0 #define RIGHTMOTOR 1 #define GP2D02_INPUT_A 0 // needed for IO init code #define GP2D02_INPUT_B 1 #define GP2D02_INPUT_C 2 #define GP2D02_CLOCK 3 #define DBUTTON 4 // handy start button void go(int vL, int vR) { /* input velocities must be -128 to 127 */ /* this converts them to 0 to 255 range */ vL = vL + 128; aServo_SetAbsolute(LEFTMOTOR, (unsigned char)vL); vR = vR + 128; aServo_SetAbsolute(RIGHTMOTOR, (unsigned char)vR); } int getHeadingError(int d0) { int d; int e; d = aCompass_ReadInt(); /* error between actual heading and desired heading */ e = d0 - d; /* handle wrap-around */ if (e < -1800) e = e + 3600; if (e > 1800) e = e - 3600; return e; } void track_compass_iter(int d) { int e; int err; int vL,vR; e = getHeadingError(d); /* multiply by bigger constant to respond more quickly */ /* divide by a constant to respond more slowly */ err = e * 1; /* limit the error */ if (err > 100) err = 100; if (err < -100) err = -100; aPrint_String("ERR = "); aPrint_IntDec(err); aPrint_Char('\n'); /* steer left or right based on error */ /* slow down one motor to steer */ if (err >= 0) { vL = 100; vR = 100 - err; } else { vL = 100 + err; vR = 100; } go(vL,vR); } void motortest() { /* should go forward 1 sec, backward 1 sec, stop */ go(100,100); aCore_Sleep(10000); go(-100,-100); aCore_Sleep(10000); go(0,0); } void main() { int nflag; int heading; /* set initial direction (arbitrary) */ heading = 100; /* configure the GP2D02s */ aDig_Config(GP2D02_INPUT_A, ADIG_INPUT); aDig_Config(GP2D02_INPUT_B, ADIG_INPUT); aDig_Config(GP2D02_INPUT_C, ADIG_INPUT); aDig_Config(GP2D02_CLOCK, ADIG_OUTPUT); aDig_Write(GP2D02_CLOCK, 1); /* wait for start button transition from 0 to 1 */ nflag=1; while (nflag) { nflag=aDig_ReadInt(DBUTTON); } /* pause 0.25 seconds */ aCore_Sleep(2500); /* follow compass heading of 100 degrees */ /* track compass heading till a GP2D02 sees something */ while (1) { /* exit loop when end condition is found */ if (aGP_ReadGP2D02() > 200) break; /* run program to see if front GP2D02s see anything */ aMulti_Run00(1,1); /* default behavior is compass tracking */ track_compass_iter(heading); } /* stop */ go(0,0); }

The "mdemo1.tea" program is the obstacle avoidance subroutine as shown below.

/* filename: mdemo1.tea */ /* This program just makes the robot do some arbitrary motions in response to obstacles. */ #include <aCore.tea> #include <aMulti.tea> #include <aPrint.tea> #include <aServo.tea> #include <aCompass.tea> #include <aDig.tea> #include <aGP.tea> #define LEFTMOTOR 0 #define RIGHTMOTOR 1 #define GP2D02_INPUT_A 0 #define GP2D02_INPUT_B 1 #define GP2D02_INPUT_C 2 #define GP2D02_CLOCK 3 #define DBUTTON 4 /* global variables for GP2D02 rangers */ int rangeA = 0; int rangeB = 0; int rangeC = 0; void quick_pulse(char x) { asm { /* configure stack for quick write of 1 then 0 */ pushlb 0 pushsb 4 pushlb aPortDigitalBlockSize multb pushls aPortDigital + aOffsetDigitalIO adds pushlb 1 pushss 3 /* write 1 then write 0 */ /* (GP2D02 needs a very quick clock pulse) */ popbmx popbmx } } void read_all_GP2D02() { char k; /* clear the global range variables */ rangeA = 0; rangeB = 0; rangeC = 0; /* initialize all the modules */ aDig_Write(GP2D02_CLOCK, 0); aCore_Sleep(750); for (k = 0; k != 8; k++) { /* pulse the clock line */ quick_pulse(GP2D02_CLOCK); /* shift in the new bits of each reading */ rangeA = (rangeA << 1) | aDig_ReadChar(GP2D02_INPUT_A); rangeB = (rangeB << 1) | aDig_ReadChar(GP2D02_INPUT_B); rangeC = (rangeC << 1) | aDig_ReadChar(GP2D02_INPUT_C); } /* delay ensures modules get reset properly */ aDig_Write(GP2D02_CLOCK, 1); aCore_Sleep(20); } void go(int vL, int vR) { /* input velocities must be -128 to 127 */ /* this converts them to 0 to 255 range */ vL = vL + 128; aServo_SetAbsolute(LEFTMOTOR, (unsigned char)vL); vR = vR + 128; aServo_SetAbsolute(RIGHTMOTOR, (unsigned char)vR); } void main(char callingProc) { /* enter loop to perform object avoidance behaviors */ while (1) { /* read the sensors */ read_all_GP2D02(); aPrint_IntDec(rangeA); aPrint_Char(','); aPrint_IntDec(rangeB); aPrint_Char(','); aPrint_IntDec(rangeC); aPrint_Char('\n'); aCore_Sleep(10000); if ((rangeA < 100) && (rangeB < 100) && (rangeC < 100)) { /* break out of loop if no sensor detects anything */ break; } else { /* sensors saw something so try to avoid it */ if (rangeB > 100) { /* back up */ aPrint_String("B sees object\n"); go(-100,-100); aCore_Sleep(10000); go(0,0); aCore_Sleep(5000); } else if (rangeA > 100) { /* spin right */ aPrint_String("A sees object\n"); go(-100,100); aCore_Sleep(10000); go(0,0); aCore_Sleep(5000); } else if (rangeC > 100) { /* spin left */ aPrint_String("C sees object\n"); go(100,-100); aCore_Sleep(10000); go(0,0); aCore_Sleep(5000); } } } /* exit program once obstacle has been avoided */ aMulti_Halt(callingProc, 0); }

Contents

Compile and Load Programs

The following Console commands will compile and load the programs and configure the Stem to launch mdemo0 at power-up:

steep "mdemo0" steep "mdemo1" load "mdemo0" 2 0 load "mdemo1" 2 1 2 18 15 0 2 19

So what good is a program for a robot that doesn't really exist? It's a good starting point for a similar project. This example also shows how code from other examples can all fit together in one application.

Revision History:

2004-09-22: Example Created.

Related Examples:

Robot Using Compass Guidance with the Devantech CMPS03

Connecting Extra GP2D02 and GP2D05 Rangers to a BrainStem Example

One TEA Program with Multiple TEA Files as Subtasks Example