Navigation for Trinity Contest Example

Related
Products

Product image for PPRK BrainStem Easy Kit

Introduction
Source Code - Common TEA Definitions
Source Code - Low Level Routines
Source Code - TEA Main Calling Routine
Comments

Introduction

This program demonstrates a navigation strategy for the Trinity College Firefighting Home Robot Contest. In that contest, a robot must maneuver through a model home and try to find a candle in one of four rooms. The code enables a BrainStem PPRK to begin at the start position in the model home and visit each room.

Many routines are reused from the multi-tasking wall-hug example. That example uses a subprocess to manage the basic wall-hug behavior. A high level process can launch a desired wall-hug operation and watch other sensors to determine when to stop the operation. By performing a sequence of wall-hugs and simple timed motions, the robot can follow a complex path.

The sequence for navigating the house has 27 steps. There are three basic operations: driving the wheels for a set period of time (dead-reckoning), wall-hugging for a set period of time, and wall-hugging until reaching a corner. Most of the operations are wall-hugs. This makes the navigation very reliable. A strategy that only uses sensor readings would be even better, but using a few dead-reckoning moves makes the code simpler. A disadvantage is that the timed moves will depend on the batteries and servos. The robot used to verify this example used 4AA alkaline batteries to power its servos. They provide slightly more speed than NiCad or NiMH batteries.

The following diagram shows the path traveled by the robot through the contest house. The gray lines show where the robot wall-hugs into a corner. The red lines show where the robot performs timed wall-hugs. The blue lines show where the robot performs dead-reckoning moves.

Path followed by PPRK robot using the code in this example.

The corners in the contest house are all right angles. An "inner" corner is a corner where the robot must turn 90 degrees to avoid running into the wall. If the robot is hugging a wall to its right, it must turn left 90 degrees to continue following the wall. An "outer" corner is a corner where the robot must turn 90 degrees or more to avoid losing the wall. If the robot is hugging a wall to its right, it must turn right 90 degrees to continue following the wall.

The robot can detect an inner corner by checking the "trailing" sensor pointing backwards at the wall. When the robot turns, the distance between the trailing sensor and the wall will decrease. The robot can stop when the IR sensor reading is greater than a desired threshold. The robot can detect an "outer" corner in a similar manner. When the robot turns, the distance between the trailing sensor and the wall will increase. The robot can stop when the IR sensor reading is less than a desired threshold. A routine called hugTill makes the robot hug a wall until a sensor reading reaches a threshold. This routine enables the robot to wall-hug until it reaches corners. It is also used early in the navigation routine with a sensor looking sideways to find the wall of the "island room". (The "island room" is not connected to any other walls in the structure.)

A dummy function called lookForCandle is placed in the code to show where the robot would need to stop and monitor its sensors in order to find a lit candle in a room. An ideal sensor for this task would be the UVTron Flame Detector. This device is sensitive enough to detect a lit candle inside a room when the robot is positioned just inside the doorway.

Contents

Source Code - Common TEA Definitions

The programming consists of three source files. The "mhuginc.tea", shown below, is the "glue" that holds the two programs together. It defines common includes and common defined values for all the programs.

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.

/* 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 "mhugtrin.tea" program. The "mhugx.tea" program is stored in TEA file slot 1. It is configured as bootstrap program 2.

/* 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 "mhugtrin.tea" program contains the top-level logic for navigation. The "mhugtrin.tea" program is stored in TEA file slot 0. It should be configured as bootstrap program 1.

/* filename: mhugtrin.tea */ /* BrainStem PPRK program */ /* follows planned route through */ /* floor plan of Trinity College */ /* Firefighting Contest model house */ #include "mhuginc.tea" #define RHUG 180 // wall-hug distance #define ROTS 50 // rotation speed #define ICTHR 350 // inner corner threshold #define OCTHR 110 // outer corner threshold #define HUG_LT 0 // perform "less-than" with sensor reading #define HUG_GT 1 // perform "greater-than" with sensor reading 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 hugTill(char ccomp, char chugx, int nhugr, char crchk, int nrthr) { int r; pickhug(chugx,nhugr); while (1) { r=aA2D_ReadInt(crchk); if (ccomp==HUG_GT && r>nrthr) break; if (ccomp==HUG_LT && r<nrthr) break; } stophug(); } void hugByTime(char chugx, int nhugr, int ntime) { pickhug(chugx,nhugr); aCore_Sleep(ntime); stophug(); } void lookForCandle() { // a candle search would go here // just fake it for now with a time delay idle(10000); } void main() { // 5 second start delay // (provides time to grab robot) aCore_Sleep(50000); // wall-hug in and out of first inner corner hugTill(HUG_GT,0,RHUG,APPRK_IR1,ICTHR); hugByTime(0,RHUG,15000); // wall-hug till wall of ROOM 1 acquired then lost hugTill(HUG_GT,0,RHUG,APPRK_IR2,190); hugTill(HUG_LT,0,RHUG,APPRK_IR2,170); // wall-hug a bit more to get centered with doorway hugByTime(0,RHUG,15000); // enter ROOM 1 pprk_scoot(-100,0,100,0,20000); lookForCandle(); // exit ROOM 1 pprk_scoot(100,0,-100,0,20000); // wall-hug in and out of next inner corner hugTill(HUG_GT,0,RHUG,APPRK_IR1,ICTHR); hugByTime(0,RHUG,15000); // wall-hug to next outside corner hugTill(HUG_LT,0,RHUG,APPRK_IR1,OCTHR); // perform tricky maneuver to negotiate outside corner // and switch to opposite side for next wall-hug pprk_scoot(0,-100,100,0,10000); pprk_scoot(100,-100,0,0,20000); hugByTime(5,RHUG,15000); // wall-hug to corner outside ROOM 2 hugTill(HUG_GT,5,RHUG,APPRK_IR2,ICTHR); // enter ROOM 2 hugByTime(5,RHUG,35000); lookForCandle(); // exit ROOM 2 with timed hug to clear next inner corner hugByTime(1,RHUG,62000); // wall-hug to next outside corner hugTill(HUG_LT,1,RHUG,APPRK_IR3,OCTHR); // perform tricky maneuver to negotiate outside corner pprk_scoot(-100,100,0,0,10000); pprk_scoot(-100,0,100,0,10000); pprk_scoot(0,100,100,0,15000); hugByTime(1,RHUG,40000); // wall-hug to next outside corner // (will stop lined up with ROOM 3 doorway) hugTill(HUG_LT,1,RHUG,APPRK_IR3,OCTHR); // enter ROOM 3 and look for candle pprk_scoot(0,100,-100,0,20000); lookForCandle(); // exit ROOM 3 pprk_scoot(0,-100,100,0,20000); // enter ROOM 4 pprk_scoot(-100,100,0,0,150000); pprk_scoot(-100,0,100,0,150000); hugByTime(4,RHUG,18000); lookForCandle(); // DONE!!! }

Contents

Comments

Is this the starting point for a truly competitive fire-fighting robot? Probably not. The robot will take about 1 minute and 15 seconds to peek in all 4 rooms. At the contest, there will be robots that are a lot faster. There is also a LOT of work left to make this a fully functional firefighter. The robot will need candle sensors and an extinguisher and programming for the "end game". The contest rules might even prohibit such a robot since it is made almost entirely from a kit. Even though this specific example may not make a competitive firefighter, the navigation principles may still be applied to other robots. The robot Nancy in our gallery used a similar combined wall-hug and dead-reckoning navigation strategy and performed quite well at the contest.

Revision History:

2003-04-19: Example Created.

Related Examples:

Multitasking PPRK Wall-Hug Example