+++ Additional Codework

2 files changed, 158 insertions, 79 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 459d4c1..691b1d0 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -7,7 +7,7 @@ void Navigator::Stop()
 	this->targetAngle = -1.0f;

 	this->targetX = -1.0f;

 	this->targetY = -1.0f;

-	this->speed = 0;

+	this->robotSpeed = 0;

 	this->rotationSpeed = 0;

 

 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)

@@ -20,12 +20,12 @@ void Navigator::Stop()
 //-----------------------------------------------------------------------------

 void Navigator::Rotate(float rotationSpeed)

 {

-	this->rotationSpeed = rotationSpeed;

+	this->rotationSpeed = min(rotationSpeed, 255.0f);;

 	this->direction = -1.0f;

 	this->targetAngle = -1.0f;

 	this->targetX = -1.0f;

 	this->targetY = -1.0f;

-	this->speed = 0;

+	this->robotSpeed = 0;

 

 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)

 	{

@@ -35,6 +35,66 @@ void Navigator::Rotate(float rotationSpeed)
 }

 

 //-----------------------------------------------------------------------------

+void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)

+{

+	this->rotationSpeed = min(rotationSpeed, 255.0f);

+	this->direction = newDirection;

+	this->targetAngle = newAngle;

+	this->targetX = -1.0f;

+	this->targetY = -1.0f;

+	this->robotSpeed = newSpeed;

+

+	if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI)

+	{

+		if(rotationSpeed > 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+	else

+	{

+		if(rotationSpeed < 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+

+	Update();

+}

+

+//-----------------------------------------------------------------------------

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

+	this->robotSpeed = newSpeed;

+

+	if(targetAngle - locationeer->GetOrientation() > PI)

+	{

+		if(rotationSpeed > 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+	else

+	{

+		if(rotationSpeed < 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+

+	Update();

+}

+

+//-----------------------------------------------------------------------------

 void Navigator::Update()

 {

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

@@ -103,97 +163,110 @@ void Navigator::Update()
 	//Calculate directional/rotational engine speed

 	if(hasDistanceToDrive)

 	{

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

 

-

-		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;

-

-		if(speed > maxRobotSpeed)

+		if(targetX >= 0 && targetY >= 0)

 		{

-			speed = maxRobotSpeed;

+			float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);

+			relativeDirection = directionToTarget - locationeer->GetOrientation();

 		}

 

-		maxMotorSpeed = speed / (sqrt(2) / 2);

+		float xDrive = cos(relativeDirection);

+		float yDrive = sin(relativeDirection);

 

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

+		float vBack = yDrive;

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

 

+		float speedCorrection = 1.0f;

 

-		relAngel = direction - orientation

-		

-		robotSpeed = sin(45) * maxMotorSpeed

-		maxMotorSpeed = robotSpeed / sin(45)

+		float maxEngineSpeed = 255.0f;

+		float minEngineSpeed = -255.0f;

 

-		if(relAngel > 45)

-		{

-			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);

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

 

-			back = speed / sin(relAngel);

-		}

-		else

+		float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

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

+

+		if(calcSpeed != 1.0f)

 		{

-			

+			speedCorrection = 1.0f / calcSpeed;

 		}

 

-		left = 

-		back = sin(relAngel) * speed

-

-

-		direction = 0:

-		orientation = 0:

-		left = speed

-		right = -speed

-		back = 0

+		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;

+		if(maxOverallSpeed > maxEngineSpeed)

+		{

+			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);

+		}

+		

 

-		direction = 0:

-		orientation = 90:

-		left = speed

-		right = speed

-		back = (sinVcos)(45) * speed

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

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

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

 

-		direction = 0:

-		orientation = 45:

-		left = speed

-		right = 0

-		back = -speed

+		maxSingleSpeed = max(max(vLeft, vBack), vRight);

+		float minSingleSpeed = min(min(vLeft, vBack), vRight);

 

-		direction = 0:

-		orientation = 180:

-		left = -speed

-		right = speed

-		back = 0

+		if(hasOrientationToAdjust)

+		{

+			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(!hasOrientationToAdjust)

+	else if(hasOrientationToAdjust)

 	{

-		Stop();

-	}

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

 	}

 }

-

-// Aktualieren ohne Parameter

-/*void Navigator::Update() {	

-	// Richtung in x und y-Kompontente zerlegen

-	double y = cos((double)direction*0.01745); // richtung ist winkel

-	double x = sin((double)direction*0.01745);

-	

-	// Abweichung der Ausrichtung ermitteln(als winkel)

-	int w = sensor.GetAusrichtung() - ausrichtung;

-

-	// Stärke der einzelnen Motoren berechnen	

-	double v0 = (-x+sqrt(3)*y)/2;

-	double v1 = x;

-	double v2 = (-x-sqrt(3)*y)/2;

-

-	// Ausgerechnete Stärke an die Motoren übergeben

-	board.motor(0,(int)((double)v0*speed +w));

-	board.motor(1,(int)((double)v1*speed +w));

-	board.motor(2,(int)((double)v2*speed +w));

-}

-

-// Aktualieren mit allen Parametern

-void Navigator::Drive(float newDirection, float newAngle, float newSpeed) {

-	SetDirection(newDirection);

-	SetAngle(newAngle);

-	SetSpeed(newSpeed);

-	Update(); // Und anwenden

-}*/

diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index ed8b3c8..8da52f6 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,6 +1,7 @@
 #ifndef _NAVIGATOR_H

 #define _NAVIGATOR_H

 

+//#include <math.h>

 #include "../../stdafx.h"

 

 class Navigator : public IO_Module

@@ -14,7 +15,7 @@ public:
 		this->targetAngle = -1.0f;

 		this->targetX = -1.0f;

 		this->targetY = -1.0f;

-		this->speed = 0;

+		this->robotSpeed = 0;

 		this->rotationSpeed = 0;

 	}

 

@@ -26,7 +27,7 @@ public:
 		this->targetAngle = -1.0f;

 		this->targetX = -1.0f;

 		this->targetY = -1.0f;

-		this->speed = 0;

+		this->robotSpeed = 0;

 		this->rotationSpeed = 0;

 	}

 

@@ -35,7 +36,7 @@ protected:
 	float targetAngle;

 	float targetX;

 	float targetY;

-	float speed;

+	float robotSpeed;

 	float rotationSpeed;

 

 public:

@@ -45,9 +46,14 @@ public:
 

 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);

 

-	void DriveTo(float newX, float newY, float newAngle);

+	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);

 

 	void Rotate(float rotationSpeed);

+

+	void SetSpeed(float newSpeed)

+	{

+		this->robotSpeed = newSpeed;

+	}

 };

 

 #endif