summaryrefslogtreecommitdiffstats
path: root/source/Concept/Framework/modules/executor
diff options
context:
space:
mode:
authorsicarius <devnull@localhost>2007-02-22 21:59:02 +0100
committersicarius <devnull@localhost>2007-02-22 21:59:02 +0100
commitf544ab78229d3a4d54b910135ba61bb816009589 (patch)
tree6b1d9aad746299631a0b67661c2d2be62b5e7525 /source/Concept/Framework/modules/executor
parentdc8b1cde313f3f8a60462187bbeeea4277fd0313 (diff)
downloadrc2007-soccer-f544ab78229d3a4d54b910135ba61bb816009589.tar
rc2007-soccer-f544ab78229d3a4d54b910135ba61bb816009589.zip
The Last Day ?
Diffstat (limited to 'source/Concept/Framework/modules/executor')
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c52
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.h4
2 files changed, 47 insertions, 9 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 928a5ff..552c494 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -86,15 +86,36 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
}
CalculateDirection();
+}
+
+void Navigator::RotateTo(float newAngle, float rotationSpeed) {
+ Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+ this->rotationSpeed = min(rotationSpeed, 255.0f);
+ this->targetAngle = newAngle * PI / 180.0f;
+
+ if(targetAngle - locationeer->GetOrientation() > PI)
+ {
+ if(rotationSpeed > 0)
+ {
+ rotationSpeed = -rotationSpeed;
+ }
+ }
+ else
+ {
+ if(rotationSpeed < 0)
+ {
+ rotationSpeed = -rotationSpeed;
+ }
+ }
+
+ CalculateDirection();
}
-
-//-----------------------------------------------------------------------------
-void Navigator::Update()
-{
+
+bool Navigator::TargetReached() {
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool targetReached = false;
- bool targetAngleReached = false;
if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{
@@ -106,6 +127,14 @@ void Navigator::Update()
targetReached = true;
}
+ return targetReached;
+}
+
+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)
{
targetAngle = EMPTY_FLOAT;
@@ -113,15 +142,20 @@ void Navigator::Update()
targetAngleReached = true;
}
-
- if(targetReached && targetAngleReached)
+ return targetAngleReached;
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+ if(TargetReached() && AngleReached())
{
Stop();
}
- else if(targetReached || targetAngleReached)
+ /*else if(HasTarget() && !TargetReached())
{
CalculateDirection();
- }
+ }*/
if(!(correctionCountdown--))
{
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 81919f9..7f4b36d 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -49,6 +49,7 @@ public:
void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
+ void RotateTo(float newAngle,float roationSpeed);
void Rotate(float rotationSpeed);
@@ -65,6 +66,9 @@ public:
{
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
}
+
+ bool TargetReached();
+ bool AngleReached();
};
#endif