diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 58 |
1 files changed, 39 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--))
{
|