+++ improved navigator

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}">

 			<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

diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps
index bb686cb..3bc029f 100644
--- a/source/Concept/Framework/RoboCode.aps
+++ b/source/Concept/Framework/RoboCode.aps
@@ -1 +1 @@
-<AVRStudio><MANAGEMENT><ProjectName>RoboCode</ProjectName><Created>16-Feb-2007 15:16:46</Created><LastEdit>18-Feb-2007 18:45:02</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>16-Feb-2007 15:16:46</Created><Version>4</Version><Build>4, 12, 0, 462</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\RoboCode.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>Y:\Concept\Framework\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET></CURRENT_TARGET><CURRENT_PART></CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM></COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>modules\io_module.c</SOURCEFILE><SOURCEFILE>atmega128io.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>robot.c</SOURCEFILE><SOURCEFILE>tools.c</SOURCEFILE><SOURCEFILE>modules\input\distance_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\ir_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\keyboard.c</SOURCEFILE><SOURCEFILE>modules\input\mouse_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\sensor.c</SOURCEFILE><SOURCEFILE>modules\interpreter\position_tracker.c</SOURCEFILE><SOURCEFILE>modules\output\display.c</SOURCEFILE><SOURCEFILE>modules\output\dribbler.c</SOURCEFILE><SOURCEFILE>modules\output\engine.c</SOURCEFILE><SOURCEFILE>modules\output\kicker.c</SOURCEFILE><SOURCEFILE>modules\output\led.c</SOURCEFILE><SOURCEFILE>modules\interpreter\ball_tracker.c</SOURCEFILE><SOURCEFILE>modules\executor\navigator.c</SOURCEFILE><HEADERFILE>stdafx.h</HEADERFILE><HEADERFILE>tools.h</HEADERFILE><HEADERFILE>atmega128io.h</HEADERFILE><HEADERFILE>defines.h</HEADERFILE><HEADERFILE>robot.h</HEADERFILE><HEADERFILE>modules\input\distance_sensor.h</HEADERFILE><HEADERFILE>modules\input\ir_sensor.h</HEADERFILE><HEADERFILE>modules\input\keyboard.h</HEADERFILE><HEADERFILE>modules\input\mouse_sensor.h</HEADERFILE><HEADERFILE>modules\input\sensor.h</HEADERFILE><HEADERFILE>modules\output\display.h</HEADERFILE><HEADERFILE>modules\output\dribbler.h</HEADERFILE><HEADERFILE>modules\output\engine.h</HEADERFILE><HEADERFILE>modules\output\kicker.h</HEADERFILE><HEADERFILE>modules\output\led.h</HEADERFILE><HEADERFILE>modules\interpreter\position_tracker.h</HEADERFILE><HEADERFILE>modules\io_module.h</HEADERFILE><HEADERFILE>modules\interpreter\ball_tracker.h</HEADERFILE><HEADERFILE>modules\executor\navigator.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega128</PART><HEX>1</HEX><LIST>0</LIST><MAP>0</MAP><OUTPUTFILENAME>RoboCode.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</ISDIRTY><OPTIONS><OPTION><FILE>atmega128io.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\executor\navigator.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\distance_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\ir_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\keyboard.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\mouse_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\interpreter\ball_tracker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\interpreter\position_tracker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\io_module.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\display.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\dribbler.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\engine.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\kicker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\led.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>robot.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>tools.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS><INCLUDE>modules\</INCLUDE><INCLUDE>modules\executor\</INCLUDE><INCLUDE>modules\input\</INCLUDE><INCLUDE>modules\interpreter\</INCLUDE><INCLUDE>modules\logic\</INCLUDE><INCLUDE>modules\output\</INCLUDE></INCDIRS><LIBDIRS/><LIBS><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char </OPTIONSFORALL><LINKEROPTIONS>-lm </LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>0</USES_WINAVR><GCC_LOC>C:\WinAVR\bin\avr-g++.exe</GCC_LOC><MAKE_LOC>C:\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>Y:\Concept\Framework\stdafx.h</Name><Name>Y:\Concept\Framework\tools.h</Name><Name>Y:\Concept\Framework\atmega128io.h</Name><Name>Y:\Concept\Framework\defines.h</Name><Name>Y:\Concept\Framework\robot.h</Name><Name>Y:\Concept\Framework\modules\input\distance_sensor.h</Name><Name>Y:\Concept\Framework\modules\input\ir_sensor.h</Name><Name>Y:\Concept\Framework\modules\input\keyboard.h</Name><Name>Y:\Concept\Framework\modules\input\mouse_sensor.h</Name><Name>Y:\Concept\Framework\modules\input\sensor.h</Name><Name>Y:\Concept\Framework\modules\output\display.h</Name><Name>Y:\Concept\Framework\modules\output\dribbler.h</Name><Name>Y:\Concept\Framework\modules\output\engine.h</Name><Name>Y:\Concept\Framework\modules\output\kicker.h</Name><Name>Y:\Concept\Framework\modules\output\led.h</Name><Name>Y:\Concept\Framework\modules\interpreter\position_tracker.h</Name><Name>Y:\Concept\Framework\modules\io_module.h</Name><Name>Y:\Concept\Framework\modules\interpreter\ball_tracker.h</Name><Name>Y:\Concept\Framework\modules\executor\navigator.h</Name><Name>Y:\Concept\Framework\modules\io_module.c</Name><Name>Y:\Concept\Framework\atmega128io.c</Name><Name>Y:\Concept\Framework\main.c</Name><Name>Y:\Concept\Framework\robot.c</Name><Name>Y:\Concept\Framework\tools.c</Name><Name>Y:\Concept\Framework\modules\input\distance_sensor.c</Name><Name>Y:\Concept\Framework\modules\input\ir_sensor.c</Name><Name>Y:\Concept\Framework\modules\input\keyboard.c</Name><Name>Y:\Concept\Framework\modules\input\mouse_sensor.c</Name><Name>Y:\Concept\Framework\modules\input\sensor.c</Name><Name>Y:\Concept\Framework\modules\interpreter\position_tracker.c</Name><Name>Y:\Concept\Framework\modules\output\display.c</Name><Name>Y:\Concept\Framework\modules\output\dribbler.c</Name><Name>Y:\Concept\Framework\modules\output\engine.c</Name><Name>Y:\Concept\Framework\modules\output\kicker.c</Name><Name>Y:\Concept\Framework\modules\output\led.c</Name><Name>Y:\Concept\Framework\modules\interpreter\ball_tracker.c</Name><Name>Y:\Concept\Framework\modules\executor\navigator.c</Name></Files></ProjectFiles><Files><File00000><FileId>00000</FileId><FileName>stdafx.h</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>defines.h</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>robot.h</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>atmega128io.h</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>tools.h</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>modules\interpreter\position_tracker.h</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>modules\io_module.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>modules\output\display.h</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>modules\input\ir_sensor.h</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>modules\output\engine.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>modules\input\sensor.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>modules\input\keyboard.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>modules\io_module.c</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>robot.c</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>modules\input\ir_sensor.c</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>modules\output\dribbler.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>modules\output\kicker.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>modules\output\led.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>modules\input\distance_sensor.h</FileName><Status>1</Status></File00018><File00019><FileId>00019</FileId><FileName>modules\input\mouse_sensor.h</FileName><Status>1</Status></File00019><File00020><FileId>00020</FileId><FileName>modules\input\sensor.c</FileName><Status>1</Status></File00020><File00021><FileId>00021</FileId><FileName>main.c</FileName><Status>1</Status></File00021><File00022><FileId>00022</FileId><FileName>modules\interpreter\ball_tracker.h</FileName><Status>1</Status></File00022><File00023><FileId>00023</FileId><FileName>modules\interpreter\ball_tracker.c</FileName><Status>1</Status></File00023><File00024><FileId>00024</FileId><FileName>modules\interpreter\position_tracker.c</FileName><Status>1</Status></File00024><File00025><FileId>00025</FileId><FileName>modules\input\distance_sensor.c</FileName><Status>1</Status></File00025><File00026><FileId>00026</FileId><FileName>modules\output\engine.c</FileName><Status>1</Status></File00026><File00027><FileId>00027</FileId><FileName>modules\output\dribbler.c</FileName><Status>1</Status></File00027><File00028><FileId>00028</FileId><FileName>modules\executor\navigator.h</FileName><Status>1</Status></File00028><File00029><FileId>00029</FileId><FileName>modules\executor\navigator.c</FileName><Status>1</Status></File00029></Files><Workspace><File00000><Position>299 73 1025 554</Position><LineCol>6 0</LineCol></File00000><File00001><Position>33064 32865 33664 33111</Position><LineCol>155 0</LineCol></File00001><File00002><Position>33064 32865 33664 33111</Position><LineCol>45 0</LineCol></File00002><File00003><Position>33064 32865 33664 33111</Position><LineCol>16 0</LineCol></File00003><File00004><Position>33064 32865 33664 33111</Position><LineCol>11 0</LineCol></File00004><File00005><Position>33064 32865 33664 33111</Position><LineCol>5 0</LineCol></File00005><File00006><Position>32293 32095 33019 32550</Position><LineCol>8 0</LineCol></File00006><File00007><Position>33064 32865 33790 33320</Position><LineCol>102 0</LineCol></File00007><File00008><Position>33064 32865 33664 33111</Position><LineCol>2 0</LineCol></File00008><File00009><Position>32390 32237 33016 32538</Position><LineCol>55 0</LineCol></File00009><File00010><Position>32302 32121 32932 32426</Position><LineCol>27 0</LineCol></File00010><File00011><Position>32327 32152 32957 32457</Position><LineCol>6 0</LineCol></File00011><File00012><Position>32349 32181 32979 32486</Position><LineCol>1 0</LineCol></File00012><File00013><Position>32371 32210 33001 32515</Position><LineCol>17 0</LineCol></File00013><File00014><Position>32393 32239 33023 32544</Position><LineCol>9 0</LineCol></File00014><File00015><Position>32305 32123 32935 32428</Position><LineCol>3 0</LineCol></File00015><File00016><Position>32327 32152 32957 32457</Position><LineCol>18 0</LineCol></File00016><File00017><Position>32349 32181 32979 32486</Position><LineCol>1 0</LineCol></File00017><File00018><Position>32371 32210 33001 32515</Position><LineCol>71 0</LineCol></File00018><File00019><Position>32393 32239 33023 32544</Position><LineCol>16 0</LineCol></File00019><File00020><Position>32305 32123 32935 32428</Position><LineCol>1 0</LineCol></File00020><File00021><Position>299 72 1025 527</Position><LineCol>177 4</LineCol><State>Maximized</State></File00021><File00022><Position>32349 32181 32979 32486</Position><LineCol>3 0</LineCol></File00022><File00023><Position>293 68 1019 523</Position><LineCol>52 0</LineCol></File00023><File00024><Position>32393 32239 33023 32544</Position><LineCol>7 0</LineCol></File00024><File00025><Position>32305 32123 32935 32428</Position><LineCol>1 0</LineCol></File00025><File00026><Position>32327 32152 32957 32457</Position><LineCol>1 0</LineCol></File00026><File00027><Position>32349 32181 32979 32486</Position><LineCol>1 0</LineCol></File00027><File00028><Position>32371 32210 33001 32515</Position><LineCol>1 0</LineCol></File00028><File00029><Position>32393 32239 33023 32544</Position><LineCol>241 3</LineCol></File00029></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>

+<AVRStudio><MANAGEMENT><ProjectName>RoboCode</ProjectName><Created>16-Feb-2007 15:16:46</Created><LastEdit>18-Feb-2007 22:21:14</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>16-Feb-2007 15:16:46</Created><Version>4</Version><Build>4, 12, 0, 462</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\RoboCode.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET></CURRENT_TARGET><CURRENT_PART></CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM></COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>modules\io_module.c</SOURCEFILE><SOURCEFILE>atmega128io.c</SOURCEFILE><SOURCEFILE>main.c</SOURCEFILE><SOURCEFILE>robot.c</SOURCEFILE><SOURCEFILE>tools.c</SOURCEFILE><SOURCEFILE>modules\input\distance_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\ir_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\keyboard.c</SOURCEFILE><SOURCEFILE>modules\input\mouse_sensor.c</SOURCEFILE><SOURCEFILE>modules\input\sensor.c</SOURCEFILE><SOURCEFILE>modules\interpreter\position_tracker.c</SOURCEFILE><SOURCEFILE>modules\output\display.c</SOURCEFILE><SOURCEFILE>modules\output\dribbler.c</SOURCEFILE><SOURCEFILE>modules\output\engine.c</SOURCEFILE><SOURCEFILE>modules\output\kicker.c</SOURCEFILE><SOURCEFILE>modules\output\led.c</SOURCEFILE><SOURCEFILE>modules\interpreter\ball_tracker.c</SOURCEFILE><SOURCEFILE>modules\executor\navigator.c</SOURCEFILE><HEADERFILE>stdafx.h</HEADERFILE><HEADERFILE>tools.h</HEADERFILE><HEADERFILE>atmega128io.h</HEADERFILE><HEADERFILE>defines.h</HEADERFILE><HEADERFILE>robot.h</HEADERFILE><HEADERFILE>modules\input\distance_sensor.h</HEADERFILE><HEADERFILE>modules\input\ir_sensor.h</HEADERFILE><HEADERFILE>modules\input\keyboard.h</HEADERFILE><HEADERFILE>modules\input\mouse_sensor.h</HEADERFILE><HEADERFILE>modules\input\sensor.h</HEADERFILE><HEADERFILE>modules\output\display.h</HEADERFILE><HEADERFILE>modules\output\dribbler.h</HEADERFILE><HEADERFILE>modules\output\engine.h</HEADERFILE><HEADERFILE>modules\output\kicker.h</HEADERFILE><HEADERFILE>modules\output\led.h</HEADERFILE><HEADERFILE>modules\interpreter\position_tracker.h</HEADERFILE><HEADERFILE>modules\io_module.h</HEADERFILE><HEADERFILE>modules\interpreter\ball_tracker.h</HEADERFILE><HEADERFILE>modules\executor\navigator.h</HEADERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega128</PART><HEX>1</HEX><LIST>0</LIST><MAP>0</MAP><OUTPUTFILENAME>RoboCode.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</ISDIRTY><OPTIONS><OPTION><FILE>atmega128io.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\executor\navigator.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\distance_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\ir_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\keyboard.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\mouse_sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\input\sensor.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\interpreter\ball_tracker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\interpreter\position_tracker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\io_module.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\display.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\dribbler.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\engine.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\kicker.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>modules\output\led.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>robot.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>tools.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS><INCLUDE>modules\</INCLUDE><INCLUDE>modules\executor\</INCLUDE><INCLUDE>modules\input\</INCLUDE><INCLUDE>modules\interpreter\</INCLUDE><INCLUDE>modules\logic\</INCLUDE><INCLUDE>modules\output\</INCLUDE></INCDIRS><LIBDIRS/><LIBS><LIB>libm.a</LIB></LIBS><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS>-lm</LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>0</USES_WINAVR><GCC_LOC>C:\WinAVR\bin\avr-g++.exe</GCC_LOC><MAKE_LOC>C:\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\stdafx.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\defines.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.h</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\io_module.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\atmega128io.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\main.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\robot.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\tools.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\distance_sensor.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\ir_sensor.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\keyboard.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\mouse_sensor.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\input\sensor.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\position_tracker.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\display.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\dribbler.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\engine.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\kicker.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\output\led.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\interpreter\ball_tracker.c</Name><Name>I:\Projects\RoboCup WM 2006 07\trunk\rc2007-soccer\source\Concept\Framework\modules\executor\navigator.c</Name></Files></ProjectFiles><Files><File00000><FileId>00000</FileId><FileName>stdafx.h</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>defines.h</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>robot.h</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>atmega128io.h</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>tools.h</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>modules\interpreter\position_tracker.h</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>modules\io_module.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>modules\output\display.h</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>modules\input\ir_sensor.h</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>modules\output\engine.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>modules\input\sensor.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>modules\input\keyboard.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>modules\io_module.c</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>robot.c</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>modules\input\ir_sensor.c</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>modules\output\dribbler.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>modules\output\kicker.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>modules\output\led.h</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>modules\input\distance_sensor.h</FileName><Status>1</Status></File00018><File00019><FileId>00019</FileId><FileName>modules\input\mouse_sensor.h</FileName><Status>1</Status></File00019><File00020><FileId>00020</FileId><FileName>modules\input\sensor.c</FileName><Status>1</Status></File00020><File00021><FileId>00021</FileId><FileName>main.c</FileName><Status>1</Status></File00021><File00022><FileId>00022</FileId><FileName>modules\interpreter\ball_tracker.h</FileName><Status>1</Status></File00022><File00023><FileId>00023</FileId><FileName>modules\interpreter\ball_tracker.c</FileName><Status>1</Status></File00023><File00024><FileId>00024</FileId><FileName>modules\interpreter\position_tracker.c</FileName><Status>1</Status></File00024><File00025><FileId>00025</FileId><FileName>modules\input\distance_sensor.c</FileName><Status>1</Status></File00025><File00026><FileId>00026</FileId><FileName>modules\output\engine.c</FileName><Status>1</Status></File00026><File00027><FileId>00027</FileId><FileName>modules\output\dribbler.c</FileName><Status>1</Status></File00027><File00028><FileId>00028</FileId><FileName>modules\executor\navigator.h</FileName><Status>1</Status></File00028><File00029><FileId>00029</FileId><FileName>modules\executor\navigator.c</FileName><Status>1</Status></File00029></Files><Workspace><File00000><Position>232 74 958 555</Position><LineCol>6 0</LineCol></File00000><File00001><Position>267 72 1025 459</Position><LineCol>60 34</LineCol><State>Maximized</State></File00001><File00002><Position>32994 32862 33594 33108</Position><LineCol>45 0</LineCol></File00002><File00003><Position>32994 32862 33594 33108</Position><LineCol>16 0</LineCol></File00003><File00004><Position>32994 32862 33594 33108</Position><LineCol>11 0</LineCol></File00004><File00005><Position>32994 32862 33594 33108</Position><LineCol>5 0</LineCol></File00005><File00006><Position>32223 32093 32949 32548</Position><LineCol>8 0</LineCol></File00006><File00007><Position>32994 32862 33720 33317</Position><LineCol>102 0</LineCol></File00007><File00008><Position>32994 32862 33594 33108</Position><LineCol>2 0</LineCol></File00008><File00009><Position>32320 32235 32946 32536</Position><LineCol>55 0</LineCol></File00009><File00010><Position>32232 32119 32862 32424</Position><LineCol>27 0</LineCol></File00010><File00011><Position>32257 32150 32887 32455</Position><LineCol>6 0</LineCol></File00011><File00012><Position>32279 32179 32909 32484</Position><LineCol>1 0</LineCol></File00012><File00013><Position>32301 32208 32931 32513</Position><LineCol>17 0</LineCol></File00013><File00014><Position>32323 32237 32953 32542</Position><LineCol>9 0</LineCol></File00014><File00015><Position>32235 32121 32865 32426</Position><LineCol>3 0</LineCol></File00015><File00016><Position>32257 32150 32887 32455</Position><LineCol>18 0</LineCol></File00016><File00017><Position>32279 32179 32909 32484</Position><LineCol>1 0</LineCol></File00017><File00018><Position>32301 32208 32931 32513</Position><LineCol>71 0</LineCol></File00018><File00019><Position>32323 32237 32953 32542</Position><LineCol>16 0</LineCol></File00019><File00020><Position>32235 32121 32865 32426</Position><LineCol>1 0</LineCol></File00020><File00021><Position>229 70 955 525</Position><LineCol>177 0</LineCol></File00021><File00022><Position>32279 32179 32909 32484</Position><LineCol>3 0</LineCol></File00022><File00023><Position>223 66 949 521</Position><LineCol>52 0</LineCol></File00023><File00024><Position>32323 32237 32953 32542</Position><LineCol>7 0</LineCol></File00024><File00025><Position>32235 32121 32865 32426</Position><LineCol>1 0</LineCol></File00025><File00026><Position>32257 32150 32887 32455</Position><LineCol>1 0</LineCol></File00026><File00027><Position>32279 32179 32909 32484</Position><LineCol>1 0</LineCol></File00027><File00028><Position>32301 32208 32931 32513</Position><LineCol>1 0</LineCol></File00028><File00029><Position>32323 32237 32953 32542</Position><LineCol>214 5</LineCol></File00029></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>

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

+		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<Position_Tracker>(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<Engine>(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: