Multitasking Wall-Hug Example

Related
Products

Product image for PPRK BrainStem Easy Kit

Introduction
Source Code - Overview and Startup Configuration
Source Code - Common TEA Definitions
Source Code - Low Level Routines
Source Code - TEA Main Calling Routine

Introduction

These programs enable a BrainStem PPRK to use an arbitrary side and sensor for following a wall. The code takes advantage of some of the true multitasking features of the BrainStem. A multitasking approach makes it possible to treat the wall-hug as a "black box" behavior.

The PPRK has unique symmetry that makes it possible for the robot to follow walls at nearly any orientation to the platform. An effective hug strategy is to have one wheel parallel to the wall. When this wheel is closest to the wall, two of the sensors point at the wall. The robot can pick one of the sensors and wall-hug in that direction. Since there are three wheels, it is possible to do six different wall-hugs of this type.

It is also possible to wall-hug with one wheel perpendicular to the wall. In this configuration, only one sensor points at the wall. The two wheels to the rear of that sensor must drive the robot in the direction of the sensor. The front wheel is only used when steering. Other wall-hugs may be possible but the parallel and perpendicular cases are the simplest. This example only deals with the case where one wheel is parallel to the wall.

Orientations of the PPRK explored in this example.

Two different orientations for wall-hugging with a PPRK.

In order to wall-hug, the wheel closest to the wall must spin near its maximum speed. It can not spin at maximum speed because there must be some leeway for steering. The other two wheels must spin slowly to counteract that wheel and make the robot go straight. The wall-hug routine must continually measure the distance to the wall and rotate the robot towards the wall or away from it based on the error between the desired tracking distance and actual distance.

Contents

Source Code - Overview and Startup Configuration

The low-level wall-hug routines run in a separate process. A wall-hug enable flag, the type of hug, and the hug distance are stored in the Scratchpad. The hug process monitors these Scratchpad locations to determine what operation to perform. When enabled, the process performs an endless loop of control calculations. When disabled, the process stops the motors and writes to a semaphore to indicate that the hug is stopped. A flag makes sure the semaphore is only written once after the hug stops.

The semaphore is required to synchronize the stop operation. Another process can write to the Scratchpad to disable the hug, but it may take several instructions before the hug actually stops. This lag makes it possible for the two processes to send conflicting commands to the motors. After disabling the hug, the other process must use the await_signal routine to stall until its semaphore is written. The hug process uses the send_signal routine to tell the other process when it is safe to continue. These two routines use TEA opcodes to access the semaphore ports.

The programming consists of three source files. The "mhug.tea" program, which is the main program, is stored in TEA file slot 0. It is configured to bootstrap as program 1. The "mhugx.tea" program is stored in TEA file slot 1. It is configured as bootstrap program 2. See the Bootstrap VM Execution page for more details. The following commands configure the bootstrap programs:

2 18 15 0 /* cmdVAL_SET, ParamID=15, VM Boot File 1 */ 2 18 16 1 /* cmdVAL_SET, ParamID=16, VM Boot File 2 */ 2 19 /* cmdVAL_SAV */

When the BrainStem is powered up, it will launch the program in file slot 0 as process 0 and launch the program in file slot 1 as process 1.

An idle routine disables the servos for a period of time. With use, the servos may drift out of alignment. This means they may not stop completely when given a speed a of 0. Disabling the servos will force the PPRK to come to a complete stop. The idle routine must re-enable the servos so that the PPRK can move around again.

Contents

Source Code - Common TEA Definitions

The "mhuginc.tea" is the "glue" that holds the two programs together. It defines common includes and common defined values for all the programs.

/* filename: mhuginc.tea */ /* BrainStem PPRK program */ /* common includes */ /* wall-hug control constants */ /* timed rotation routine */ #include <aCore.tea> #include <aPPRK.tea> #include <aA2D.tea> #include <aPad.tea> #define HUGENA 31 #define HUGNUM 30 #define HUGSET 28 #define MAINPROC 0 #define MAINFILE 0 #define HUGPROC 1 #define HUGFILE 1 #define SERVOS_ON (unsigned char)128 #define SERVOS_OFF 0 /* write common parameter to all servo config ports */ void servo_config(char c) { asm { pushsb 3 popbm aPortServo+aPortServoBlockSize*APPRK_S1+aOffsetServoConfig pushsb 3 popbm aPortServo+aPortServoBlockSize*APPRK_S2+aOffsetServoConfig pushsb 3 popbm aPortServo+aPortServoBlockSize*APPRK_S3+aOffsetServoConfig } } /* perform a motion for a set amount of time then stop */ void pprk_scoot(char v1, char v2, char v3, char vr, int t) { aPPRK_Go(v1,v2,v3,vr); aCore_Sleep(t); aPPRK_Go(0,0,0,0); } /* turn motors off, sit still for a while, then turn motors back on */ void idle(int t) { servo_config(SERVOS_OFF); aCore_Sleep(t); servo_config(SERVOS_ON); } /* send an int to a subprocess via a semaphore */ void send_signal(int proc, int data) { asm { pushss 4 /* push data */ pushss 8 /* push proc */ pushls aPortSemaphore /* sema+proc */ adds popsmx } } /* receive int via a semaphore */ int await_signal() { int val=0; asm { pushms aPortProcID pushls aPortSemaphore adds pushmsx popss 2 } return val; }

Contents

Source Code - Low Level Routines

The "mhugx.tea" program performs the low-level wall-hug calculations. It runs concurrently with the "mhug.tea" as a separately threaded program.

/* filename: mhugx.tea */ /* BrainStem PPRK program */ /* hug controller process */ #include "mhuginc.tea" void hugR(char v1, char v2, char v3, char ir, int set) { int err; err=(set-aA2D_ReadInt(ir))/4; if (err>46) err=46; if (err< -46) err= -46; aPPRK_Go(v1,v2,v3,(char)err); } void hugL(char v1, char v2, char v3, char ir, int set) { int err; err=(aA2D_ReadInt(ir)-set)/4; if (err>46) err=46; if (err< -46) err= -46; aPPRK_Go(v1,v2,v3,(char)err); } void pickhug(char hugtype, int set) { switch (hugtype) { case 0: hugR(20,-80,20,APPRK_IR3,set); break; case 1: hugR(-80,20,20,APPRK_IR2,set); break; case 2: hugR(20,20,-80,APPRK_IR1,set); break; case 3: hugL(-20,80,-20,APPRK_IR1,set); break; case 4: hugL(-20,-20,80,APPRK_IR2,set); break; case 5: hugL(80,-20,-20,APPRK_IR3,set); break; } } void main() { int stopped=1; while (1) { if (aPad_ReadChar(HUGENA)) { stopped=0; pickhug(aPad_ReadChar(HUGNUM),aPad_ReadInt(HUGSET)); } else { if (!stopped) { stopped=1; aPPRK_Go(0,0,0,0); send_signal(MAINPROC,0); } } } }

Contents

Source Code - TEA Main Calling Routine

The "mhug.tea" program contains the top-level logic for a wall-hugging demonstration.

/* filename: mhug.tea */ /* BrainStem PPRK program */ /* follow wall for 2 seconds */ /* rotate for 2 seconds to switch sides */ /* after 3 rotates, switch hug side */ #include "mhuginc.tea" #define RHUG 160 // wall-hug distance #define ROTS 50 // rotation speed void pickhug(char num, int set) { aPad_WriteChar(HUGENA,1); aPad_WriteChar(HUGNUM,num); aPad_WriteInt(HUGSET,set); } void stophug() { // disable hug then wait for sync semaphore aPad_WriteChar(HUGENA,0); await_signal(); } void dohug(char n, char r) { pickhug(n,RHUG); aCore_Sleep(20000); stophug(); pprk_scoot(0,0,0,r,20000); } void main() { // 5 second start delay aCore_Sleep(50000); while (1) { dohug(0, ROTS); dohug(1, ROTS); dohug(2, ROTS); idle(20000); dohug(3,-ROTS); dohug(4,-ROTS); dohug(5,-ROTS); idle(20000); } }

Revision History:

2003-04-17: Example Created.

Related Examples:

PPRK Navigation for Trinity Contest Example