rc2007-soccer/source/Concept/Framework/modules/executor/navigator.c

#include "navigator.h"

//-----------------------------------------------------------------------------
void Navigator::Stop()
{	
	this->direction = EMPTY_FLOAT;
	this->targetAngle = EMPTY_FLOAT;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = 0;
	this->rotationSpeed = 0;

	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		(parent->GetModule<Engine>(i))->SetSpeed(0);
		(parent->GetModule<Engine>(i))->SetEnabled(true);
	}
}

//-----------------------------------------------------------------------------
void Navigator::Rotate(float rotationSpeed)
{
	this->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed);
	this->direction = EMPTY_FLOAT;
	this->targetAngle = EMPTY_FLOAT;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = 0;

	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
	{
		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
		(parent->GetModule<Engine>(i))->SetEnabled(true);
	}
}

//-----------------------------------------------------------------------------
void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)
{
	this->rotationSpeed = min(rotationSpeed, 255.0f);
	this->direction = newDirection * PI / 180.0f;
	this->targetAngle = newAngle * PI / 180.0f;
	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->robotSpeed = newSpeed;

	if(targetAngle == EMPTY_FLOAT)
	{
		rotationSpeed = 0;
	}
	else
	{
		rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
	}

	CalculateEngines();
}

//-----------------------------------------------------------------------------
void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
{
	if(newX < 0 || newY < 0) return;

	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

	this->rotationSpeed = min(rotationSpeed, 255.0f);
	this->targetX = newX;
	this->targetY = newY;
	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
	this->targetAngle = newAngle * PI / 180.0f;
	this->robotSpeed = newSpeed;

	if(targetAngle - locationeer->GetOrientation() > PI)
	{
		if(rotationSpeed > 0)
		{
			rotationSpeed = -rotationSpeed;
		}
	}
	else
	{
		if(rotationSpeed < 0)
		{
			rotationSpeed = -rotationSpeed;
		}
	}

	CalculateDirection();
}

//-----------------------------------------------------------------------------
void Navigator::Update()
{
	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

	bool targetReached = false;
	bool targetAngleReached = false;

	if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
	{
		targetX = EMPTY_FLOAT;
		targetY = EMPTY_FLOAT;
		direction = EMPTY_FLOAT;
		robotSpeed = 0;

		targetReached = true;
	}

	if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
	{
		targetAngle = EMPTY_FLOAT;
		rotationSpeed = 0;

		targetAngleReached = true;
	}

	if(targetReached && targetAngleReached)
	{
		Stop();
	}
	else if(targetReached || targetAngleReached)
	{
		CalculateDirection();
	}

	if(!(correctionCountdown--))
	{
		CalculateDirection();
	}
}

//-----------------------------------------------------------------------------
void Navigator::CalculateDirection()
{
	correctionCountdown = CYCLES_PER_CORRECTION;

	if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;

	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	
	if(HasTarget())
	{
		direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
	}

	if(targetAngle != EMPTY_FLOAT)
	{
		rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
	}

	CalculateEngines();
}

//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
	if(direction != EMPTY_FLOAT)
	{
		float relativeDirection = this->direction - locationeer->GetOrientation();

		float xDrive = cos(relativeDirection + PI / 6.0f);
		float yDrive = sin(relativeDirection + PI / 6.0f);

		float vLeft = xDrive;
		float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
		float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

		float speedCorrection = 1.0f;

		float maxEngineSpeed = 255.0f;
		float minEngineSpeed = -255.0f;

		float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

		/*float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
											vBack * (vBack + vLeft + vRight));*/

		if(calcSpeed != 1.0f)
		{
			speedCorrection = 1.0f / calcSpeed;
		}

		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
		if(maxOverallSpeed > maxEngineSpeed)
		{
			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
		}
		

		vLeft = vLeft * this->robotSpeed * speedCorrection;
		vBack = vBack * this->robotSpeed * speedCorrection;
		vRight = vRight * this->robotSpeed * speedCorrection;

		maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
		float minSingleSpeed = min(min(vLeft, vBack), vRight);

		if(rotationSpeed)
		{
			if(this->rotationSpeed > 0)
			{
				if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
				{
					vLeft += maxEngineSpeed - maxSingleSpeed;
					vBack += maxEngineSpeed - maxSingleSpeed;
					vRight += maxEngineSpeed - maxSingleSpeed;
				}
				else
				{
					vLeft += this->rotationSpeed;
					vBack += this->rotationSpeed;
					vRight += this->rotationSpeed;
				}
			}
			else
			{
				if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
				{
					vLeft -= minEngineSpeed - minSingleSpeed;
					vBack -= minEngineSpeed - minSingleSpeed;
					vRight -= minEngineSpeed - minSingleSpeed;
				}
				else
				{
					vLeft -= this->rotationSpeed;
					vBack -= this->rotationSpeed;
					vRight -= this->rotationSpeed;
				}
			}
		}

		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
		curEngine->SetSpeed(vLeft);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
		curEngine->SetSpeed(vBack);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
		curEngine->SetSpeed(vRight);
		curEngine->SetEnabled(true);
		curEngine = NULL;
	}
	else if(rotationSpeed)
	{
		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
		curEngine->SetSpeed(this->rotationSpeed);
		curEngine->SetEnabled(true);
		curEngine = NULL;
	}	
	else
	{
		Stop();
	}
}