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"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}">
<File
RelativePath=".\main.cpp">
RelativePath=".\main.c">
</File>
</Filter>
<Filter
@ -136,14 +136,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\io_module.cpp">
RelativePath=".\modules\io_module.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\io_module.h">
RelativePath=".\modules\io_module.h">
</File>
</Filter>
</Filter>
@ -154,14 +154,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\engine.cpp">
RelativePath=".\modules\output\engine.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\engine.h">
RelativePath=".\modules\output\engine.h">
</File>
</Filter>
</Filter>
@ -172,7 +172,7 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\robot.cpp">
RelativePath=".\robot.c">
</File>
</Filter>
<Filter
@ -190,14 +190,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\sensor.cpp">
RelativePath=".\modules\input\sensor.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\sensor.h">
RelativePath=".\modules\input\sensor.h">
</File>
</Filter>
</Filter>
@ -208,14 +208,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\led.cpp">
RelativePath=".\modules\output\led.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\led.h">
RelativePath=".\modules\output\led.h">
</File>
</Filter>
</Filter>
@ -226,14 +226,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\distance_sensor.cpp">
RelativePath=".\modules\input\distance_sensor.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\distance_sensor.h">
RelativePath=".\modules\input\distance_sensor.h">
</File>
</Filter>
</Filter>
@ -244,14 +244,14 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\ir_sensor.cpp">
RelativePath=".\modules\input\ir_sensor.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<File
RelativePath=".\ir_sensor.h">
RelativePath=".\modules\input\ir_sensor.h">
</File>
</Filter>
</Filter>
@ -262,14 +262,140 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\kicker.cpp">
RelativePath=".\modules\output\kicker.c">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="">
<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>
</Filter>
</Filter>
@ -281,7 +407,7 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\atmega128io.cpp">
RelativePath=".\atmega128io.c">
</File>
</Filter>
<Filter
@ -299,7 +425,7 @@
Name="Source Files"
Filter="">
<File
RelativePath=".\tools.cpp">
RelativePath=".\tools.c">
</File>
</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
#define SPEED_PER_PWM 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
enum IOModuleNames

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

@ -3,10 +3,10 @@
//-----------------------------------------------------------------------------
void Navigator::Stop()
{
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
@ -21,10 +21,10 @@ void Navigator::Stop()
void Navigator::Rotate(float rotationSpeed)
{
this->rotationSpeed = min(rotationSpeed, 255.0f);;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
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->direction = newDirection;
this->targetAngle = newAngle;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = newSpeed;
if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI)
if(targetAngle == EMPTY_FLOAT)
{
if(rotationSpeed > 0)
{
rotationSpeed = -rotationSpeed;
}
rotationSpeed = 0;
}
else
{
if(rotationSpeed < 0)
{
rotationSpeed = -rotationSpeed;
}
rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
}
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);
bool hasDistanceToDrive = true;
bool hasOrientationToAdjust = true;
bool targetReached = false;
bool targetAngleReached = false;
//Check for distance to drive
if((targetX >= 0) != (targetY >= 0))
if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{
targetX = -1.0f;
targetY = -1.0f;
targetX = EMPTY_FLOAT;
targetY = EMPTY_FLOAT;
direction = EMPTY_FLOAT;
robotSpeed = 0;
hasDistanceToDrive = false;
}
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;
}
targetReached = true;
}
//Check for orientation to adjust
if(targetAngle >= 0)
if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
{
if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
{
hasOrientationToAdjust = false;
}
else
{
hasOrientationToAdjust = true;
}
}
else
{
if(rotationSpeed != 0)
{
hasOrientationToAdjust = true;
}
else
{
hasOrientationToAdjust = false;
}
targetAngle = EMPTY_FLOAT;
rotationSpeed = 0;
targetAngleReached = true;
}
//Calculate directional/rotational engine speed
if(hasDistanceToDrive)
if(targetReached && targetAngleReached)
{
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();
if(targetX >= 0 && targetY >= 0)
{
float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
relativeDirection = directionToTarget - locationeer->GetOrientation();
}
float xDrive = cos(relativeDirection + PI / 6.0f);
float yDrive = sin(relativeDirection + PI / 6.0f);
float xDrive = cos(relativeDirection);
float yDrive = sin(relativeDirection);
float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f;
float vBack = yDrive;
float vRight = (-yDrive - sqrt(3) * xDrive)/2;
float vLeft = xDrive;
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
float speedCorrection = 1.0f;
float maxEngineSpeed = 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 +
vBack * (vBack + vLeft + vRight));
/*float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
vBack * (vBack + vLeft + vRight));*/
if(calcSpeed != 1.0f)
{
@ -204,10 +191,10 @@ void Navigator::Update()
vBack = vBack * 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);
if(hasOrientationToAdjust)
if(rotationSpeed)
{
if(this->rotationSpeed > 0)
{
@ -252,7 +239,7 @@ void Navigator::Update()
curEngine->SetEnabled(true);
curEngine = NULL;
}
else if(hasOrientationToAdjust)
else if(rotationSpeed)
{
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(this->rotationSpeed);

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

@ -11,10 +11,11 @@ public:
{
this->parent = NULL;
this->moduleId = 0;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->correctionCountdown = CYCLES_PER_CORRECTION;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
}
@ -23,15 +24,17 @@ public:
{
this->parent = NULL;
this->moduleId = navigatorId;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->correctionCountdown = CYCLES_PER_CORRECTION;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
}
protected:
uint16 correctionCountdown;
float direction;
float targetAngle;
float targetX;
@ -54,6 +57,15 @@ public:
{
this->robotSpeed = newSpeed;
}
void CalculateDirection();
void CalculateEngines();
bool HasTarget()
{
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
}
};
#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;
break;
default:
mainDirection = -1.0f;
mainDirection = EMPTY_FLOAT;
return;
break;
}
@ -104,7 +104,7 @@ void Ball_Tracker::Update()
secondDirection = 33.0f * PI / 18.0f;
break;
default:
secondDirection = -1.0f;
secondDirection = EMPTY_FLOAT;
return;
break;
}
@ -120,6 +120,6 @@ void Ball_Tracker::Update()
}
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->moduleId = 0;
this->direction = -1.0f;
this->direction = EMPTY_FLOAT;
}
Ball_Tracker(uint32 trackerId)
{
this->parent = NULL;
this->moduleId = trackerId;
this->direction = -1.0f;
this->direction = EMPTY_FLOAT;
}
protected: