From 3c3c628b617dc53f0b7b59285c7d67888074c33d Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Sun, 18 Feb 2007 00:14:00 +0000
Subject: +++ Additional Codework

---
 .../Concept/Framework/modules/executor/navigator.c | 199 +++++++++++++++++++++
 1 file changed, 199 insertions(+)
 create mode 100755 source/Concept/Framework/modules/executor/navigator.c

(limited to 'source/Concept/Framework/modules/executor/navigator.c')

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
+}*/
-- 
cgit v1.2.3