++++ fixed navigator logics

4 files changed, 69 insertions, 28 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
diff --git a/source/Concept/Framework/modules/logic/logic.c b/source/Concept/Framework/modules/logic/logic.c
index 00599d8..f698495 100755
--- a/source/Concept/Framework/modules/logic/logic.c
+++ b/source/Concept/Framework/modules/logic/logic.c
@@ -3,10 +3,7 @@
 //-----------------------------------------------------------------------------

 void Logic::OnBallOwned()

 {

-	Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);

-	

-	//ourWireless->Send(WIRELESS_CODE);

-	//ourWireless->Send();

+	this->SetKeeper(false);

 }

 

 //-----------------------------------------------------------------------------

@@ -27,12 +24,12 @@ void Logic::Update()
 	// is Keeper?

 	if(isKeeper) {

 		// turn around al little bit...

-		if((status == STATUS_KEEPER_TURN_LEFT && ourNavigator->AngleReached()) ||

+		if((status == STATUS_KEEPER_TURN_LEFT && !ourNavigator->IsMoving()) ||

 			 (status != STATUS_KEEPER_TURN_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) {

 			status = STATUS_KEEPER_TURN_RIGHT;

 			ourNavigator->RotateTo(315,200);

 		}

-		else if(status == STATUS_KEEPER_TURN_RIGHT && ourNavigator->AngleReached()) {

+		else if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) {

 			status = STATUS_KEEPER_TURN_LEFT;

 			ourNavigator->RotateTo(45, 200);

 		}

diff --git a/source/Concept/Framework/modules/logic/logic.h b/source/Concept/Framework/modules/logic/logic.h
index 296956b..ad8bad7 100755
--- a/source/Concept/Framework/modules/logic/logic.h
+++ b/source/Concept/Framework/modules/logic/logic.h
@@ -3,9 +3,6 @@
 

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

 

-#define STATUS_KEEPER_TURN_RIGHT 1

-#define STATUS_KEEPER_TURN_LEFT 2

-

 class Logic : public IO_Module

 {

 public:

@@ -29,9 +26,18 @@ protected:
 	bool isKeeper;

 	uint8 status;

 

+	enum LogicalStatus

+	{

+		STATUS_KEEPER_TURN_RIGHT,

+		STATUS_KEEPER_TURN_LEFT,

+	};

+

 	void OnBallOwned();

 	void OnBallLost();

 

+	void OnBecomeKeeper();

+	void OnBecomeAttacker();

+

 public:

 	void Update();

 

@@ -46,6 +52,14 @@ public:
 	}

 

 	void SetKeeper(bool newStatus) {

+		if(!this->isKeeper && newStatus)

+		{

+			this->OnBecomeKeeper();

+		}

+		else if(this->isKeeper && !newStatus)

+		{

+			this->OnBecomeAttacker();

+		}

 		this->isKeeper = newStatus;

 	}