Newsgroups: comp.robotics
Path: brunix!sgiblab!darwin.sura.net!source.asset.com!jgraham
From: jgraham@source.asset.com (Jeffrey Scott Graham)
Subject: AAAI-93 RBL report
Message-ID: <1993Jul23.112909.14625@source.asset.com>
Date: Fri, 23 Jul 1993 11:29:09 GMT
Distribution: global
Organization: Asset Source for Software Engineering Technology
Lines: 1025


REPORT from AAAI-93 Robot Building Lab


INTRODUCTION
============

As Carl Kadie suggested, I am posting this report of our robot
"Pieces", including subsumption source code.

We, team 12, were Anne Brink, Bob Touchton, and Jeffrey Graham
(me).  We had not met before the MA2 tutorial.  When the time came
to choose team mates, we were all sitting at the same table... a
no-brainer.

We fell into natural task assignments; Anne had previous Lego
experience, Bob the high-order problem solving skills, and I had
the AI programming expertise.  So, Anne did the hardware, I did the
software, and Bob (who may have had the most well-rounded
experience), got to do problem-solving, debugging hw & sw, and some
of everything else!

Our strategy was simple.  Build, test, refine & add.  These steps
were done on hardware first, then software, then both as problems
arose and needed solving.

What follows is an overview of our experiences/trials/tribulations.
All inaccuracies result directly from sleep deprivation, freezing,
minor starvation, and caffeine withdrawal.


MONDAY
======
After the MA2 tutorial, we had a team meeting to discuss strategy,
time management, egg rolls, etc.  We got to know each other, our
social and technical skills.  Schedules were attempted; Anne and
Bob both needed to attend conference sessions throughout the week. 
I was free to work on the 'bot.

We decided that a round robot would be best for purposes of
maneuvering, and for avoiding getting stuck.  Anne objected on
philosophical grounds because making the 'bot round meant
mutilating lego.  We decided that two-wheel differential drive and
two casters (one in front & one in back) would be best.  We guessed
that we would need shaft encoders to make Pieces track true.

Since magnets would be provided, we decided to incorporate them as
the simplest method to possess the coffee pot.  Everyone initially
thinks that we will push the pot into the goal zone; By Wednesday
our strategy changed to pulling the pot.

Two wheel types where provided in the kit: the tractor-type (there
were 4 of these), and the motorcycle type (2, much large diameter,
much smaller tread area).  Testing proved that the motorcycle tires
were best suited for pushing/pulling the coffee pot.

Shaft encoders were added.  We could not get the one we were
testing to work.  After an hour, we try the other one, and it is
ok.  One of the TAs show us how the encoder was being shorted out
by its own (uninsulated) leads.  All leads to all sensors are
immediately insulated to prevent further problems.

On the software side, the entire program is outlined.  Code to test
motors and sensors is fleshed out, as well as calibration routines.

Several times during the edit/compile/download cycle, the Mac
Powerbook would just hang.  The TAs inform us that IC has a memory
leak, and eventually causes the hang.  Rebooting every hour became
a habit.

With the motor software built, Pieces was made to go through the
motions: forward, backward, left turn in place, right turn in
place, right & left arc, right & left reverse arcs.  Pieces tracked
straight enough, and when it did not gears/axles/wheels where
realigned.  We really wanted to avoid doing shaft encoder software. 
Timing seemed an easy enough, accurate enough method to coordinate
turns.

Code is written to accelerate and decelerate gracefully.  This
smoothes the motions, and hopefully lengthens gear train life.

Dip switch logic is added.  A switch is dedicated for room
selection, contest selection, sensor testing, and calibration. 
This proves to be very, very useful.



WEDNESDAY
=========

Pieces gets sensors.  Two front bumper switches, one rear bumper (a
micro switch), the start light, a separate photoresistor to check
for the black floor goal area, and the IR receiver/transmitter
tower.  Later in the day, a lego piece is added across the front,
joining the two front bumper switches.  This solved a problem seen
by other 'bots during the competition-- hitting a corner without
activating a corner bump sensor.  Thursday, the rear micro switch
is abandoned in favor of an identical front-end arrangement with 2
switches and a bumper plate.  This necessitates a software change,
but it is minor.

The motorcycle tires come off their rims a lot.

The IR tower was made with lego glued to the top of the provided
wood dowel.  To keep it simple, we used all 4 provided receivers,
one in each direction.  Shielding is added, then later removed
after testing revealed that it really did not make a difference.

A rigid coffee pot gripper is designed from lego and attached to
Pieces' front end.  Testing showed little success in sticking to
the pot.  A new "vertical" design is implemented, on a base that
could swivel from side to side.  Testing showed adequate
performance.

Pieces becomes unresponsive over the course of the day's testing/
bumps/ impacts.  Grey dust from gear grinding is found underneath
the 'bot.  This is a sign of bad things to come.

Fresh motor & controller-board batteries return Pieces to his
former spastic self.

Shaft encoders are ignored.

Most of the remaining code is fleshed out.  The "find_black_floor"
behavior has not been decided.  Somewhere during the coding/testing
of the bumper behavior, the "RIGAMORTIS" bug creeps in; Pieces
refuses to move away when he bumps into something.  Reverse (the
normal first bump move) does not seem to work.  This is puzzling,
and debugging code is added to root out the problem.  

Feeling anxious with the constraints of time ( and seeing other
teams doing excellent work, especially with the fascinating
mathematics and kinetics of mounting IR sensors on the servo), we
put off solving our little rigamortis problem.


It is now after midnight.  Someone decides that, to maximize
stress, the temperature in the room should be lowered to about 60
degrees, and coffee should be denied.  Laughter could not be heard,
but was suspected, beneath much grumbling by the teams. Someone
wore a cloth that once had been part of an exhibit partition.  WE
just froze.  Coffee was found in a vending machine; it was mostly
ok if you did not look at the bottom of the styrofoam cup!


The contest start & stop code is added, tested, then modified. 
Testing begins on the IR receivers.  Immediately, we have problems. 
Pieces cannot detect the beacon, anywhere, from any distance. 
Sanity checks run rampant; code is checked, batteries, connectors--
all are ok.  Around three in the morning, we hit the panic button.
We start asking the other teams for help.  The TAs get involved. 
Finally, we look back to the code.


Anne and Bob did a great job of deciphering my code, and fixing
several omissions/mistakes.

We notice, after much wrong guessing & hacking, that our routine to
check the ir sensors is not even producing any messages on the led screen.
Further investigation reveals that, if we change the physical
location of the routine, it will at least start working.  No
explanation is provided by the TAs.  Having verified that the code
is running, our short-term happiness turns to anger as we can not
yet detect the IR beacon that all the other teams are having fun
with.


In defense of my code, it must be stated that I took the warning
quite literally from the Lab manual when it said that the IR
receiving routines should only be used when necessary.  These
routines supposedly consume huge amounts of cpu time.

Reading this warning, I wrote the code so that every time Pieces
looks at the IR sensors, the ir_receive function is called,the
sensors checked, then the ir_receive is turned off.  This, I
reasoned, was the most efficient way to use the IR.  After further
conversations with other teams (thanks guys), we rewrite the code
so the ir receive is turned on once at the beginning of the
program, and left on.  Like magic, we can find the beacon.

It begins to look like we might be able to compete.  Calmed down,
Anne begins thinking up strategy for finding the black floor.

Bob and I flesh out the IR tracking behavior.  Lots of reboots due
to the memory leak.  At one point, YACC "yaks" on us.  Seems we
cannot nest more than 12 if-then-elses.  The code is kludged to
compensate for this.


We continue tweaking the bump behavior code and puzzling over the
rigamortis, but are clueless.


THURSDAY (Wednesday for most teams)
========

Too tired to panic, rigamortis still haunts Pieces.  Just 2 hours
before "ESCAPE" contest, we hard code some steps to get Pieces
through the office door ("subsuming" the normal subsumption until
then).  We enter the contest with Pieces untested with the hacked
behavior, but we figure it could not be any worse.  Fresh batteries
are installed in some kind of cosmic "osmosis" attempt to make
Pieces work "better".


ESCAPE FROM THE OFFICE CONTEST
==============================
Pieces fails miserably, twice in a row.


THE BIG HACK
============
With 45 minutes before the coffee pot contest, I take to the code
in effort to save face.

Bob and Anne nurse the gear train and secure loose lego pieces. We
write a simple test hack to make Pieces back up.  It works great.

With this knowledge, we are convinced that the code must be at
fault.  I go ripping through the code, determined to remove the
rigamortis bug.  All code that makes the robot stop is removed. 
The hardcoded steps to escape are removed, leaving pure,simple
subsumption.  The accelerate/decelerate code is ripped out.  The
bump behavior is completely replaced with simpler logic.  Somewhere
along the line, the rigamortis is cured, though we don't know it
yet.  With little to loose, we go to the coffee pot contest.  We
have no idea what Pieces will do.  Wish those camcorders were
broke!







COFFEE POT CONTEST
==================
Talk about emergent behavior!  Pieces, free from rigamortis, comes to
life.

Round 1: Both 'bots find the pot quickly. We touch 1st, but cannot
drag the pot.  The opponent touches. We touch again. It's a tug of
war. Time expires. We win!

Round 2: The opponent touches. We touch. Time expires. Its a tie.

Round 3: Pieces meets Jack?  We win.  And Piece's gear train
quietly fails...

With Pieces, Jack, and ??? remaining, the round-robin begins. 
Pieces is scheduled for the first round.  The round begins.  Pieces
makes the default "cruise straight" behavior, contacting the wall. 
Piece's bump behavior wants to make a right turn, but the gear
train failure prevents this.  Pieces, confused, cycles near the
starting light until time expires.

With one loss, Pieces gets only one more chance.  No time to check
the gearing.  Pieces has to enter round two unrepaired.  Pieces
loses in a replay of round one.


FINAL NOTES
===========
We had great fun.  There is no better way to build interest and
raise enthusiasm than with this event.

It was very interesting to see how much could be done with so
little.  Perhaps a biased view, but our small 'bots showed as much
ability as some of those big, mega-bucks 'bots.


I lost 4 pounds.

Subsumption works, if you code it correctly (i.e., no rigamortis
built into the code).

This event should happen everywhere, all the time.


LESSONS LEARNED
===============
Piece's default/lowest-level behavior "cruise" together with
"bump", gave us a distinct advantage at the start of the coffee pot
contest.  Some 'bots relied on first locating the coffee pot
beacon, then moving toward it.  Since the pot was out of detection
range in some of the heats, these robots failed.  Some just spun
around endlessly search for the pot.

I believe the root of our rigamortis problem was in "bump" behavior,
where we got too smart about where to move in relation to where we
wanted to go.  I do not know for certain what caused the problem.

Our strategy of build/test/refine worked quite well, in hardware
and software.



SOURCE CODE NOTES
==================

An  interesting observation about the code is the intelligence per
byte: 21 global variables, 271 SLOC, about 6000 bytes downloaded
from IC to the 'bot.

About an hour before the contest, configuration management flew the
coop.  The code presented here jumped through several hoops to get
to me (who owns a Mac???): mac to unix to vms to pc to vms to
comp.robotics and rbl.  Along this networking journey, I could not
resist prettying up the code.  The "BIG HACK" was reconstructed
from memory.  Unfortunately, my memory sucks.  The "bumped"
routines are new implementations of what I believe to be equivalent
behavior... sorry.

The only other code change I made was to use #defines (as you see). 
During the contest, we did not have a functioning preprocessor, so
global ints had to be used for port assignments.

The only time Pieces worried about what side of the room he start
from was in the "do_find_black_floor" behavior.  As a heuristic,
the starting room was used to help pick a path that would hopefully
lead back to the black floor goal area.

The room variable was a part of the "bumped" behavior (we schemed
to always maneuver with respect to the starting side), but it was
removed during the BIG HACK.

Note the distinct lack of timing/procedural code.  This is the
elegance of subsumption (or maybe just luck).

The only difference between code for the two contests is the
behavior arbitrator for "find black floor", since in the coffee pot
contest we do not want to find the black floor unless we are in
possession of the coffee pot (a contact switch on our gripper).

Any comments would be greatly appreciated.  I have already read that other 
teams used subsumption, but have not seen any code!



THANKS
======
To everyone who made AAAI-93 RBL possible, thank you.





	-Jeffrey Graham



SOURCE CODE
===========
#define	TRUE 			1
#define	FALSE			0

#define	ROOM_RIGHT		0
#define	ROOM_LEFT		1

#define	CONTEST_ESCAPE		1
#define	CONTEST_COFFEE 		0

/* dip switch assignments */
#define	DIP_ROOM 		1
#define	DIP_TEST 		2
#define	DIP_CALIBRATE 		3
#define	DIP_CONTEST 		4

#define	DIP_ON 			0
#define	DIP_OFF 		1

/* DEFINE MOTOR PORTS */
#define	MOTOR_LEFT		0
#define	MOTOR_RIGHT 		3


/* DEFINE CONTACT SWITCH BUMPER PORTS */
#define	BUMPER_FRONT_LEFT	0
#define	BUMPER_FRONT_RIGHT	2
#define	BUMPER_REAR_LEFT	24
#define	BUMPER_REAR_RIGHT	25
#define	BUMPER_COFFEE_POT	3

/* DEFINE IR SENSOR PORTS */
#define	IR_DETECTOR_LEFT	4
#define	IR_DETECTOR_RIGHT	5
#define	IR_DETECTOR_FRONT	6
#define	IR_DETECTOR_REAR	7

/* DEFINE BELLY LIGHT SENSOR PORT (analog photoresistor for contest start) */
#define	START_LIGHT		26

/* DEFINE BLACK FLOOR SENSOR PORT (analog reflectance sensor for contest stop*/
#define	BLACK_FLOOR_SENSOR	27

/* Mnemonics for motor control */
#define	STOP			0
#define	FORWARD			1
#define	BACKWARD		2
#define	LEFT_TURN		3
#define	RIGHT_TURN		4
#define	LEFT_ARC		5
#define	RIGHT_ARC		6
#define	BACKWARD_LEFT_ARC	7
#define	BACKWARD_RIGHT_ARC	8

#define	msecs_per_move_normal	100
#define	msecs_per_move_bump	2000


/* global variables */
int ROOM			= ROOM_LEFT;
int CONTEST			= CONTEST_ESCAPE;
persistent int START_LIGHT_THRESHOLD;
persistent int BLACK_FLOOR_THRESHOLD;

int DEFAULT_DIRECTION		= FORWARD;
int CURRENT_DIRECTION		= FORWARD;
int IR_DIRECTION		= STOP;

/* if bump occurs, ignore the IR behavior for 3 secs to give bump behavior */
/* time to get robot unbumped/unstuck... this would be better implemented  */
/* as a higher-level behavior */
long msecs_to_defeat_ir	= 3000L;
long mtime_to_enable_ir = 0L;


/* behavior states  and variables */
int have_ir_direction	= FALSE;
int ir_direction_left	= FALSE;
int ir_direction_right	= FALSE;
int ir_direction_front	= FALSE;
int ir_direction_rear	= FALSE;

int have_black_floor	= FALSE;

int have_bump		= FALSE;
int bump_front_left	= FALSE;
int bump_front_right	= FALSE;
int bump_rear_left	= FALSE;
int bump_rear_right	= FALSE;

int have_coffee_pot	= FALSE;


/**************************************************/
/**************************************************/
/***  some utility routines                     ***/
/**************************************************/
/**************************************************/

void say(char msg[])
	{
	printf("%s\n",msg);
	wait(30);
	}

void wait(int msecs)
	{
	long timer_a;
	timer_a = mseconds() + (long) msecs;

	while ( timer_a > mseconds() )
               defer();
	}

void init()
	{
	say("Initializing...");

	/* play some startup music */
	tone(200.0,2.0);
	tone(500.0,2.0);

	/* turn on the IR receive for the coffee pot  frequency */
	set_ir_receive_frequency(100);
	ir_receive_on();

	/* check dip switches and do their routines */
	if  ( dip_switch(DIP_ROOM) == DIP_ON )
		{
		say("Starting on Left");
		ROOM = ROOM_LEFT;
		}
	else 
  		{
		say("Starting on Right");
		ROOM = ROOM_RIGHT;
		}

	if ( dip_switch(DIP_TEST) == DIP_ON  )
		init_test();

	if ( dip_switch(DIP_CALIBRATE) == DIP_ON )
		init_calibrate();

	if ( dip_switch(DIP_CONTEST)  == DIP_ON )
		{
		say("Doing Escape contest");
		CONTEST = CONTEST_ESCAPE;
		}
	else
		{
		say("Doing Coffee pot contest");
		CONTEST  = CONTEST_COFFEE;
		}
	}


void init_calibrate()
	{
	say("Calibrating Black Floor Sensor");
	BLACK_FLOOR_THRESHOLD = _calculate_analog_trigger(BLACK_FLOOR_SENSOR);
	printf("The BLACK_FLOOR_THRESHOLD is %d\n",BLACK_FLOOR_THRESHOLD);

	say("Calibrating Start Light Sensor");
	START_LIGHT_THRESHOLD = _calculate_analog_trigger(START_LIGHT);
	printf("The START_LIGHT_THRESHOLD is %d\n",START_LIGHT_THRESHOLD);

	sleep(2.0);
        }


void init_test()
	{
	/* motors */
	say("Move forward");
	move(FORWARD, msecs_per_move_normal);
	say("Move backward");
	move(BACKWARD, msecs_per_move_normal);
	say("Move left turn");
	move(LEFT_TURN, msecs_per_move_normal);
	say("Move right turn");
	move(RIGHT_TURN, msecs_per_move_normal);
	say("Move Forward left arc");
	move(LEFT_ARC, msecs_per_move_normal);
	say("Move Forward right arc");
	move(RIGHT_ARC, msecs_per_move_normal);
	say("Move Backward left arc");
	move(BACKWARD_LEFT_ARC, msecs_per_move_normal);
	say("Move Backward right arc");
	move(BACKWARD_RIGHT_ARC, msecs_per_move_normal);
	say("Move stop");
	move(STOP, msecs_per_move_normal);

	/* bumpers*/
	init_test_digital_sensor("bumper FR",BUMPER_FRONT_RIGHT);
	init_test_digital_sensor("bumper FL",BUMPER_FRONT_LEFT);
	init_test_digital_sensor("bumper RL",BUMPER_REAR_LEFT);
	init_test_digital_sensor("bumper RR",BUMPER_REAR_RIGHT);
	init_test_digital_sensor("bumper pot",BUMPER_COFFEE_POT);

	/* IR receivers */
	init_test_ir_sensor("IR Left",IR_DETECTOR_LEFT);
	init_test_ir_sensor("IR Right",IR_DETECTOR_RIGHT);
	init_test_ir_sensor("IR Front",IR_DETECTOR_FRONT);
	init_test_ir_sensor("IR Rear",IR_DETECTOR_REAR);

	/* start_light */
	init_test_analog_sensor("Start Light",START_LIGHT);

	/* for analog port black floor */
	init_test_analog_sensor("Black Floor",BLACK_FLOOR_SENSOR);
	}


void init_test_ir_sensor(char msg[], int port)
	{
	ir_receive_on();
	while ( ! left_button() )
		{
		printf("%s = %d Hit CHOOSE\n",msg, ir_counts(port) > 5);
		sleep(0.05);
		}

	ir_receive_off();
	while ( left_button() );
	}


void init_test_analog_sensor(char msg[], int port)
	{
	while ( ! left_button() )
		{
		printf("%s = %d Hit CHOOSE\n",msg, analog(port));
		sleep(0.05);
		}
	while( left_button() );
	}

void init_test_digital_sensor(char msg[], int port)
	{
	while (! left_button() )
		{
		printf("%s = %d Hit CHOOSE\n",msg, digital(port));
		sleep(0.05);
		}
	while( left_button() );
	}

void motor_cmd(int left_speed, int right_speed, int duration)
	{
	motor(MOTOR_LEFT,left_speed);
	motor(MOTOR_RIGHT,right_speed);
	wait(duration);
	}


void move(int cmd, int duration)
	{
	CURRENT_DIRECTION = cmd;

	if ( cmd == FORWARD)
		motor_cmd(100,100, duration);
	else if ( cmd == BACKWARD)
		motor_cmd(-100,-100, duration);
	else if ( cmd == LEFT_TURN )
		motor_cmd(-100,100, duration);
	else if ( cmd == RIGHT_TURN)
		motor_cmd(100,-100, duration);
	else if ( cmd == LEFT_ARC)
		motor_cmd(20,100, duration);
	else if ( cmd == RIGHT_ARC)
		motor_cmd(100,20, duration);
	else if ( cmd == BACKWARD_LEFT_ARC)
		motor_cmd(-20,-100, duration);
	else if ( cmd == BACKWARD_RIGHT_ARC)
		motor_cmd(-100,-20, duration);
	else
		/* STOP */
		motor_cmd(0,0, duration);
	}


void start_machine(int port)
	{
	ir_xmit_period= 8000;		/* Transmit at 125 Hz */
	ir_freq_rcv_select= 100;        /* Recieve 100 Hz */

	while (_average_analog_sensor(START_LIGHT, 6) > START_LIGHT_THRESHOLD )
		say("Waiting for contest to start");

	printf("G O  !!! \n");
	reset_system_time();
	set_ir_transmit_period(ir_xmit_period);
	set_ir_receive_frequency(ir_freq_rcv_select);
	ir_transmit_on();
	start_process(_stop_machine(60.), 1, 100);	/* give it one tick */
	}



/* calculate a trigger value */
int _calculate_analog_trigger(int port)
	{
	int on_value,off_value,trigger_value;

	while (! left_button() )
		{
		printf("%d when ON       Press CHOOSE\n", analog(port));
		sleep(0.05);
		}

	on_value= _average_analog_sensor(port, 20);
	while (left_button());

	while (! left_button() ) 
		{
		printf("%d when OFF     Press CHOOSE\n", analog(port));
		sleep(0.05);
		}

	off_value= _average_analog_sensor(port, 20);

	while (left_button());
	trigger_value= (on_value + off_value) / 2;
	return(trigger_value);
	}


int _average_analog_sensor(int port, int samples)
	{
	int i, sum=0;

	for (i= 0; i< samples; i++)
		sum += analog(port);

	return sum/samples;
	}


/*  hogs processor when time is up  */
void _stop_machine(float time)
	{
	while (seconds() < time);
	ir_transmit_off();	/* turn off IR */
	ir_receive_off();	/* turn off IR */
	while (1)
		{
		hog_processor();
		alloff();
		servo_off();
		beeper_off();
		}
	}




/********************************************************/
/********************************************************/
/***  the continuously running sensor data routines   ***/
/********************************************************/
/********************************************************/


void continuously_check_bumpers()
	{
	while ( TRUE )
		{
		bump_front_left		= digital(BUMPER_FRONT_LEFT);
		bump_front_right	= digital(BUMPER_FRONT_RIGHT);
		bump_rear_left		= digital(BUMPER_REAR_LEFT);
		bump_rear_right		= digital(BUMPER_REAR_RIGHT);
		have_coffee_pot		= digital(BUMPER_COFFEE_POT);

		have_bump = bump_front_left + bump_front_right + bump_rear_left + bump_rear_right;

		if  ( have_coffee_pot )
			DEFAULT_DIRECTION = BACKWARD;
		else
			DEFAULT_DIRECTION = FORWARD;

		/* if have a bump, calculate a time in the future to allow */
		/* IR tracking behavior to continue.  This helps prevent   */
		/* repeated bump/react/track cycles (by allowing default   */
		/* cruise behavior to enter the cycle */
		if ( have_bump )
			mtime_to_enable_ir = mseconds() + msecs_to_defeat_ir;

		defer();
		}
        }


/* the next three routines are necessary because AWK stack wont handle */
/* more than 12 embedded if-then-elses */
void check_triple_irs()
	{
	if ((ir_direction_left) && (ir_direction_front) && (ir_direction_right))
		IR_DIRECTION = FORWARD;
	else
	if ((ir_direction_left) && (ir_direction_rear) && (ir_direction_right))
		IR_DIRECTION = BACKWARD;
	else
	if ((ir_direction_left) && (ir_direction_front) && (ir_direction_rear))
		IR_DIRECTION = LEFT_TURN;
	else
	if ((ir_direction_right) && (ir_direction_front) && (ir_direction_rear))
		IR_DIRECTION = RIGHT_TURN;
	else
		/* should never get here */
		IR_DIRECTION = FORWARD;
	}


void check_double_irs()
	{
	if ((ir_direction_left) && (ir_direction_front))
		IR_DIRECTION = LEFT_ARC;
	else if ((ir_direction_right) && (ir_direction_front))
		IR_DIRECTION = RIGHT_ARC;
	else if ((ir_direction_rear) && (ir_direction_left))
		IR_DIRECTION = LEFT_TURN;
	else if ((ir_direction_rear) && (ir_direction_right))
		IR_DIRECTION = RIGHT_TURN;
	else
		/* should never get here */
		IR_DIRECTION = FORWARD;
	}


void check_single_irs()
	{
	if ( ir_direction_rear )
		IR_DIRECTION = LEFT_TURN;
	else if ( ir_direction_right )
		IR_DIRECTION = RIGHT_TURN;
	else if ( ir_direction_left )
		IR_DIRECTION = LEFT_TURN;
	else if ( ir_direction_front)
		IR_DIRECTION = FORWARD;
	else
		/* should never get here */
		IR_DIRECTION = FORWARD;
	}


void continuously_track_coffee_pot()
	{
	while ( TRUE)
		{
		ir_direction_left  = (ir_counts(IR_DETECTOR_LEFT)  > 5);
		ir_direction_right = (ir_counts(IR_DETECTOR_RIGHT) > 5);
		ir_direction_rear  = (ir_counts(IR_DETECTOR_REAR)  > 5);
		ir_direction_front = (ir_counts(IR_DETECTOR_FRONT) > 5);

		have_ir_direction = ir_direction_left + ir_direction_right + ir_direction_front + ir_direction_rear;
		if ( mseconds() < mtime_to_enable_ir )
			{
			printf("IR defeated by bump\n");
			have_ir_direction = FALSE;
			}
		else
			{
			if ( have_ir_direction == 3 )
				check_triple_irs();
			else if ( have_ir_direction == 2)
				check_double_irs();
			else if ( have_ir_direction == 1)
				check_single_irs();
			else
				IR_DIRECTION = DEFAULT_DIRECTION;
			}
		defer();
		}
	}


void continuously_check_black_floor()
	{
	while ( TRUE )
		{
		have_black_floor = analog(BLACK_FLOOR_SENSOR) > BLACK_FLOOR_THRESHOLD;
		defer();
                }
	}



/****************************************************/
/****************************************************/
/***   THE SUBSUMPTION STUFF.... by J.S.G.        ***/
/****************************************************/
/****************************************************/



/* the find black floor behavior */
void do_find_black_floor()
	{
	move(BACKWARD,msecs_per_move_normal);
	if ( ROOM == ROOM_LEFT )
		move(BACKWARD_LEFT_ARC, msecs_per_move_normal);
	else
		move(BACKWARD_RIGHT_ARC, msecs_per_move_normal);
	}


/* the track coffee pot behavior */
void do_track_coffee_pot()
	{
	move(IR_DIRECTION, msecs_per_move_normal);
	}


/* the ultimate behavior */
void do_stop()
	{
	move(STOP, msecs_per_move_normal);
	say("Goal Achieved");

	/* play some celebration music */
	tone(500.0,2.0);
	tone(800.0,2.0);

	_stop_machine(0.0);
	}



int whatis_reverse(int adirection)
	{
	int	reverse_direction;

	if ( adirection == FORWARD)
		reverse_direction = BACKWARD;
	else if ( adirection == BACKWARD)
		reverse_direction = FORWARD;
	else if ( adirection == LEFT_TURN )
		reverse_direction = RIGHT_TURN;
	else if ( adirection == RIGHT_TURN)
		reverse_direction = LEFT_TURN;
	else if ( adirection == LEFT_ARC)
		reverse_direction = BACKWARD_RIGHT_ARC;
	else if ( adirection == RIGHT_ARC)
		reverse_direction = BACKWARD_LEFT_ARC;
	else if ( adirection == BACKWARD_LEFT_ARC)
		reverse_direction = RIGHT_ARC;
	else
		/* BACKWARD_RIGHT_ARC */
		reverse_direction = LEFT_ARC;

	return(reverse_direction);
	}


int whatis_goaround(int adirection)
	{
	int	goaround_direction;

	if ( adirection == FORWARD)
		goaround_direction = RIGHT_ARC;
	else if ( adirection == BACKWARD)
		goaround_direction =RIGHT_ARC;
	else if ( adirection == LEFT_TURN )
		goaround_direction = LEFT_ARC;
	else if ( adirection == RIGHT_TURN)
		goaround_direction = RIGHT_ARC;
	else if ( adirection == LEFT_ARC)
		goaround_direction = RIGHT_TURN;
	else if ( adirection == RIGHT_ARC)
		goaround_direction = LEFT_TURN;
	else if ( adirection == BACKWARD_LEFT_ARC)
		goaround_direction = LEFT_TURN;
	else
		/* BACKWARD_RIGHT_ARC */
		goaround_direction = RIGHT_TURN;

	return(goaround_direction);
	}

/* the new get unbumped/unstuck behavior */
void do_bumped()
	{
	int	reverse_direction;
	int	goaround_direction;

	/* determine reverse direction */
	reverse_direction = whatis_reverse(CURRENT_DIRECTION);

	/* determine goaround direction */
	goaround_direction = whatis_goaround(CURRENT_DIRECTION);

	/* reverse */
	move(reverse_direction, msecs_per_move_bump);

	/* go around */
	move(goaround_direction, msecs_per_move_bump);
	}




/* the default behavior is to cruise straight ahead if robot has not acquired
/*  the coffee pot and revers if it has acquired the coffee pot */
void do_cruise()
	{
	move(DEFAULT_DIRECTION, msecs_per_move_normal);
	}


/* here is the behavior arbitration routine for the coffee pot contest */
void behavior_arbitrate_coffee()
	{
	while ( TRUE )
		{
		if  ( ( have_coffee_pot ) && (have_black_floor) )
			do_stop();
		else if ( have_bump )
			do_bumped();
		else if ( have_coffee_pot )
			do_find_black_floor();
		else if ( have_ir_direction )
			do_track_coffee_pot();
		else
			do_cruise();

		defer();
		}
	}



/* the behavior arbitration routine for the escape from the office contest */
void behavior_arbitrate_escape()
	{
	while ( TRUE )
		{
		if  (have_black_floor)
			do_stop();
		else if ( have_bump )
			do_bumped();
		else if ( have_ir_direction )
			do_track_coffee_pot();
		else
			do_cruise();

		defer();
		}
	}



void main()
	{
	/* do any initialization stuff */
	init();

	/* wait for contest to start */
	start_machine( START_LIGHT );

	/* enable the sensing processes that always run */
	start_process( continuously_track_coffee_pot() );
	start_process( continuously_check_bumpers() );
	start_process( continuously_check_black_floor() );

	/* start the behavior arbitration processs */
	if ( CONTEST == CONTEST_ESCAPE )
		start_process(behavior_arbitrate_escape());
	else
		start_process(behavior_arbitrate_coffee());
	}


