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