/*------------------------------------------------------------------\
|                                                                   |
| A variation on the wall follower which uses a simple              |
| priority-based, behaviour-based architecture.                     |
|                                                                   |
| Simon Parsons                                                     |
| City University of New York                                       |
| June 2003                                                         |
|                                                                   |
\------------------------------------------------------------------*/

/*
   The behaviours are arranged in a stack of if...else statements. 
   The highest priority task is at the top, the lowest at the 
   bottom.
                                                                   */
                                                        
// Name sensors

#define BUMP_LEFT 	SENSOR_1
#define BUMP_RIGHT	SENSOR_3

// Name variables

#define LEFT_MOTOR	OUT_A
#define RIGHT_MOTOR	OUT_C

// Define constants

#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

task main()
{

	// Initialise sensors

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

	// Set random seed

	//SetRandomSeed(20);

      // Start motors

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

	while(true)
	{

       // If there is something on the left, backup and then
       // turn right away from it.

		if (BUMP_LEFT == 1)
		{
		  AvoidLeft();
		}

       // If there is something on the right, backup and then
       // turn left away from it.

		else
		if (BUMP_RIGHT ==1)
		{
		  AvoidRight();
		}

	// If nothing on left or right, drive normally

		else
		{
		  DriveOn();
		}
	}


}

// The functions that create the behaviours.

void AvoidLeft()
{
		  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.
}

void AvoidRight()
{
		  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.
}

void DriveOn()
{
	SetPower(LEFT_MOTOR, DRIVE_LEFT);   // Set power, then
  SetPower(RIGHT_MOTOR, DRIVE_RIGHT); // drive on.
	OnFwd(LEFT_MOTOR+RIGHT_MOTOR);      //

}