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
Show stats Download patch file Download diff file Expand all files Collapse all files

54
source/Concept/Framework/Concept.vcproj
View file

@ -399,6 +399,60 @@
</File>
</Filter>
</Filter>
<Filter
Name="Logic"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\logic\logic.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\logic\logic.h">
</File>
</Filter>
</Filter>
<Filter
Name="Wireless"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\wireless.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\wireless.h">
</File>
</Filter>
</Filter>
<Filter
Name="Aktuator"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\executor\aktuator.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\executor\aktuator.h">
</File>
</Filter>
</Filter>
</Filter>
<Filter
Name="Hardware Interface"

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

9
source/Concept/Framework/modules/logic/logic.c
View file

@ -3,10 +3,7 @@
//-----------------------------------------------------------------------------
void Logic::OnBallOwned()
{
Wireless* ourWireless = parent->GetModule<Wireless>(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);
}

20
source/Concept/Framework/modules/logic/logic.h
View file

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