neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

++++ fixed navigator logics

Browse source
This commit is contained in:
masterm 2007-02-22 23:04:02 +00:00
parent d8e83400c8
commit e51f1c2038
5 changed files with 123 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

58
source/Concept/Framework/modules/executor/navigator.c
View file

@ -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--))
{

10
source/Concept/Framework/modules/executor/navigator.h
View file

@ -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