++++ fixed navigator logics

2 files changed, 49 insertions, 19 deletions

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