From f544ab78229d3a4d54b910135ba61bb816009589 Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Thu, 22 Feb 2007 20:59:02 +0000
Subject: The Last Day ?

---
 .../Concept/Framework/modules/executor/navigator.c | 52 ++++++++++++++++++----
 .../Concept/Framework/modules/executor/navigator.h |  4 ++
 2 files changed, 47 insertions(+), 9 deletions(-)

(limited to 'source/Concept/Framework/modules/executor')

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 928a5ff..552c494 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -85,16 +85,37 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 		}
 	}
 
+	CalculateDirection();
+}
+
+void Navigator::RotateTo(float newAngle, float rotationSpeed) {
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	this->rotationSpeed = min(rotationSpeed, 255.0f);	
+	this->targetAngle = newAngle * PI / 180.0f;
+
+	if(targetAngle - locationeer->GetOrientation() > PI)
+	{
+		if(rotationSpeed > 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+	else
+	{
+		if(rotationSpeed < 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+
 	CalculateDirection();
 }
-
-//-----------------------------------------------------------------------------
-void Navigator::Update()
-{
+
+bool Navigator::TargetReached() {
 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
 
 	bool targetReached = false;
-	bool targetAngleReached = false;
 
 	if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
 	{
@@ -106,6 +127,14 @@ void Navigator::Update()
 		targetReached = true;
 	}
 
+	return targetReached;
+}
+
+bool Navigator::AngleReached() {
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	bool targetAngleReached = false;
+
 	if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
 	{
 		targetAngle = EMPTY_FLOAT;
@@ -113,15 +142,20 @@ void Navigator::Update()
 
 		targetAngleReached = true;
 	}
-
-	if(targetReached && targetAngleReached)
+	return targetAngleReached;
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	if(TargetReached() && AngleReached())
 	{
 		Stop();
 	}
-	else if(targetReached || targetAngleReached)
+	/*else if(HasTarget() && !TargetReached())
 	{
 		CalculateDirection();
-	}
+	}*/
 
 	if(!(correctionCountdown--))
 	{
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 81919f9..7f4b36d 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -49,6 +49,7 @@ public:
 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
 
 	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
+	void RotateTo(float newAngle,float roationSpeed);
 
 	void Rotate(float rotationSpeed);
 
@@ -65,6 +66,9 @@ public:
 	{
 		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
 	}
+
+	bool TargetReached();
+	bool AngleReached();
 };
 
 #endif
-- 
cgit v1.2.3