Codework

1 files changed, 30 insertions, 119 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 5dbf8bf..928a5ff 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -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();

-

-		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;

+	// get the relative position of the robot

+	float relativeDirection = this->direction - locationeer->GetOrientation();

 

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

+	// calculate x and y-koordinates

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

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

 

-		float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

-		//									vBack * (vBack + vLeft + vRight));

+	// calculate relative speed of engines

+	float vLeft = xDrive;

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

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

 

-		if(calcSpeed != 1.0f)

-		{

-			speedCorrection = 1.0f / calcSpeed;

-		}

+	// Get the maximal value

+	float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));

 

-		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;

-		if(maxOverallSpeed > maxEngineSpeed)

-		{

-			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);

-		}*/

-		

+	// 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;

 

-		vLeft = vLeft * this->robotSpeed;// * speedCorrection;

-		vBack = vBack * this->robotSpeed;// * speedCorrection;

-		vRight = vRight * this->robotSpeed;// * speedCorrection;

+	//(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);

 

-		/*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;

 }