From 354ef5f0d0ad5487288e8d25fc9b1fcecf730eba Mon Sep 17 00:00:00 2001 From: masterm Date: Mon, 19 Feb 2007 17:15:02 +0000 Subject: +++ improved navigator --- source/Concept/Framework/Concept.vcproj | 162 ++++++++++++++++++--- source/Concept/Framework/RoboCode.aps | 2 +- source/Concept/Framework/defines.h | 5 +- .../Concept/Framework/modules/executor/navigator.c | 159 ++++++++++---------- .../Concept/Framework/modules/executor/navigator.h | 28 +++- .../Framework/modules/interpreter/ball_tracker.c | 6 +- .../Framework/modules/interpreter/ball_tracker.h | 4 +- 7 files changed, 247 insertions(+), 119 deletions(-) diff --git a/source/Concept/Framework/Concept.vcproj b/source/Concept/Framework/Concept.vcproj index 1348191..5de195f 100644 --- a/source/Concept/Framework/Concept.vcproj +++ b/source/Concept/Framework/Concept.vcproj @@ -107,7 +107,7 @@ Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx" UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"> + RelativePath=".\main.c"> + RelativePath=".\modules\io_module.c"> + RelativePath=".\modules\io_module.h"> @@ -154,14 +154,14 @@ Name="Source Files" Filter=""> + RelativePath=".\modules\output\engine.c"> + RelativePath=".\modules\output\engine.h"> @@ -172,7 +172,7 @@ Name="Source Files" Filter=""> + RelativePath=".\robot.c"> + RelativePath=".\modules\input\sensor.c"> + RelativePath=".\modules\input\sensor.h"> @@ -208,14 +208,14 @@ Name="Source Files" Filter=""> + RelativePath=".\modules\output\led.c"> + RelativePath=".\modules\output\led.h"> @@ -226,14 +226,14 @@ Name="Source Files" Filter=""> + RelativePath=".\modules\input\distance_sensor.c"> + RelativePath=".\modules\input\distance_sensor.h"> @@ -244,14 +244,14 @@ Name="Source Files" Filter=""> + RelativePath=".\modules\input\ir_sensor.c"> + RelativePath=".\modules\input\ir_sensor.h"> @@ -262,14 +262,140 @@ Name="Source Files" Filter=""> + RelativePath=".\modules\output\kicker.c"> + RelativePath=".\modules\output\kicker.h"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -281,7 +407,7 @@ Name="Source Files" Filter=""> + RelativePath=".\atmega128io.c"> + RelativePath=".\tools.c"> RoboCode16-Feb-2007 15:16:4618-Feb-2007 18:45:02241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfY:\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000modules\io_module.catmega128io.cmain.crobot.ctools.cmodules\input\distance_sensor.cmodules\input\ir_sensor.cmodules\input\keyboard.cmodules\input\mouse_sensor.cmodules\input\sensor.cmodules\interpreter\position_tracker.cmodules\output\display.cmodules\output\dribbler.cmodules\output\engine.cmodules\output\kicker.cmodules\output\led.cmodules\interpreter\ball_tracker.cmodules\executor\navigator.cstdafx.htools.hatmega128io.hdefines.hrobot.hmodules\input\distance_sensor.hmodules\input\ir_sensor.hmodules\input\keyboard.hmodules\input\mouse_sensor.hmodules\input\sensor.hmodules\output\display.hmodules\output\dribbler.hmodules\output\engine.hmodules\output\kicker.hmodules\output\led.hmodules\interpreter\position_tracker.hmodules\io_module.hmodules\interpreter\ball_tracker.hmodules\executor\navigator.hdefaultNOatmega128100RoboCode.elfdefault\0modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\libm.a-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char -lm default0C:\WinAVR\bin\avr-g++.exeC:\WinAVR\utils\bin\make.exeY:\Concept\Framework\stdafx.hY:\Concept\Framework\tools.hY:\Concept\Framework\atmega128io.hY:\Concept\Framework\defines.hY:\Concept\Framework\robot.hY:\Concept\Framework\modules\input\distance_sensor.hY:\Concept\Framework\modules\input\ir_sensor.hY:\Concept\Framework\modules\input\keyboard.hY:\Concept\Framework\modules\input\mouse_sensor.hY:\Concept\Framework\modules\input\sensor.hY:\Concept\Framework\modules\output\display.hY:\Concept\Framework\modules\output\dribbler.hY:\Concept\Framework\modules\output\engine.hY:\Concept\Framework\modules\output\kicker.hY:\Concept\Framework\modules\output\led.hY:\Concept\Framework\modules\interpreter\position_tracker.hY:\Concept\Framework\modules\io_module.hY:\Concept\Framework\modules\interpreter\ball_tracker.hY:\Concept\Framework\modules\executor\navigator.hY:\Concept\Framework\modules\io_module.cY:\Concept\Framework\atmega128io.cY:\Concept\Framework\main.cY:\Concept\Framework\robot.cY:\Concept\Framework\tools.cY:\Concept\Framework\modules\input\distance_sensor.cY:\Concept\Framework\modules\input\ir_sensor.cY:\Concept\Framework\modules\input\keyboard.cY:\Concept\Framework\modules\input\mouse_sensor.cY:\Concept\Framework\modules\input\sensor.cY:\Concept\Framework\modules\interpreter\position_tracker.cY:\Concept\Framework\modules\output\display.cY:\Concept\Framework\modules\output\dribbler.cY:\Concept\Framework\modules\output\engine.cY:\Concept\Framework\modules\output\kicker.cY:\Concept\Framework\modules\output\led.cY:\Concept\Framework\modules\interpreter\ball_tracker.cY:\Concept\Framework\modules\executor\navigator.c00000stdafx.h100001defines.h100002robot.h100003atmega128io.h100004tools.h100005modules\interpreter\position_tracker.h100006modules\io_module.h100007modules\output\display.h100008modules\input\ir_sensor.h100009modules\output\engine.h100010modules\input\sensor.h100011modules\input\keyboard.h100012modules\io_module.c100013robot.c100014modules\input\ir_sensor.c100015modules\output\dribbler.h100016modules\output\kicker.h100017modules\output\led.h100018modules\input\distance_sensor.h100019modules\input\mouse_sensor.h100020modules\input\sensor.c100021main.c100022modules\interpreter\ball_tracker.h100023modules\interpreter\ball_tracker.c100024modules\interpreter\position_tracker.c100025modules\input\distance_sensor.c100026modules\output\engine.c100027modules\output\dribbler.c100028modules\executor\navigator.h100029modules\executor\navigator.c1299 73 1025 5546 033064 32865 33664 33111155 033064 32865 33664 3311145 033064 32865 33664 3311116 033064 32865 33664 3311111 033064 32865 33664 331115 032293 32095 33019 325508 033064 32865 33790 33320102 033064 32865 33664 331112 032390 32237 33016 3253855 032302 32121 32932 3242627 032327 32152 32957 324576 032349 32181 32979 324861 032371 32210 33001 3251517 032393 32239 33023 325449 032305 32123 32935 324283 032327 32152 32957 3245718 032349 32181 32979 324861 032371 32210 33001 3251571 032393 32239 33023 3254416 032305 32123 32935 324281 0299 72 1025 527177 4Maximized32349 32181 32979 324863 0293 68 1019 52352 032393 32239 33023 325447 032305 32123 32935 324281 032327 32152 32957 324571 032349 32181 32979 324861 032371 32210 33001 325151 032393 32239 33023 32544241 3 +RoboCode16-Feb-2007 15:16:4618-Feb-2007 22:21:14241016-Feb-2007 15:16:4644, 12, 0, 462AVR GCCdefault\RoboCode.elfI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\falseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31000modules\io_module.catmega128io.cmain.crobot.ctools.cmodules\input\distance_sensor.cmodules\input\ir_sensor.cmodules\input\keyboard.cmodules\input\mouse_sensor.cmodules\input\sensor.cmodules\interpreter\position_tracker.cmodules\output\display.cmodules\output\dribbler.cmodules\output\engine.cmodules\output\kicker.cmodules\output\led.cmodules\interpreter\ball_tracker.cmodules\executor\navigator.cstdafx.htools.hatmega128io.hdefines.hrobot.hmodules\input\distance_sensor.hmodules\input\ir_sensor.hmodules\input\keyboard.hmodules\input\mouse_sensor.hmodules\input\sensor.hmodules\output\display.hmodules\output\dribbler.hmodules\output\engine.hmodules\output\kicker.hmodules\output\led.hmodules\interpreter\position_tracker.hmodules\io_module.hmodules\interpreter\ball_tracker.hmodules\executor\navigator.hdefaultNOatmega128100RoboCode.elfdefault\0modules\modules\executor\modules\input\modules\interpreter\modules\logic\modules\output\libm.a-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char-lmdefault0C:\WinAVR\bin\avr-g++.exeC:\WinAVR\utils\bin\make.exeI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\stdafx.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\defines.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.hI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\main.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.cI:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.c00000stdafx.h100001defines.h100002robot.h100003atmega128io.h100004tools.h100005modules\interpreter\position_tracker.h100006modules\io_module.h100007modules\output\display.h100008modules\input\ir_sensor.h100009modules\output\engine.h100010modules\input\sensor.h100011modules\input\keyboard.h100012modules\io_module.c100013robot.c100014modules\input\ir_sensor.c100015modules\output\dribbler.h100016modules\output\kicker.h100017modules\output\led.h100018modules\input\distance_sensor.h100019modules\input\mouse_sensor.h100020modules\input\sensor.c100021main.c100022modules\interpreter\ball_tracker.h100023modules\interpreter\ball_tracker.c100024modules\interpreter\position_tracker.c100025modules\input\distance_sensor.c100026modules\output\engine.c100027modules\output\dribbler.c100028modules\executor\navigator.h100029modules\executor\navigator.c1232 74 958 5556 0267 72 1025 45960 34Maximized32994 32862 33594 3310845 032994 32862 33594 3310816 032994 32862 33594 3310811 032994 32862 33594 331085 032223 32093 32949 325488 032994 32862 33720 33317102 032994 32862 33594 331082 032320 32235 32946 3253655 032232 32119 32862 3242427 032257 32150 32887 324556 032279 32179 32909 324841 032301 32208 32931 3251317 032323 32237 32953 325429 032235 32121 32865 324263 032257 32150 32887 3245518 032279 32179 32909 324841 032301 32208 32931 3251371 032323 32237 32953 3254216 032235 32121 32865 324261 0229 70 955 525177 032279 32179 32909 324843 0223 66 949 52152 032323 32237 32953 325427 032235 32121 32865 324261 032257 32150 32887 324551 032279 32179 32909 324841 032301 32208 32931 325131 032323 32237 32953 32542214 5 diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h index 73505e2..1cec0f3 100644 --- a/source/Concept/Framework/defines.h +++ b/source/Concept/Framework/defines.h @@ -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 diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 691b1d0..ceeda60 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -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(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(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(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; + targetReached = true; } - else if(targetX >= 0 && targetY >= 0) + + if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) { - if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) - { - targetX = -1.0f; - targetY = -1.0f; + targetAngle = EMPTY_FLOAT; + rotationSpeed = 0; - hasDistanceToDrive = false; - } - else - { - hasDistanceToDrive = true; - } + targetAngleReached = true; } - else + + if(targetReached && targetAngleReached) { - if(direction >= 0) - { - hasDistanceToDrive = true; - } - else - { - hasDistanceToDrive = false; - } + Stop(); + } + else if(targetReached || targetAngleReached) + { + CalculateDirection(); } - //Check for orientation to adjust - if(targetAngle >= 0) + if(!(correctionCountdown--)) { - if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) - { - hasOrientationToAdjust = false; - } - else - { - hasOrientationToAdjust = true; - } + CalculateDirection(); } - else +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateDirection() +{ + correctionCountdown = CYCLES_PER_CORRECTION; + + if(targetAngle == EMPTY_FLOAT && !HasTarget()) return; + + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + if(HasTarget()) { - if(rotationSpeed != 0) - { - hasOrientationToAdjust = true; - } - else - { - hasOrientationToAdjust = false; - } + direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); } - //Calculate directional/rotational engine speed - if(hasDistanceToDrive) + if(targetAngle != EMPTY_FLOAT) { - float relativeDirection = this->direction - locationeer->GetOrientation(); + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation())); + } - if(targetX >= 0 && targetY >= 0) - { - float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); - relativeDirection = directionToTarget - locationeer->GetOrientation(); - } + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateEngines() +{ + if(direction != EMPTY_FLOAT) + { + float relativeDirection = this->direction - locationeer->GetOrientation(); - float xDrive = cos(relativeDirection); - float yDrive = sin(relativeDirection); + float xDrive = cos(relativeDirection + PI / 6.0f); + float yDrive = sin(relativeDirection + PI / 6.0f); - 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(IO_ENGINE_DRIVE_LEFT); curEngine->SetSpeed(this->rotationSpeed); diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 8da52f6..56e7b8f 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -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 diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 6679d3e..1701121 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -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; } } diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index ed8801f..bea4a19 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -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: -- cgit v1.2.3