neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Codework

Browse source
This commit is contained in:
sicarius 2007-02-22 13:12:03 +00:00
parent 9525458918
commit 655dd7522f
14 changed files with 212 additions and 166 deletions
Download patch file Download diff file Expand all files Collapse all files

149
source/Concept/Framework/modules/executor/navigator.c
View file

@ -54,30 +54,6 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
}
CalculateEngines();
//dings,,,
/*float xDrive = cos(direction + PI / 6.0f);
float yDrive = sin(direction + PI / 6.0f);
float vLeft = xDrive;
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
vLeft = vLeft * this->robotSpeed + rotationSpeed;
vBack = vBack * this->robotSpeed + rotationSpeed;
vRight = vRight * this->robotSpeed + 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;*/
}
//-----------------------------------------------------------------------------
@ -178,108 +154,43 @@ void Navigator::CalculateDirection()
//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
// Use the position_tracker
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
if(direction != EMPTY_FLOAT)
{
float relativeDirection = this->direction - locationeer->GetOrientation();
// get the relative position of the robot
float relativeDirection = this->direction - locationeer->GetOrientation();
float xDrive = cos(relativeDirection + PI / 6.0f);
float yDrive = sin(relativeDirection + PI / 6.0f);
// calculate x and y-koordinates
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;
// calculate relative speed of engines
float vLeft = xDrive;
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
/*float speedCorrection = 1.0f;
// Get the maximal value
float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));
float maxEngineSpeed = 255.0f;
float minEngineSpeed = -255.0f;
// 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;
float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
float calcSpeed = 1.0f;//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();
}
// 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;
}