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

+++ improved navigator

Browse source
This commit is contained in:
masterm 2007-02-19 17:15:02 +00:00
parent c3ce4e7069
commit 354ef5f0d0
7 changed files with 254 additions and 126 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -107,7 +107,7 @@
Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx" Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"> UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}">
<File <File
RelativePath=".\main.cpp"> RelativePath=".\main.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
@ -136,14 +136,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\io_module.cpp"> RelativePath=".\modules\io_module.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\io_module.h"> RelativePath=".\modules\io_module.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -154,14 +154,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\engine.cpp"> RelativePath=".\modules\output\engine.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\engine.h"> RelativePath=".\modules\output\engine.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -172,7 +172,7 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\robot.cpp"> RelativePath=".\robot.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
@ -190,14 +190,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\sensor.cpp"> RelativePath=".\modules\input\sensor.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\sensor.h"> RelativePath=".\modules\input\sensor.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -208,14 +208,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\led.cpp"> RelativePath=".\modules\output\led.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\led.h"> RelativePath=".\modules\output\led.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -226,14 +226,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\distance_sensor.cpp"> RelativePath=".\modules\input\distance_sensor.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\distance_sensor.h"> RelativePath=".\modules\input\distance_sensor.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -244,14 +244,14 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\ir_sensor.cpp"> RelativePath=".\modules\input\ir_sensor.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\ir_sensor.h"> RelativePath=".\modules\input\ir_sensor.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -262,14 +262,140 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\kicker.cpp"> RelativePath=".\modules\output\kicker.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
Name="Header Files" Name="Header Files"
Filter=""> Filter="">
<File <File
RelativePath=".\kicker.h"> RelativePath=".\modules\output\kicker.h">
</File>
</Filter>
</Filter>
<Filter
Name="Keyboard"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\input\keyboard.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\input\keyboard.h">
</File>
</Filter>
</Filter>
<Filter
Name="Mouse_Sensor"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\input\mouse_sensor.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\input\mouse_sensor.h">
</File>
</Filter>
</Filter>
<Filter
Name="Dribbler"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\output\dribbler.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\output\dribbler.h">
</File>
</Filter>
</Filter>
<Filter
Name="Display"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\output\display.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\output\display.h">
</File>
</Filter>
</Filter>
<Filter
Name="Ball_Tracker"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\interpreter\ball_tracker.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\interpreter\ball_tracker.h">
</File>
</Filter>
</Filter>
<Filter
Name="Position_Tracker"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\interpreter\position_tracker.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\interpreter\position_tracker.h">
</File>
</Filter>
</Filter>
<Filter
Name="Navigator"
Filter="">
<Filter
Name="Source Files"
Filter="">
<File
RelativePath=".\modules\executor\navigator.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\modules\executor\navigator.h">
</File> </File>
</Filter> </Filter>
</Filter> </Filter>
@ -281,7 +407,7 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\atmega128io.cpp"> RelativePath=".\atmega128io.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter
@ -299,7 +425,7 @@
Name="Source Files" Name="Source Files"
Filter=""> Filter="">
<File <File
RelativePath=".\tools.cpp"> RelativePath=".\tools.c">
</File> </File>
</Filter> </Filter>
<Filter <Filter

2
source/Concept/Framework/RoboCode.aps
View file

File diff suppressed because one or more lines are too long

5
source/Concept/Framework/defines.h
View file

@ -58,7 +58,10 @@
//Constants //Constants
#define SPEED_PER_PWM 1 #define SPEED_PER_PWM 1
#define DISTANCE_PER_VALUE 1 #define DISTANCE_PER_VALUE 1
#define PI 3.14159265f #define PI 3.14159265358979323846f
#define CYCLES_PER_CORRECTION 200
#define EMPTY_FLOAT 81188.1484f
//IO Module Names //IO Module Names
enum IOModuleNames enum IOModuleNames

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

@ -3,10 +3,10 @@
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void Navigator::Stop() void Navigator::Stop()
{ {
this->direction = -1.0f; this->direction = EMPTY_FLOAT;
this->targetAngle = -1.0f; this->targetAngle = EMPTY_FLOAT;
this->targetX = -1.0f; this->targetX = EMPTY_FLOAT;
this->targetY = -1.0f; this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0; this->robotSpeed = 0;
this->rotationSpeed = 0; this->rotationSpeed = 0;
@ -21,10 +21,10 @@ void Navigator::Stop()
void Navigator::Rotate(float rotationSpeed) void Navigator::Rotate(float rotationSpeed)
{ {
this->rotationSpeed = min(rotationSpeed, 255.0f);; this->rotationSpeed = min(rotationSpeed, 255.0f);;
this->direction = -1.0f; this->direction = EMPTY_FLOAT;
this->targetAngle = -1.0f; this->targetAngle = EMPTY_FLOAT;
this->targetX = -1.0f; this->targetX = EMPTY_FLOAT;
this->targetY = -1.0f; this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0; this->robotSpeed = 0;
for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
@ -40,26 +40,20 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
this->rotationSpeed = min(rotationSpeed, 255.0f); this->rotationSpeed = min(rotationSpeed, 255.0f);
this->direction = newDirection; this->direction = newDirection;
this->targetAngle = newAngle; this->targetAngle = newAngle;
this->targetX = -1.0f; this->targetX = EMPTY_FLOAT;
this->targetY = -1.0f; this->targetY = EMPTY_FLOAT;
this->robotSpeed = newSpeed; this->robotSpeed = newSpeed;
if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI) if(targetAngle == EMPTY_FLOAT)
{ {
if(rotationSpeed > 0) rotationSpeed = 0;
{
rotationSpeed = -rotationSpeed;
}
} }
else else
{ {
if(rotationSpeed < 0) rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
{
rotationSpeed = -rotationSpeed;
}
} }
Update(); CalculateEngines();
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -91,7 +85,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
} }
} }
Update(); CalculateDirection();
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -99,94 +93,87 @@ void Navigator::Update()
{ {
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool hasDistanceToDrive = true; bool targetReached = false;
bool hasOrientationToAdjust = true; bool targetAngleReached = false;
//Check for distance to drive if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
if((targetX >= 0) != (targetY >= 0))
{ {
targetX = -1.0f; targetX = EMPTY_FLOAT;
targetY = -1.0f; targetY = EMPTY_FLOAT;
direction = EMPTY_FLOAT;
robotSpeed = 0;
hasDistanceToDrive = false; targetReached = true;
}
else if(targetX >= 0 && targetY >= 0)
{
if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{
targetX = -1.0f;
targetY = -1.0f;
hasDistanceToDrive = false;
}
else
{
hasDistanceToDrive = true;
}
}
else
{
if(direction >= 0)
{
hasDistanceToDrive = true;
}
else
{
hasDistanceToDrive = false;
}
} }
//Check for orientation to adjust if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
if(targetAngle >= 0)
{ {
if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) targetAngle = EMPTY_FLOAT;
{ rotationSpeed = 0;
hasOrientationToAdjust = false;
} targetAngleReached = true;
else
{
hasOrientationToAdjust = true;
}
}
else
{
if(rotationSpeed != 0)
{
hasOrientationToAdjust = true;
}
else
{
hasOrientationToAdjust = false;
}
} }
//Calculate directional/rotational engine speed if(targetReached && targetAngleReached)
if(hasDistanceToDrive) {
Stop();
}
else if(targetReached || targetAngleReached)
{
CalculateDirection();
}
if(!(correctionCountdown--))
{
CalculateDirection();
}
}
//-----------------------------------------------------------------------------
void Navigator::CalculateDirection()
{
correctionCountdown = CYCLES_PER_CORRECTION;
if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
if(HasTarget())
{
direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
}
if(targetAngle != EMPTY_FLOAT)
{
rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
}
CalculateEngines();
}
//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
if(direction != EMPTY_FLOAT)
{ {
float relativeDirection = this->direction - locationeer->GetOrientation(); float relativeDirection = this->direction - locationeer->GetOrientation();
if(targetX >= 0 && targetY >= 0) float xDrive = cos(relativeDirection + PI / 6.0f);
{ float yDrive = sin(relativeDirection + PI / 6.0f);
float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
relativeDirection = directionToTarget - locationeer->GetOrientation();
}
float xDrive = cos(relativeDirection); float vLeft = xDrive;
float yDrive = sin(relativeDirection); float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f;
float vBack = yDrive;
float vRight = (-yDrive - sqrt(3) * xDrive)/2;
float speedCorrection = 1.0f; float speedCorrection = 1.0f;
float maxEngineSpeed = 255.0f; float maxEngineSpeed = 255.0f;
float minEngineSpeed = -255.0f; float minEngineSpeed = -255.0f;
float maxSingleSpeed = max(max(vLeft, vBack), vRight); float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + /*float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
vBack * (vBack + vLeft + vRight)); vBack * (vBack + vLeft + vRight));*/
if(calcSpeed != 1.0f) if(calcSpeed != 1.0f)
{ {
@ -204,10 +191,10 @@ void Navigator::Update()
vBack = vBack * this->robotSpeed * speedCorrection; vBack = vBack * this->robotSpeed * speedCorrection;
vRight = vRight * this->robotSpeed * speedCorrection; vRight = vRight * this->robotSpeed * speedCorrection;
maxSingleSpeed = max(max(vLeft, vBack), vRight); maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float minSingleSpeed = min(min(vLeft, vBack), vRight); float minSingleSpeed = min(min(vLeft, vBack), vRight);
if(hasOrientationToAdjust) if(rotationSpeed)
{ {
if(this->rotationSpeed > 0) if(this->rotationSpeed > 0)
{ {
@ -252,7 +239,7 @@ void Navigator::Update()
curEngine->SetEnabled(true); curEngine->SetEnabled(true);
curEngine = NULL; curEngine = NULL;
} }
else if(hasOrientationToAdjust) else if(rotationSpeed)
{ {
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT); Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(this->rotationSpeed); curEngine->SetSpeed(this->rotationSpeed);

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

@ -11,10 +11,11 @@ public:
{ {
this->parent = NULL; this->parent = NULL;
this->moduleId = 0; this->moduleId = 0;
this->direction = -1.0f; this->correctionCountdown = CYCLES_PER_CORRECTION;
this->targetAngle = -1.0f; this->direction = EMPTY_FLOAT;
this->targetX = -1.0f; this->targetAngle = EMPTY_FLOAT;
this->targetY = -1.0f; this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0; this->robotSpeed = 0;
this->rotationSpeed = 0; this->rotationSpeed = 0;
} }
@ -23,15 +24,17 @@ public:
{ {
this->parent = NULL; this->parent = NULL;
this->moduleId = navigatorId; this->moduleId = navigatorId;
this->direction = -1.0f; this->correctionCountdown = CYCLES_PER_CORRECTION;
this->targetAngle = -1.0f; this->direction = EMPTY_FLOAT;
this->targetX = -1.0f; this->targetAngle = EMPTY_FLOAT;
this->targetY = -1.0f; this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0; this->robotSpeed = 0;
this->rotationSpeed = 0; this->rotationSpeed = 0;
} }
protected: protected:
uint16 correctionCountdown;
float direction; float direction;
float targetAngle; float targetAngle;
float targetX; float targetX;
@ -54,6 +57,15 @@ public:
{ {
this->robotSpeed = newSpeed; this->robotSpeed = newSpeed;
} }
void CalculateDirection();
void CalculateEngines();
bool HasTarget()
{
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
}
}; };
#endif #endif

6
source/Concept/Framework/modules/interpreter/ball_tracker.c
View file

@ -68,7 +68,7 @@ void Ball_Tracker::Update()
mainDirection = 33.0f * PI / 18.0f; mainDirection = 33.0f * PI / 18.0f;
break; break;
default: default:
mainDirection = -1.0f; mainDirection = EMPTY_FLOAT;
return; return;
break; break;
} }
@ -104,7 +104,7 @@ void Ball_Tracker::Update()
secondDirection = 33.0f * PI / 18.0f; secondDirection = 33.0f * PI / 18.0f;
break; break;
default: default:
secondDirection = -1.0f; secondDirection = EMPTY_FLOAT;
return; return;
break; break;
} }
@ -120,6 +120,6 @@ void Ball_Tracker::Update()
} }
else else
{ {
direction = -1.0f; direction = EMPTY_FLOAT;
} }
} }

4
source/Concept/Framework/modules/interpreter/ball_tracker.h
View file

@ -10,14 +10,14 @@ public:
{ {
this->parent = NULL; this->parent = NULL;
this->moduleId = 0; this->moduleId = 0;
this->direction = -1.0f; this->direction = EMPTY_FLOAT;
} }
Ball_Tracker(uint32 trackerId) Ball_Tracker(uint32 trackerId)
{ {
this->parent = NULL; this->parent = NULL;
this->moduleId = trackerId; this->moduleId = trackerId;
this->direction = -1.0f; this->direction = EMPTY_FLOAT;
} }
protected: protected: