From e51f1c203897019ee5669e44d35cf1241514d442 Mon Sep 17 00:00:00 2001
From: masterm <devnull@localhost>
Date: Thu, 22 Feb 2007 23:04:02 +0000
Subject: ++++ fixed navigator logics

---
 .../Concept/Framework/modules/executor/navigator.c | 58 +++++++++++++++-------
 .../Concept/Framework/modules/executor/navigator.h | 10 ++++
 2 files changed, 49 insertions(+), 19 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 552c494..023bf79 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -66,7 +66,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	this->rotationSpeed = min(rotationSpeed, 255.0f);
 	this->targetX = newX;
 	this->targetY = newY;
-	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
+	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
 	this->targetAngle = newAngle * PI / 180.0f;
 	this->robotSpeed = newSpeed;
 
@@ -88,11 +88,17 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	CalculateDirection();
 }
 
-void Navigator::RotateTo(float newAngle, float rotationSpeed) {
+//-----------------------------------------------------------------------------
+void Navigator::RotateTo(float newAngle, float rotationSpeed) 
+{
 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
 
+	this->targetX = EMPTY_FLOAT;
+	this->targetY = EMPTY_FLOAT;
+	this->direction = EMPTY_FLOAT;
 	this->rotationSpeed = min(rotationSpeed, 255.0f);	
 	this->targetAngle = newAngle * PI / 180.0f;
+	this->robotSpeed = 0;
 
 	if(targetAngle - locationeer->GetOrientation() > PI)
 	{
@@ -112,50 +118,64 @@ void Navigator::RotateTo(float newAngle, float rotationSpeed) {
 	CalculateDirection();
 }
 
-bool Navigator::TargetReached() {
+//-----------------------------------------------------------------------------
+bool Navigator::TargetReached() 
+{
 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
 
 	bool targetReached = false;
 
-	if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+	if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f))
 	{
-		targetX = EMPTY_FLOAT;
-		targetY = EMPTY_FLOAT;
-		direction = EMPTY_FLOAT;
-		robotSpeed = 0;
-
 		targetReached = true;
 	}
 
 	return targetReached;
 }
 
-bool Navigator::AngleReached() {
+//-----------------------------------------------------------------------------
+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)
+	if(!HasTargetAngle() || (fabs(targetAngle - locationeer->GetOrientation()) < 0.1f))
 	{
-		targetAngle = EMPTY_FLOAT;
-		rotationSpeed = 0;
-
 		targetAngleReached = true;
 	}
+
 	return targetAngleReached;
 }
 
 //-----------------------------------------------------------------------------
 void Navigator::Update()
 {
-	if(TargetReached() && AngleReached())
+	if(this->direction == EMPTY_FLOAT || (HasTarget() && TargetReached()))
 	{
-		Stop();
+		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
+		{
+			Stop();
+		}
+		else if(!AngleReached())
+		{
+			RotateTo(this->targetAngle, this->rotationSpeed);
+		}
+		else
+		{
+			Rotate(this->rotationSpeed);
+		}
 	}
-	/*else if(HasTarget() && !TargetReached())
+	else
 	{
-		CalculateDirection();
-	}*/
+		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
+		{
+			this->rotationSpeed = 0;
+			this->targetAngle = EMPTY_FLOAT;
+
+			CalculateDirection();
+		}
+	}
 
 	if(!(correctionCountdown--))
 	{
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 7f4b36d..53fa74b 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -67,8 +67,18 @@ public:
 		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
 	}
 
+	bool HasTargetAngle()
+	{
+		return (targetAngle != EMPTY_FLOAT);
+	}
+
 	bool TargetReached();
 	bool AngleReached();
+
+	bool IsMoving()
+	{
+		return (direction != EMPTY_FLOAT || rotationSpeed != 0);
+	}
 };
 
 #endif
-- 
cgit v1.2.3