From e51f1c203897019ee5669e44d35cf1241514d442 Mon Sep 17 00:00:00 2001 From: masterm Date: Thu, 22 Feb 2007 23:04:02 +0000 Subject: ++++ fixed navigator logics --- .../Concept/Framework/modules/executor/navigator.c | 58 +++++++++++++++------- .../Concept/Framework/modules/executor/navigator.h | 10 ++++ source/Concept/Framework/modules/logic/logic.c | 9 ++-- source/Concept/Framework/modules/logic/logic.h | 20 ++++++-- 4 files changed, 69 insertions(+), 28 deletions(-) (limited to 'source/Concept/Framework/modules') 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(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(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(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 diff --git a/source/Concept/Framework/modules/logic/logic.c b/source/Concept/Framework/modules/logic/logic.c index 00599d8..f698495 100755 --- a/source/Concept/Framework/modules/logic/logic.c +++ b/source/Concept/Framework/modules/logic/logic.c @@ -3,10 +3,7 @@ //----------------------------------------------------------------------------- void Logic::OnBallOwned() { - Wireless* ourWireless = parent->GetModule(IO_WIRELESS_MAIN); - - //ourWireless->Send(WIRELESS_CODE); - //ourWireless->Send(); + this->SetKeeper(false); } //----------------------------------------------------------------------------- @@ -27,12 +24,12 @@ void Logic::Update() // is Keeper? if(isKeeper) { // turn around al little bit... - if((status == STATUS_KEEPER_TURN_LEFT && ourNavigator->AngleReached()) || + if((status == STATUS_KEEPER_TURN_LEFT && !ourNavigator->IsMoving()) || (status != STATUS_KEEPER_TURN_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) { status = STATUS_KEEPER_TURN_RIGHT; ourNavigator->RotateTo(315,200); } - else if(status == STATUS_KEEPER_TURN_RIGHT && ourNavigator->AngleReached()) { + else if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) { status = STATUS_KEEPER_TURN_LEFT; ourNavigator->RotateTo(45, 200); } diff --git a/source/Concept/Framework/modules/logic/logic.h b/source/Concept/Framework/modules/logic/logic.h index 296956b..ad8bad7 100755 --- a/source/Concept/Framework/modules/logic/logic.h +++ b/source/Concept/Framework/modules/logic/logic.h @@ -3,9 +3,6 @@ #include "../../stdafx.h" -#define STATUS_KEEPER_TURN_RIGHT 1 -#define STATUS_KEEPER_TURN_LEFT 2 - class Logic : public IO_Module { public: @@ -29,9 +26,18 @@ protected: bool isKeeper; uint8 status; + enum LogicalStatus + { + STATUS_KEEPER_TURN_RIGHT, + STATUS_KEEPER_TURN_LEFT, + }; + void OnBallOwned(); void OnBallLost(); + void OnBecomeKeeper(); + void OnBecomeAttacker(); + public: void Update(); @@ -46,6 +52,14 @@ public: } void SetKeeper(bool newStatus) { + if(!this->isKeeper && newStatus) + { + this->OnBecomeKeeper(); + } + else if(this->isKeeper && !newStatus) + { + this->OnBecomeAttacker(); + } this->isKeeper = newStatus; } -- cgit v1.2.3