import lejos.robotics.subsumption.Behavior;

/* 
 * Drive behavior simply drives the robot in a straight line.
 * Should have lowest priority.
 */
public class Drive implements Behavior
{

	// boolean variable to allow arbitrator thread to stop the action method
	boolean suspended = false;
	
	@Override
	// Is called by arbitrator thread to check if behavior wants to kick in.
	// It always returns true so it would block any lower priority behavior.
	// Thuss, this must be the lowest priority behavior!
	public boolean takeControl() 
	{
		return true;
	}

	@Override
	// Is called by arbitrator thread when this behavior takes over. Note that
	// this behavior must quit immediately as soon as 'suspended' is true!
	public void action() 
	{
		suspended = false;
		MyMain.leftMotor.setSpeed(MyMain.NORMAL_SPEED);
		MyMain.rightMotor.setSpeed(MyMain.NORMAL_SPEED);
		
		MyMain.leftMotor.forward();
		MyMain.rightMotor.forward();
		
		while (!suspended)
		{
			Thread.yield();
		}
		
		MyMain.leftMotor.stop(true);
		MyMain.rightMotor.stop();
	}

	@Override
	// Is called by arbitrator thread to stop this behavior.
	public void suppress()
	{
		suspended = true;
	}
}