5 files changed, 123 insertions, 28 deletions
diff --git a/source/Concept/Framework/Concept.vcproj b/source/Concept/Framework/Concept.vcproj
index 5de195f..b06f1f4 100644
--- a/source/Concept/Framework/Concept.vcproj
+++ b/source/Concept/Framework/Concept.vcproj
@@ -399,6 +399,60 @@
 					</File>
 				</Filter>
 			</Filter>
+			<Filter
+				Name="Logic"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\logic\logic.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\logic\logic.h">
+					</File>
+				</Filter>
+			</Filter>
+			<Filter
+				Name="Wireless"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\wireless.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\wireless.h">
+					</File>
+				</Filter>
+			</Filter>
+			<Filter
+				Name="Aktuator"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\executor\aktuator.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\executor\aktuator.h">
+					</File>
+				</Filter>
+			</Filter>
 		</Filter>
 		<Filter
 			Name="Hardware Interface"
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;
 	}