diff options
| author | sicarius <devnull@localhost> | 2007-02-22 21:59:02 +0100 |
|---|---|---|
| committer | sicarius <devnull@localhost> | 2007-02-22 21:59:02 +0100 |
| commit | f544ab78229d3a4d54b910135ba61bb816009589 (patch) | |
| tree | 6b1d9aad746299631a0b67661c2d2be62b5e7525 /source/Concept/Framework/modules/executor | |
| parent | dc8b1cde313f3f8a60462187bbeeea4277fd0313 (diff) | |
| download | rc2007-soccer-f544ab78229d3a4d54b910135ba61bb816009589.tar rc2007-soccer-f544ab78229d3a4d54b910135ba61bb816009589.zip | |
The Last Day ?
Diffstat (limited to 'source/Concept/Framework/modules/executor')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 52 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.h | 4 |
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 |