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 == EMPTY_FLOAT || newY == EMPTY_FLOAT) 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::RotateTo(float newAngle, float rotationSpeed) 
{
	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

	this->targetX = EMPTY_FLOAT;
	this->targetY = EMPTY_FLOAT;
	this->direction = EMPTY_FLOAT;
	this->rotationSpeed = min(rotationSpeed, 255.0f);	
	this->targetAngle = newAngle * PI / 180.0f;
	this->robotSpeed = 0;

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

	CalculateDirection();
}

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

	bool targetReached = false;

	if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE))
	{
		targetReached = true;
	}

	return targetReached;
}

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

	bool targetAngleReached = false;

	if(!HasTargetAngle() || (fabs(targetAngle - locationeer->GetOrientation()) < 0.1f))
	{
		targetAngleReached = true;
	}

	return targetAngleReached;
}

//-----------------------------------------------------------------------------
void Navigator::Update()
{
	if(this->direction == EMPTY_FLOAT || (HasTarget() && TargetReached()))
	{
		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
		{
			Stop();
		}
		else if(!AngleReached())
		{
			RotateTo(this->targetAngle, this->rotationSpeed);
		}
		else
		{
			Rotate(this->rotationSpeed);
		}
	}
	else
	{
		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
		{
			this->rotationSpeed = 0;
			this->targetAngle = EMPTY_FLOAT;

			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()
{
	// Use the position_tracker
	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
	// get the relative position of the robot
	float relativeDirection = this->direction - locationeer->GetOrientation();

	// calculate x and y-koordinates
	float xDrive = cos(relativeDirection + PI / 6.0f);
	float yDrive = sin(relativeDirection + PI / 6.0f);

	// calculate relative speed of engines
	float vLeft = xDrive;
	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

	// Get the maximal value
	float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));

	// calculate the correct speeds of the engines
	vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
	vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
	vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;

	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,2);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);

	// Transfer the values to the engines
	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;
}