summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--source/Concept/Framework/Concept.vcproj54
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c58
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.h10
-rwxr-xr-xsource/Concept/Framework/modules/logic/logic.c9
-rwxr-xr-xsource/Concept/Framework/modules/logic/logic.h20
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;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-21 21:36:34 +0100