summaryrefslogtreecommitdiffstats
path: root/source/Concept/Framework/modules/executor
diff options
context:
space:
mode:
authormasterm <devnull@localhost>2007-02-23 00:04:02 +0100
committermasterm <devnull@localhost>2007-02-23 00:04:02 +0100
commite51f1c203897019ee5669e44d35cf1241514d442 (patch)
tree22cf71cf0c443e09c60aaa9adc318fea3d8d4505 /source/Concept/Framework/modules/executor
parentd8e83400c8780fdd04018cd2f59313a3e4533d71 (diff)
downloadrc2007-soccer-e51f1c203897019ee5669e44d35cf1241514d442.tar
rc2007-soccer-e51f1c203897019ee5669e44d35cf1241514d442.zip
++++ fixed navigator logics
Diffstat (limited to 'source/Concept/Framework/modules/executor')
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c58
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.h10
2 files changed, 49 insertions, 19 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