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> </File>
</Filter> </Filter>
</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>
<Filter <Filter
Name="Hardware Interface" 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->rotationSpeed = min(rotationSpeed, 255.0f);
this->targetX = newX; this->targetX = newX;
this->targetY = newY; 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->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = newSpeed; this->robotSpeed = newSpeed;
@ -88,11 +88,17 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
CalculateDirection(); 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); 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->rotationSpeed = min(rotationSpeed, 255.0f);
this->targetAngle = newAngle * PI / 180.0f; this->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = 0;
if(targetAngle - locationeer->GetOrientation() > PI) if(targetAngle - locationeer->GetOrientation() > PI)
{ {
@ -112,50 +118,64 @@ void Navigator::RotateTo(float newAngle, float rotationSpeed) {
CalculateDirection(); CalculateDirection();
} }
bool Navigator::TargetReached() { //-----------------------------------------------------------------------------
bool Navigator::TargetReached()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool targetReached = false; 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; targetReached = true;
} }
return targetReached; return targetReached;
} }
bool Navigator::AngleReached() { //-----------------------------------------------------------------------------
bool Navigator::AngleReached()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool targetAngleReached = false; 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; targetAngleReached = true;
} }
return targetAngleReached; return targetAngleReached;
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Navigator::Update() 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--)) if(!(correctionCountdown--))
{ {

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

@ -67,8 +67,18 @@ public:
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
} }
bool HasTargetAngle()
{
return (targetAngle != EMPTY_FLOAT);
}
bool TargetReached(); bool TargetReached();
bool AngleReached(); bool AngleReached();
bool IsMoving()
{
return (direction != EMPTY_FLOAT || rotationSpeed != 0);
}
}; };
#endif #endif

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

@ -3,10 +3,7 @@
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Logic::OnBallOwned() void Logic::OnBallOwned()
{ {
Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN); this->SetKeeper(false);
//ourWireless->Send(WIRELESS_CODE);
//ourWireless->Send();
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -27,12 +24,12 @@ void Logic::Update()
// is Keeper? // is Keeper?
if(isKeeper) { if(isKeeper) {
// turn around al little bit... // 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_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) {
status = STATUS_KEEPER_TURN_RIGHT; status = STATUS_KEEPER_TURN_RIGHT;
ourNavigator->RotateTo(315,200); 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; status = STATUS_KEEPER_TURN_LEFT;
ourNavigator->RotateTo(45, 200); ourNavigator->RotateTo(45, 200);
} }

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

@ -3,9 +3,6 @@
#include "../../stdafx.h" #include "../../stdafx.h"
#define STATUS_KEEPER_TURN_RIGHT 1
#define STATUS_KEEPER_TURN_LEFT 2
class Logic : public IO_Module class Logic : public IO_Module
{ {
public: public:
@ -29,9 +26,18 @@ protected:
bool isKeeper; bool isKeeper;
uint8 status; uint8 status;
enum LogicalStatus
{
STATUS_KEEPER_TURN_RIGHT,
STATUS_KEEPER_TURN_LEFT,
};
void OnBallOwned(); void OnBallOwned();
void OnBallLost(); void OnBallLost();
void OnBecomeKeeper();
void OnBecomeAttacker();
public: public:
void Update(); void Update();
@ -46,6 +52,14 @@ public:
} }
void SetKeeper(bool newStatus) { void SetKeeper(bool newStatus) {
if(!this->isKeeper && newStatus)
{
this->OnBecomeKeeper();
}
else if(this->isKeeper && !newStatus)
{
this->OnBecomeAttacker();
}
this->isKeeper = newStatus; this->isKeeper = newStatus;
} }