+++ Codework, Navigator&Positiontracker done (buggy?)

9 files changed, 168 insertions, 46 deletions

diff --git a/Information/Hardware/Portbelegung.ods b/Information/Hardware/Portbelegung.ods
index 13e4bae..397a912 100644
--- a/Information/Hardware/Portbelegung.ods
+++ b/Information/Hardware/Portbelegung.ods
diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps
index 0cae013..83aaae6 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>19-Feb-2007 18:25:08</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><File00030><FileId>00030</FileId><FileName>modules\output\display.c</FileName><Status>1</Status></File00030><File00031><FileId>00031</FileId><FileName>modules\input\mouse_sensor.c</FileName><Status>1</Status></File00031></Files><Workspace><File00000><Position>299 75 1025 556</Position><LineCol>6 0</LineCol></File00000><File00001><Position>33058 32861 33658 33107</Position><LineCol>155 0</LineCol></File00001><File00002><Position>33058 32861 33658 33107</Position><LineCol>45 0</LineCol></File00002><File00003><Position>33058 32861 33658 33107</Position><LineCol>16 0</LineCol></File00003><File00004><Position>33058 32861 33658 33107</Position><LineCol>11 0</LineCol></File00004><File00005><Position>33058 32861 33658 33107</Position><LineCol>43 0</LineCol></File00005><File00006><Position>32287 32091 33013 32546</Position><LineCol>8 0</LineCol></File00006><File00007><Position>33058 32861 33784 33316</Position><LineCol>73 0</LineCol></File00007><File00008><Position>33058 32861 33658 33107</Position><LineCol>2 0</LineCol></File00008><File00009><Position>32384 32233 33010 32534</Position><LineCol>109 0</LineCol></File00009><File00010><Position>32296 32117 32926 32422</Position><LineCol>27 0</LineCol></File00010><File00011><Position>32321 32148 32951 32453</Position><LineCol>6 0</LineCol></File00011><File00012><Position>32343 32177 32973 32482</Position><LineCol>1 0</LineCol></File00012><File00013><Position>32365 32206 32995 32511</Position><LineCol>16 71</LineCol></File00013><File00014><Position>32387 32235 33017 32540</Position><LineCol>9 0</LineCol></File00014><File00015><Position>32299 32119 32929 32424</Position><LineCol>3 0</LineCol></File00015><File00016><Position>32321 32148 32951 32453</Position><LineCol>21 0</LineCol></File00016><File00017><Position>32343 32177 32973 32482</Position><LineCol>1 0</LineCol></File00017><File00018><Position>32365 32206 32995 32511</Position><LineCol>71 0</LineCol></File00018><File00019><Position>299 72 1025 527</Position><LineCol>60 30</LineCol><State>Maximized</State></File00019><File00020><Position>32299 32119 32929 32424</Position><LineCol>1 0</LineCol></File00020><File00021><Position>293 68 1019 523</Position><LineCol>168 19</LineCol></File00021><File00022><Position>32343 32177 32973 32482</Position><LineCol>3 0</LineCol></File00022><File00023><Position>287 64 1013 519</Position><LineCol>65 0</LineCol></File00023><File00024><Position>32387 32235 33017 32540</Position><LineCol>7 0</LineCol></File00024><File00025><Position>32299 32119 32929 32424</Position><LineCol>1 0</LineCol></File00025><File00026><Position>32321 32148 32951 32453</Position><LineCol>1 0</LineCol></File00026><File00027><Position>32343 32177 32973 32482</Position><LineCol>1 0</LineCol></File00027><File00028><Position>32365 32206 32995 32511</Position><LineCol>39 0</LineCol></File00028><File00029><Position>296 70 1022 525</Position><LineCol>156 20</LineCol></File00029><File00030><Position>300 100 930 405</Position><LineCol>0 0</LineCol></File00030><File00031><Position>322 129 952 434</Position><LineCol>0 0</LineCol></File00031></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>

+<AVRStudio><MANAGEMENT><ProjectName>RoboCode</ProjectName><Created>16-Feb-2007 15:16:46</Created><LastEdit>20-Feb-2007 21:59:37</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><File00030><FileId>00030</FileId><FileName>modules\output\display.c</FileName><Status>1</Status></File00030><File00031><FileId>00031</FileId><FileName>modules\input\mouse_sensor.c</FileName><Status>1</Status></File00031><File00032><FileId>00032</FileId><FileName>c:\WinAVR\avr\include\math.h</FileName><Status>1</Status></File00032></Files><Workspace><File00000><Position>291 76 1017 557</Position><LineCol>6 0</LineCol></File00000><File00001><Position>291 72 1025 553</Position><LineCol>53 14</LineCol><State>Maximized</State></File00001><File00002><Position>33047 32859 33647 33105</Position><LineCol>45 0</LineCol></File00002><File00003><Position>33047 32859 33647 33105</Position><LineCol>16 0</LineCol></File00003><File00004><Position>33047 32859 33647 33105</Position><LineCol>16 22</LineCol></File00004><File00005><Position>33047 32859 33647 33105</Position><LineCol>14 0</LineCol></File00005><File00006><Position>32276 32089 33002 32544</Position><LineCol>8 0</LineCol></File00006><File00007><Position>33047 32859 33773 33314</Position><LineCol>107 69</LineCol></File00007><File00008><Position>33047 32859 33647 33105</Position><LineCol>2 0</LineCol></File00008><File00009><Position>32373 32231 32999 32532</Position><LineCol>101 30</LineCol></File00009><File00010><Position>32285 32115 32915 32420</Position><LineCol>27 0</LineCol></File00010><File00011><Position>32310 32146 32940 32451</Position><LineCol>6 0</LineCol></File00011><File00012><Position>32332 32175 32962 32480</Position><LineCol>1 0</LineCol></File00012><File00013><Position>32354 32204 32984 32509</Position><LineCol>93 0</LineCol></File00013><File00014><Position>32376 32233 33006 32538</Position><LineCol>9 0</LineCol></File00014><File00015><Position>32288 32117 32918 32422</Position><LineCol>3 0</LineCol></File00015><File00016><Position>32310 32146 32940 32451</Position><LineCol>21 0</LineCol></File00016><File00017><Position>32332 32175 32962 32480</Position><LineCol>1 0</LineCol></File00017><File00018><Position>32354 32204 32984 32509</Position><LineCol>71 0</LineCol></File00018><File00019><Position>288 70 1014 525</Position><LineCol>68 22</LineCol></File00019><File00020><Position>32288 32117 32918 32422</Position><LineCol>1 0</LineCol></File00020><File00021><Position>282 66 1008 521</Position><LineCol>159 4</LineCol></File00021><File00022><Position>32332 32175 32962 32480</Position><LineCol>3 0</LineCol></File00022><File00023><Position>276 62 1002 517</Position><LineCol>65 0</LineCol></File00023><File00024><Position>32376 32233 33006 32538</Position><LineCol>55 0</LineCol></File00024><File00025><Position>32288 32117 32918 32422</Position><LineCol>1 0</LineCol></File00025><File00026><Position>32310 32146 32940 32451</Position><LineCol>1 0</LineCol></File00026><File00027><Position>32332 32175 32962 32480</Position><LineCol>1 0</LineCol></File00027><File00028><Position>32354 32204 32984 32509</Position><LineCol>49 0</LineCol></File00028><File00029><Position>285 68 1011 523</Position><LineCol>184 43</LineCol></File00029><File00030><Position>289 98 919 403</Position><LineCol>0 0</LineCol></File00030><File00031><Position>311 127 941 432</Position><LineCol>0 0</LineCol></File00031><File00032><Position>339 160 955 462</Position><LineCol>144 0</LineCol></File00032></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 8cf2d7a..586a3d8 100644
--- a/source/Concept/Framework/defines.h
+++ b/source/Concept/Framework/defines.h
@@ -58,9 +58,10 @@
 //Constants

 #define SPEED_PER_PWM 1

 #define DISTANCE_PER_VALUE 1

+#define TICKS_PER_CM 205.0f

 
 #define PI 3.14159265358979323846f

-#define CYCLES_PER_CORRECTION 200

+#define CYCLES_PER_CORRECTION 20

 #define EMPTY_FLOAT 81188.1484f

 

 //IO Module Names

diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c
index 8a7d90f..b0fc96b 100755
--- a/source/Concept/Framework/main.c
+++ b/source/Concept/Framework/main.c
@@ -145,27 +145,29 @@ int main()
 	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);

 	uint32 i = 1;

 	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);

+	Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

+	

 	float rotation = 0;

 

-	Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);

-	Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

+	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);

+	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

 

 

 	//Run

 	while(true)

 	{

-		/*ourDisplay->Print(i++,1,1);

-		ourDisplay->Print(mouse_left->GetXMovement(),1,2);

-		ourDisplay->Print("        ");

-		ourDisplay->Print(mouse_left->GetYMovement(),10,2);

-		ourDisplay->Print("        ");

-		ourDisplay->Print(mouse_right->GetXMovement(),1,3);

-		ourDisplay->Print("        ");

-		ourDisplay->Print(mouse_right->GetYMovement(),10,3);

-		ourDisplay->Print("        ");

-		ourDisplay->Print("                    ",1,4); // clear

+		ourDisplay->Print(i++,1,1);

+				

+		if(!(i % 200))

+		{

+			ourDisplay->Clear();

+			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionX(),1,2);

+			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionY(),1,3);

+			ourDisplay->PrintFloat(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);

+		}

+

 		

-		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);

+		/*distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);

 		value = distanceSensor->GetDistance();

 		ourDisplay->Print(value, 1, 4);

 		ourDisplay->Print(";");

@@ -180,47 +182,41 @@ int main()
 		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);

 		value = distanceSensor->GetDistance();

 		ourDisplay->Print(value);

-		ourDisplay->Print(";");		

-

-

-		msleep(500);*/

-

+		ourDisplay->Print(";");		*/

+		

 		uint8 someInput = ourKeyboard->GetInput();

-		//ourDisplay->Clear();

-		ourDisplay->Print(i++,1,1);

 		//ourDisplay->Print("Ready to accept...", 1, 2);

-		ourDisplay->Print(someInput, 1, 3);

-		ourDisplay->PrintFloat(rotation, 1, 4);

 		switch(someInput)

 		{

 			case 0:

 				ourNavigator->Stop();

+				break;

 			case 1:

-				ourNavigator->Drive(225, rotation, 200, rotation);

+				ourNavigator->Drive(225, rotation, 100, rotation);

 				break;

 			case 2:

-				ourNavigator->Drive(180, rotation, 200, rotation);

+				ourNavigator->Drive(180, rotation, 100, rotation);

 				break;

 			case 3:

-				ourNavigator->Drive(135, rotation, 200, rotation);

+				ourNavigator->Drive(135, rotation, 100, rotation);

 				break;

 			case 4:

-				ourNavigator->Drive(270, rotation, 200, rotation);

+				ourNavigator->Drive(270, rotation, 100, rotation);

 				break;

 			case 5:

 				ourNavigator->Drive(0, rotation, 0, rotation);

 				break;

 			case 6:

-				ourNavigator->Drive(90, rotation, 200, rotation);

+				ourNavigator->Drive(90, rotation, 100, rotation);

 				break;

 			case 7:

-				ourNavigator->Drive(315, rotation, 200, rotation);

+				ourNavigator->Drive(315, rotation, 100, rotation);

 				break;

 			case 8:

-				ourNavigator->Drive(0, rotation, 200, rotation);

+				ourNavigator->Drive(0, rotation, 100, rotation);

 				break;

 			case 9:

-				ourNavigator->Drive(45, rotation, 200, rotation);

+				ourNavigator->Drive(45, rotation, 100, rotation);

 				break;

 			case 10:

 				rotation += 10;

@@ -230,8 +226,6 @@ int main()
 				break;

 		}

 		

-		msleep(50);

-		

 		/*

 		//ourDisplay->Clear();

 		ourDisplay->Print(i++,1,1);

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index a69c533..5dbf8bf 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -54,6 +54,30 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
 	}

 

 	CalculateEngines();

+

+	//dings,,,

+

+	/*float xDrive = cos(direction + PI / 6.0f);

+	float yDrive = sin(direction + PI / 6.0f);

+

+	float vLeft = xDrive;

+	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;

+	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

+

+	vLeft = vLeft * this->robotSpeed + rotationSpeed;

+	vBack = vBack * this->robotSpeed + rotationSpeed;

+	vRight = vRight * this->robotSpeed + rotationSpeed;

+

+	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);

+	curEngine->SetSpeed(vLeft);

+	curEngine->SetEnabled(true);

+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);

+	curEngine->SetSpeed(vBack);

+	curEngine->SetEnabled(true);

+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);

+	curEngine->SetSpeed(vRight);

+	curEngine->SetEnabled(true);

+	curEngine = NULL;*/

 }
 
 //-----------------------------------------------------------------------------

@@ -154,6 +178,8 @@ void Navigator::CalculateDirection()
 //-----------------------------------------------------------------------------

 void Navigator::CalculateEngines()

 {

+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

+	

 	if(direction != EMPTY_FLOAT)

 	{

 		float relativeDirection = this->direction - locationeer->GetOrientation();

@@ -165,15 +191,15 @@ void Navigator::CalculateEngines()
 		float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;

 		float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;

 

-		float speedCorrection = 1.0f;

+		/*float speedCorrection = 1.0f;

 

 		float maxEngineSpeed = 255.0f;

 		float minEngineSpeed = -255.0f;

 

 		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 = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

+		//									vBack * (vBack + vLeft + vRight));

 

 		if(calcSpeed != 1.0f)

 		{

@@ -184,14 +210,14 @@ void Navigator::CalculateEngines()
 		if(maxOverallSpeed > maxEngineSpeed)

 		{

 			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);

-		}

+		}*/

 		

 

-		vLeft = vLeft * this->robotSpeed * speedCorrection;

-		vBack = vBack * this->robotSpeed * speedCorrection;

-		vRight = vRight * this->robotSpeed * speedCorrection;

+		vLeft = vLeft * this->robotSpeed;// * speedCorrection;

+		vBack = vBack * this->robotSpeed;// * speedCorrection;

+		vRight = vRight * this->robotSpeed;// * speedCorrection;

 

-		maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

+		/*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));

 		float minSingleSpeed = min(min(vLeft, vBack), vRight);

 

 		if(rotationSpeed)

@@ -226,7 +252,7 @@ void Navigator::CalculateEngines()
 					vRight -= this->rotationSpeed;

 				}

 			}

-		}

+		}*/

 

 		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);

 		curEngine->SetSpeed(vLeft);

diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h
index 460e387..b3d1b1a 100755
--- a/source/Concept/Framework/modules/input/mouse_sensor.h
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@@ -24,6 +24,8 @@ public:
 		this->configReset = 0;

 		this->configAwake = 0;

 		this->newImage = false;

+		this->positionX = 0;

+		this->positionY = 0;

 	}

 

 	Mouse_Sensor(uint32 sensorId)

@@ -47,6 +49,8 @@ public:
 				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

+				this->positionX = -3.88f * TICKS_PER_CM;

+				this->positionY = -3.88f * TICKS_PER_CM;

 				break;

 			case IO_SENSOR_MOUSE_RIGHT:

 				this->hardwarePort = &PORTC;

@@ -61,6 +65,8 @@ public:
 				this->registerDeltaY = 0x02;

 				this->configReset = 0x80;

 				this->configAwake = 0x01;

+				this->positionX = -3.88f * TICKS_PER_CM;

+				this->positionY = 3.88f * TICKS_PER_CM;

 				break;

 			default:

 				this->hardwarePort = NULL;

@@ -101,8 +107,10 @@ protected:
 	uint8 registerDeltaY;

 	uint8 configReset;

 	uint8 configAwake;

+  //Information

+	float positionX;

+	float positionY;

 

-public:

 	void WriteByte(uint8 newByte)

 	{

 		*hardwareDDR |= pinSDA;//Set SDA output

@@ -180,6 +188,8 @@ public:
 		return pixel;

 	}

 

+

+public:	

 	uint8 GetSqual()

 	{

 		return Read(registerSqual);

@@ -194,6 +204,16 @@ public:
 	{

 		return (int8)(Read(registerDeltaY));

 	}

+

+	float GetPositionX()

+	{

+		return positionX;

+	}

+

+	float GetPositionY()

+	{

+		return positionY;

+	}

 };

 

 #endif
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 262069a..c8cd52d 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -3,5 +3,65 @@
 //-----------------------------------------------------------------------------

 void Position_Tracker::Update()

 {

-	//insert code here

+	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);

+	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);

+

+	int8 leftX = mouseLeft->GetXMovement();

+	int8 leftY = mouseLeft->GetYMovement();

+	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);

+	float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));

+

+	float movementLeftX = cos(angleLeft) * distanceLeft;

+	float movementLeftY = sin(angleLeft) * distanceLeft;

+

+	if(!leftX && !leftY)

+	{

+		movementLeftX = 0;

+		movementLeftY = 0;

+	}

+

+	int8 rightX = mouseRight->GetXMovement();

+	int8 rightY = mouseRight->GetYMovement();

+	float distanceRight = sqrt(rightX * rightX + rightY * rightY);

+	float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));

+

+	float movementRightX = cos(angleRight) * distanceRight;

+	float movementRightY = sin(angleRight) * distanceRight;

+

+	if(!rightX && !rightY)

+	{

+		movementRightX = 0;

+		movementRightY = 0;

+	}

+

+	float movementDifferenceX = movementRightX - movementLeftX;

+	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());

+

+	float robotMovementX = movementDifferenceX / 2.0f;

+	float robotMovementY = movementDifferenceY / 2.0f;

+	robotMovementX += movementLeftX;

+	robotMovementY += movementLeftY + mouseLeft->GetPositionY();

+	float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);

+

+	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);

+

+	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);

+	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);

+

+	if(!robotMovementX && !robotMovementY)

+	{

+		absoluteDiffX = 0;

+		absoluteDiffY = 0;

+	}

+

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);

+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);

+

+	this->positionX += absoluteDiffX;

+	this->positionY += absoluteDiffY;

+

+	this->orientation += orientationDiff;

+

+	this->orientation = easyAngle(this->orientation);

 }

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
index b31dd8f..e93ef0c 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -10,12 +10,18 @@ public:
 	{

 		this->parent = NULL;

 		this->moduleId = 0;

+		this->positionX = 0;

+		this->positionY = 0;

+		this->orientation = 0;

 	}

 

 	Position_Tracker(uint32 trackerId)

 	{

 		this->parent = NULL;

 		this->moduleId = trackerId;

+		this->positionX = 0;

+		this->positionY = 0;

+		this->orientation = 0;

 	}

 

 protected:

diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h
index 735e773..2607001 100644
--- a/source/Concept/Framework/tools.h
+++ b/source/Concept/Framework/tools.h
@@ -14,6 +14,21 @@
 #define min(a, b) (((a) < (b)) ? (a) : (b))

 #define sign(a) (((a) < 0) ? (-1) : (1))

 

+inline float easyAngle(float angle)

+{

+	while(angle < 0)

+	{

+		angle += 2.0f * PI;

+	}

+

+	while(angle >= 2.0f * PI)

+	{

+		angle -= 2.0f * PI;

+	}

+

+	return angle;

+}

+

 inline void sleep(uint16 sec)

 {

   for (uint16 s = 0; s < sec; s++) {

@@ -48,7 +63,7 @@ inline float distance2d(float x1, float y1, float x2, float y2)
 

 inline float direction2d(float x1, float y1, float x2, float y2)

 {

-	return atan((y2 - y1) / (x2 - x1));

+	return atan2((y2 - y1), (x2 - x1));

 }
 
 #endif