/*------------------------------------------------------------------\
|                                                                   |
| A variation on the wall follower which uses a simple              |
| behaviour-based subsumption architecture implemented as a set of  |
| interacting tasks.                                                |
|                                                                   |
| Simon Parsons                                                     |
| City University of New York                                       |
| June 2003                                                         |
|                                                                   |
\------------------------------------------------------------------*/

/*
   The behaviours are organised as a set of concurrent tasks. The 
   interaction is controlled by:

   * setting semaphores to reset behaviours;

   * switching tasks off to inhibit them; and
                                            
   * suppressing them by setting semaphores.
							                        */                                                        

//------------------------------------------------------------------------
//
// Initialisation
//

// Name sensors

#define BUMP_LEFT 	SENSOR_1
#define FLOOR_LIGHT	SENSOR_2
#define BUMP_RIGHT	SENSOR_3

// Name variables

#define LEFT_MOTOR	OUT_A
#define RIGHT_MOTOR	OUT_C

// Define constants

#define THRESHOLD   50

#define DRIVE_LEFT  1
#define DRIVE_RIGHT 6
#define TURN_POWER  2

#define BACK_TIME_FIXED 10
#define BACK_TIME_RAND  20
#define TURN_TIME_FIXED 20
#define TURN_TIME_RAND  10

// Define semaphores to reset and suppress tasks

int RESET_LEFT = 0;
int RESET_RIGHT = 0;
int RESET_FORWARD = 0;
int SUPPRESS_LEFT = 0;
int SUPPRESS_RIGHT = 0;
int SUPPRESS_FORWARD = 0;

//---------------------------------------------------------------------
//
// Main task
//

// Start everything up and then hand over control to the behaviours.

task main()
{

	// Initialise sensors

	SetSensor(BUMP_LEFT, SENSOR_TOUCH);
	SetSensor(BUMP_RIGHT, SENSOR_TOUCH);
	SetSensor(FLOOR_LIGHT, SENSOR_LIGHT);

	// Set random seed

	//SetRandomSeed(20);

      // Start motors

	SetPower(LEFT_MOTOR, DRIVE_LEFT);
	SetPower(RIGHT_MOTOR, DRIVE_RIGHT);
	OnFwd(LEFT_MOTOR+RIGHT_MOTOR);

	// Start behaviours

	start GoForward;
	start WatchLeft;
	start WatchRight;
	start MonitorLight;
}

//--------------------------------------------------------------------
//
// Behaviours to implement the intentions
//

// Check for things to the left, and turn away from them.

task WatchLeft()
{
	while(true)
	{

	// Any reset code should go here

		if(RESET_LEFT)	
	 	;			

	// If we hit something on the left, inhibit forward motion and 
	// turning to the right, turn left, then renable them.

		if (BUMP_LEFT)
		{
		   stop GoForward;		
		   stop WatchRight;	
		   AvoidLeft(SUPPRESS_LEFT);
		   start WatchRight;
		   start GoForward;
		}
	}
}

// Check for things to the right, and turn away from them.

task WatchRight()
{
	while(true)
	{

	// Any reset code should go here

		if(RESET_RIGHT)	
	 	;			

	// If we hit something on the right, inhibit forward motion and 
	// turn right, then renable forward motion.

		if (BUMP_RIGHT)
		{
		   stop GoForward;		
		   AvoidRight(SUPPRESS_RIGHT);
		   start GoForward;
		}
	}
}

// Drive on.

task GoForward()
{
	while(true)
	{

	// Any reset code should go here

		if(RESET_FORWARD)	
	 	;			

	// Drive normally, but be aware of suppression.

		DriveOn(SUPPRESS_FORWARD);
	}
}


// Watch for bright patches on the floor and suppress other behaviours 
// in that case.

task MonitorLight()
{

	// Switch off motion while over a light patch.

	while(true)
	{
		if(FLOOR_LIGHT > THRESHOLD)
		{
		   SUPPRESS_FORWARD = 1;
		   SUPPRESS_RIGHT = 1;
		   SUPPRESS_LEFT = 1;
		}
		if(FLOOR_LIGHT <= THRESHOLD)
		{
		   SUPPRESS_FORWARD = 0;
		   SUPPRESS_RIGHT = 0;
		   SUPPRESS_LEFT = 0;
		}
	}
}

//--------------------------------------------------------------------
//
// The functions that create the behaviours.
//

// Backup and turn to the right unless being suppressed.

void AvoidLeft(int switch_off)
{
       if(!switch_off)
	{

		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off and
		  OnRev(LEFT_MOTOR+RIGHT_MOTOR);      // then back up.
		  Wait(BACK_TIME_FIXED
		       +Random(BACK_TIME_RAND));

		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off again,
		  SetPower(LEFT_MOTOR, TURN_POWER);   // then reset power,
		  SetPower(RIGHT_MOTOR, TURN_POWER);  // and turn.
		  OnFwd(LEFT_MOTOR);              
		  OnRev(RIGHT_MOTOR);
		  Wait(TURN_TIME_FIXED
		       +Random(TURN_TIME_RAND));
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Finally, shut 
	}						    // turn motors off.
}

// Backup and turn to the left unless suppressed.

void AvoidRight(int switch_off)
{
       if(!switch_off)
	{
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off and
		  OnRev(LEFT_MOTOR+RIGHT_MOTOR);      // then back up.
		  Wait(BACK_TIME_FIXED
		       +Random(BACK_TIME_RAND));

		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Motors off again,
		  SetPower(LEFT_MOTOR, TURN_POWER);   // then reset power,
		  SetPower(RIGHT_MOTOR, TURN_POWER);  // and turn.
		  OnFwd(RIGHT_MOTOR);              
		  OnRev(LEFT_MOTOR);
		  Wait(TURN_TIME_FIXED
		       +Random(TURN_TIME_RAND));
		  Off(LEFT_MOTOR+RIGHT_MOTOR);        // Finally, shut 
							    // turn motors off.
	}
}

// Drive ahead, but the motor powers make the robot run to the
// left. If suppressed, turn motors off.

void DriveOn(int switch_off)
{
	if(switch_off)
	{	
		Off(LEFT_MOTOR+RIGHT_MOTOR);	  // Supression means 
	}						  // turn motors off
	else
	{
		SetPower(LEFT_MOTOR, DRIVE_LEFT);   // Set power, then 		  
       	SetPower(RIGHT_MOTOR, DRIVE_RIGHT); // forward.
		OnFwd(LEFT_MOTOR+RIGHT_MOTOR);      // head off.
	}
}