+++ Additional Codework

2 files changed, 252 insertions, 0 deletions

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
new file mode 100755
index 0000000..459d4c1
--- /dev/null
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -0,0 +1,199 @@
+#include "navigator.h"

+

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

+void Navigator::Stop()

+{	

+	this->direction = -1.0f;

+	this->targetAngle = -1.0f;

+	this->targetX = -1.0f;

+	this->targetY = -1.0f;

+	this->speed = 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 = rotationSpeed;

+	this->direction = -1.0f;

+	this->targetAngle = -1.0f;

+	this->targetX = -1.0f;

+	this->targetY = -1.0f;

+	this->speed = 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::Update()

+{

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

+

+	bool hasDistanceToDrive = true;

+	bool hasOrientationToAdjust = true;

+

+	//Check for distance to drive

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

+	{

+		targetX = -1.0f;

+		targetY = -1.0f;

+

+		hasDistanceToDrive = false;

+	}

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

+	{

+		if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)

+		{

+			targetX = -1.0f;

+			targetY = -1.0f;

+

+			hasDistanceToDrive = false;

+		}

+		else

+		{

+			hasDistanceToDrive = true;

+		}

+	}

+	else

+	{

+		if(direction >= 0)

+		{

+			hasDistanceToDrive = true;

+		}

+		else

+		{

+			hasDistanceToDrive = false;

+		}

+	}

+

+	//Check for orientation to adjust

+	if(targetAngle >= 0)

+	{

+		if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)

+		{

+			hasOrientationToAdjust = false;

+		}

+		else

+		{

+			hasOrientationToAdjust = true;

+		}

+	}

+	else

+	{

+		if(rotationSpeed != 0)

+		{

+			hasOrientationToAdjust = true;

+		}

+		else

+		{

+			hasOrientationToAdjust = false;

+		}

+	}

+

+	//Calculate directional/rotational engine speed

+	if(hasDistanceToDrive)

+	{

+

+

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

+

+		if(speed > maxRobotSpeed)

+		{

+			speed = maxRobotSpeed;

+		}

+

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

+

+

+

+		relAngel = direction - orientation

+		

+		robotSpeed = sin(45) * maxMotorSpeed

+		maxMotorSpeed = robotSpeed / sin(45)

+

+		if(relAngel > 45)

+		{

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

+

+			back = speed / sin(relAngel);

+		}

+		else

+		{

+			

+		}

+

+		left = 

+		back = sin(relAngel) * speed

+

+

+		direction = 0:

+		orientation = 0:

+		left = speed

+		right = -speed

+		back = 0

+

+		direction = 0:

+		orientation = 90:

+		left = speed

+		right = speed

+		back = (sinVcos)(45) * speed

+

+		direction = 0:

+		orientation = 45:

+		left = speed

+		right = 0

+		back = -speed

+

+		direction = 0:

+		orientation = 180:

+		left = -speed

+		right = speed

+		back = 0

+

+	}

+	else if(!hasOrientationToAdjust)

+	{

+		Stop();

+	}

+	else

+	{

+	}

+}

+

+// 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
new file mode 100755
index 0000000..ed8b3c8
--- /dev/null
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -0,0 +1,53 @@
+#ifndef _NAVIGATOR_H

+#define _NAVIGATOR_H

+

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

+

+class Navigator : public IO_Module

+{

+public:

+	Navigator()

+	{

+		this->parent = NULL;

+		this->moduleId = 0;

+		this->direction = -1.0f;

+		this->targetAngle = -1.0f;

+		this->targetX = -1.0f;

+		this->targetY = -1.0f;

+		this->speed = 0;

+		this->rotationSpeed = 0;

+	}

+

+	Navigator(uint32 navigatorId)

+	{

+		this->parent = NULL;

+		this->moduleId = navigatorId;

+		this->direction = -1.0f;

+		this->targetAngle = -1.0f;

+		this->targetX = -1.0f;

+		this->targetY = -1.0f;

+		this->speed = 0;

+		this->rotationSpeed = 0;

+	}

+

+protected:

+	float direction; 

+	float targetAngle;

+	float targetX;

+	float targetY;

+	float speed;

+	float rotationSpeed;

+

+public:

+	void Update();	

+

+	void Stop();

+

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

+

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

+

+	void Rotate(float rotationSpeed);

+};

+

+#endif