+++ Additional Codework

6 files changed, 218 insertions, 90 deletions

diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps
index 4822e36..bb686cb 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>17-Feb-2007 21:48:40</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>1</ISDIRTY><OPTIONS><OPTION><FILE>atmega128io.c</FILE><OPTIONLIST></OPTIONLIST></OPTION><OPTION><FILE>main.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\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/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2 -O3 -fsigned-char </OPTIONSFORALL><LINKEROPTIONS></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>robot.cpp</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>atmega128io.h</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>main.cpp</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>tools.cpp</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>tools.h</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>atmega128io.cpp</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>modules\interpreter\position_tracker.cpp</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>modules\interpreter\position_tracker.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>modules\io_module.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>modules\output\display.h</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>modules\input\ir_sensor.h</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>modules\output\engine.h</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>modules\input\sensor.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>modules\input\keyboard.h</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>modules\io_module.c</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>robot.c</FileName><Status>1</Status></File00018><File00019><FileId>00019</FileId><FileName>modules\input\ir_sensor.c</FileName><Status>1</Status></File00019><File00020><FileId>00020</FileId><FileName>modules\output\dribbler.h</FileName><Status>1</Status></File00020><File00021><FileId>00021</FileId><FileName>modules\output\kicker.h</FileName><Status>1</Status></File00021><File00022><FileId>00022</FileId><FileName>modules\output\led.h</FileName><Status>1</Status></File00022><File00023><FileId>00023</FileId><FileName>modules\input\distance_sensor.h</FileName><Status>1</Status></File00023><File00024><FileId>00024</FileId><FileName>modules\input\mouse_sensor.h</FileName><Status>1</Status></File00024><File00025><FileId>00025</FileId><FileName>modules\input\sensor.c</FileName><Status>1</Status></File00025><File00026><FileId>00026</FileId><FileName>main.c</FileName><Status>1</Status></File00026><File00027><FileId>00027</FileId><FileName>modules\interpreter\ball_tracker.h</FileName><Status>1</Status></File00027><File00028><FileId>00028</FileId><FileName>modules\interpreter\ball_tracker.c</FileName><Status>1</Status></File00028><File00029><FileId>00029</FileId><FileName>modules\interpreter\position_tracker.c</FileName><Status>1</Status></File00029><File00030><FileId>00030</FileId><FileName>modules\input\distance_sensor.c</FileName><Status>1</Status></File00030><File00031><FileId>00031</FileId><FileName>modules\output\engine.c</FileName><Status>1</Status></File00031><File00032><FileId>00032</FileId><FileName>modules\output\dribbler.c</FileName><Status>1</Status></File00032><File00033><FileId>00033</FileId><FileName>modules\executor\navigator.h</FileName><Status>1</Status></File00033><File00034><FileId>00034</FileId><FileName>modules\executor\navigator.c</FileName><Status>1</Status></File00034></Files><Workspace><File00000><Position>310 73 1036 528</Position><LineCol>22 22</LineCol></File00000><File00001><Position>32650 32233 33250 32479</Position><LineCol>155 19</LineCol></File00001><File00002><Position>32672 32255 33272 32501</Position><LineCol>45 0</LineCol></File00002><File00003><Position>32694 32277 33294 32523</Position><LineCol>16 0</LineCol></File00003><File00004><Position>32716 32299 33316 32545</Position><LineCol>19 0</LineCol></File00004><File00005><Position>32606 32189 33206 32435</Position><LineCol>97 0</LineCol></File00005><File00006><Position>32650 32233 33250 32479</Position><LineCol>17 0</LineCol></File00006><File00007><Position>32602 32168 33328 32623</Position><LineCol>4 17</LineCol></File00007><File00008><Position>32694 32277 33294 32523</Position><LineCol>2 0</LineCol></File00008><File00009><Position>399 216 1025 517</Position><LineCol>4 0</LineCol></File00009><File00010><Position>311 100 941 405</Position><LineCol>31 2</LineCol></File00010><File00011><Position>336 131 966 436</Position><LineCol>6 0</LineCol></File00011><File00012><Position>358 160 988 465</Position><LineCol>42 16</LineCol></File00012><File00013><Position>380 189 1010 494</Position><LineCol>17 2</LineCol></File00013><File00014><Position>402 218 1032 523</Position><LineCol>22 12</LineCol></File00014><File00015><Position>314 102 944 407</Position><LineCol>3 16</LineCol></File00015><File00016><Position>336 131 966 436</Position><LineCol>18 9</LineCol></File00016><File00017><Position>358 160 988 465</Position><LineCol>1 0</LineCol></File00017><File00018><Position>380 189 1010 494</Position><LineCol>91 34</LineCol></File00018><File00019><Position>402 218 1032 523</Position><LineCol>0 0</LineCol></File00019><File00020><Position>314 102 944 407</Position><LineCol>24 13</LineCol></File00020><File00021><Position>336 131 966 436</Position><LineCol>3 16</LineCol></File00021><File00022><Position>358 160 988 465</Position><LineCol>3 16</LineCol></File00022><File00023><Position>380 189 1010 494</Position><LineCol>47 30</LineCol></File00023><File00024><Position>402 218 1032 523</Position><LineCol>187 20</LineCol></File00024><File00025><Position>314 102 944 407</Position><LineCol>1 0</LineCol></File00025><File00026><Position>336 131 966 436</Position><LineCol>125 14</LineCol></File00026><File00027><Position>358 160 988 465</Position><LineCol>19 25</LineCol></File00027><File00028><Position>380 189 1010 494</Position><LineCol>67 38</LineCol></File00028><File00029><Position>402 218 1032 523</Position><LineCol>7 0</LineCol></File00029><File00030><Position>314 102 944 407</Position><LineCol>27 0</LineCol></File00030><File00031><Position>336 131 966 436</Position><LineCol>0 0</LineCol></File00031><File00032><Position>358 160 988 465</Position><LineCol>0 0</LineCol></File00032><File00033><Position>402 218 1021 523</Position><LineCol>35 15</LineCol></File00033><File00034><Position>310 72 1025 527</Position><LineCol>78 34</LineCol><State>Maximized</State></File00034></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 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>

diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile
index d0fbfb0..fc917c5 100644
--- a/source/Concept/Framework/default/Makefile
+++ b/source/Concept/Framework/default/Makefile
@@ -13,7 +13,7 @@ COMMON = -mmcu=$(MCU)
 

 ## Compile options common for all C compilation units.

 CFLAGS = $(COMMON)

-CFLAGS += -Wall -gdwarf-2 -O3 -fsigned-char 

+CFLAGS += -Wall -gdwarf-2 -DF_CPU=16000000ULUL -O3 -fsigned-char 

 CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d 

 

 ## Assembly specific flags

@@ -22,7 +22,7 @@ ASMFLAGS += -x assembler-with-cpp -Wa,-gdwarf2
 

 ## Linker flags

 LDFLAGS = $(COMMON)

-LDFLAGS += 

+LDFLAGS += -lm 

 

 

 ## Intel Hex file production flags

@@ -36,6 +36,9 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0
 ## Include Directories

 INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output" 

 

+## Libraries

+LIBS = -lm 

+

 ## Objects that must be built in order to link

 OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o 

 

diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c
index fa6d718..7822933 100755
--- a/source/Concept/Framework/main.c
+++ b/source/Concept/Framework/main.c
@@ -132,13 +132,54 @@ int main()
 	uint16 value = 0;

 	int8 value2 = 0;

 	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);

+	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);

 	uint32 i = 1;

-	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);

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

+	float rotation = 0;

+

+	ourDisplay->Print("Starting...", 1, 1);

 

 	//Run

 	while(true)

 	{

-		msleep(500);

+		uint8 someInput = ourKeyboard->GetInput();

+		switch(someInput)

+		{

+			case 1:

+				ourNavigator->Drive(225.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 2:

+				ourNavigator->Drive(180.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 3:

+				ourNavigator->Drive(135.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 4:

+				ourNavigator->Drive(270.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 5:

+				ourNavigator->Stop();

+				break;

+			case 6:

+				ourNavigator->Drive(90.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 7:

+				ourNavigator->Drive(315.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 8:

+				ourNavigator->Drive(0.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 9:

+				ourNavigator->Drive(45.0f * PI / 180.0f, rotation, 200, rotation);

+				break;

+			case 10:

+				rotation -= 10;

+				break;

+			case 12:

+				rotation += 10;

+				break;

+		}

+		msleep(50);/*

 		ourDisplay->Clear();

 		ourDisplay->Print(i++);

 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);

@@ -172,15 +213,11 @@ int main()
 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);

 		value = ourSensor->GetIRIntensity();

 		ourDisplay->Print(value);

-		ourDisplay->Print(";");

-		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetXMovement();

-		ourDisplay->Print(value2, 10, 4);

-		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetYMovement();

-		ourDisplay->Print(value2, 15, 4);

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

 

 		localRobot->Update();

 

-		ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);

+		//ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);

 	}

 

 	//Cleanup

diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 459d4c1..691b1d0 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -7,7 +7,7 @@ void Navigator::Stop()
 	this->targetAngle = -1.0f;

 	this->targetX = -1.0f;

 	this->targetY = -1.0f;

-	this->speed = 0;

+	this->robotSpeed = 0;

 	this->rotationSpeed = 0;

 

 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)

@@ -20,12 +20,12 @@ void Navigator::Stop()
 //-----------------------------------------------------------------------------

 void Navigator::Rotate(float rotationSpeed)

 {

-	this->rotationSpeed = rotationSpeed;

+	this->rotationSpeed = min(rotationSpeed, 255.0f);;

 	this->direction = -1.0f;

 	this->targetAngle = -1.0f;

 	this->targetX = -1.0f;

 	this->targetY = -1.0f;

-	this->speed = 0;

+	this->robotSpeed = 0;

 

 	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)

 	{

@@ -35,6 +35,66 @@ void Navigator::Rotate(float rotationSpeed)
 }

 

 //-----------------------------------------------------------------------------

+void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed)

+{

+	this->rotationSpeed = min(rotationSpeed, 255.0f);

+	this->direction = newDirection;

+	this->targetAngle = newAngle;

+	this->targetX = -1.0f;

+	this->targetY = -1.0f;

+	this->robotSpeed = newSpeed;

+

+	if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI)

+	{

+		if(rotationSpeed > 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+	else

+	{

+		if(rotationSpeed < 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+

+	Update();

+}

+

+//-----------------------------------------------------------------------------

+void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)

+{

+	if(newX < 0 || newY < 0) return;

+

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

+

+	this->rotationSpeed = min(rotationSpeed, 255.0f);

+	this->targetX = newX;

+	this->targetY = newY;

+	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;

+	this->targetAngle = newAngle;

+	this->robotSpeed = newSpeed;

+

+	if(targetAngle - locationeer->GetOrientation() > PI)

+	{

+		if(rotationSpeed > 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+	else

+	{

+		if(rotationSpeed < 0)

+		{

+			rotationSpeed = -rotationSpeed;

+		}

+	}

+

+	Update();

+}

+

+//-----------------------------------------------------------------------------

 void Navigator::Update()

 {

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

@@ -103,97 +163,110 @@ void Navigator::Update()
 	//Calculate directional/rotational engine speed

 	if(hasDistanceToDrive)

 	{

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

 

-

-		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;

-

-		if(speed > maxRobotSpeed)

+		if(targetX >= 0 && targetY >= 0)

 		{

-			speed = maxRobotSpeed;

+			float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);

+			relativeDirection = directionToTarget - locationeer->GetOrientation();

 		}

 

-		maxMotorSpeed = speed / (sqrt(2) / 2);

+		float xDrive = cos(relativeDirection);

+		float yDrive = sin(relativeDirection);

 

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

+		float vBack = yDrive;

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

 

+		float speedCorrection = 1.0f;

 

-		relAngel = direction - orientation

-		

-		robotSpeed = sin(45) * maxMotorSpeed

-		maxMotorSpeed = robotSpeed / sin(45)

+		float maxEngineSpeed = 255.0f;

+		float minEngineSpeed = -255.0f;

 

-		if(relAngel > 45)

-		{

-			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);

+		float maxSingleSpeed = max(max(vLeft, vBack), vRight);

 

-			back = speed / sin(relAngel);

-		}

-		else

+		float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 

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

+

+		if(calcSpeed != 1.0f)

 		{

-			

+			speedCorrection = 1.0f / calcSpeed;

 		}

 

-		left = 

-		back = sin(relAngel) * speed

-

-

-		direction = 0:

-		orientation = 0:

-		left = speed

-		right = -speed

-		back = 0

+		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;

+		if(maxOverallSpeed > maxEngineSpeed)

+		{

+			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);

+		}

+		

 

-		direction = 0:

-		orientation = 90:

-		left = speed

-		right = speed

-		back = (sinVcos)(45) * speed

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

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

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

 

-		direction = 0:

-		orientation = 45:

-		left = speed

-		right = 0

-		back = -speed

+		maxSingleSpeed = max(max(vLeft, vBack), vRight);

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

 

-		direction = 0:

-		orientation = 180:

-		left = -speed

-		right = speed

-		back = 0

+		if(hasOrientationToAdjust)

+		{

+			if(this->rotationSpeed > 0)

+			{

+				if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)

+				{

+					vLeft += maxEngineSpeed - maxSingleSpeed;

+					vBack += maxEngineSpeed - maxSingleSpeed;

+					vRight += maxEngineSpeed - maxSingleSpeed;

+				}

+				else

+				{

+					vLeft += this->rotationSpeed;

+					vBack += this->rotationSpeed;

+					vRight += this->rotationSpeed;

+				}

+			}

+			else

+			{

+				if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)

+				{

+					vLeft -= minEngineSpeed - minSingleSpeed;

+					vBack -= minEngineSpeed - minSingleSpeed;

+					vRight -= minEngineSpeed - minSingleSpeed;

+				}

+				else

+				{

+					vLeft -= this->rotationSpeed;

+					vBack -= this->rotationSpeed;

+					vRight -= this->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;

 	}

-	else if(!hasOrientationToAdjust)

+	else if(hasOrientationToAdjust)

 	{

-		Stop();

-	}

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

+		curEngine->SetSpeed(this->rotationSpeed);

+		curEngine->SetEnabled(true);

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

+		curEngine->SetSpeed(this->rotationSpeed);

+		curEngine->SetEnabled(true);

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

+		curEngine->SetSpeed(this->rotationSpeed);

+		curEngine->SetEnabled(true);

+		curEngine = NULL;

+	}	

 	else

 	{

+		Stop();

 	}

 }

-

-// Aktualieren ohne Parameter

-/*void Navigator::Update() {	

-	// Richtung in x und y-Kompontente zerlegen

-	double y = cos((double)direction*0.01745); // richtung ist winkel

-	double x = sin((double)direction*0.01745);

-	

-	// Abweichung der Ausrichtung ermitteln(als winkel)

-	int w = sensor.GetAusrichtung() - ausrichtung;

-

-	// Stärke der einzelnen Motoren berechnen	

-	double v0 = (-x+sqrt(3)*y)/2;

-	double v1 = x;

-	double v2 = (-x-sqrt(3)*y)/2;

-

-	// Ausgerechnete Stärke an die Motoren übergeben

-	board.motor(0,(int)((double)v0*speed +w));

-	board.motor(1,(int)((double)v1*speed +w));

-	board.motor(2,(int)((double)v2*speed +w));

-}

-

-// Aktualieren mit allen Parametern

-void Navigator::Drive(float newDirection, float newAngle, float newSpeed) {

-	SetDirection(newDirection);

-	SetAngle(newAngle);

-	SetSpeed(newSpeed);

-	Update(); // Und anwenden

-}*/

diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index ed8b3c8..8da52f6 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,6 +1,7 @@
 #ifndef _NAVIGATOR_H

 #define _NAVIGATOR_H

 

+//#include <math.h>

 #include "../../stdafx.h"

 

 class Navigator : public IO_Module

@@ -14,7 +15,7 @@ public:
 		this->targetAngle = -1.0f;

 		this->targetX = -1.0f;

 		this->targetY = -1.0f;

-		this->speed = 0;

+		this->robotSpeed = 0;

 		this->rotationSpeed = 0;

 	}

 

@@ -26,7 +27,7 @@ public:
 		this->targetAngle = -1.0f;

 		this->targetX = -1.0f;

 		this->targetY = -1.0f;

-		this->speed = 0;

+		this->robotSpeed = 0;

 		this->rotationSpeed = 0;

 	}

 

@@ -35,7 +36,7 @@ protected:
 	float targetAngle;

 	float targetX;

 	float targetY;

-	float speed;

+	float robotSpeed;

 	float rotationSpeed;

 

 public:

@@ -45,9 +46,14 @@ public:
 

 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);

 

-	void DriveTo(float newX, float newY, float newAngle);

+	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);

 

 	void Rotate(float rotationSpeed);

+

+	void SetSpeed(float newSpeed)

+	{

+		this->robotSpeed = newSpeed;

+	}

 };

 

 #endif
diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h
index 0ca4dda..ca19993 100644
--- a/source/Concept/Framework/tools.h
+++ b/source/Concept/Framework/tools.h
@@ -9,6 +9,10 @@
 	void operator delete(void* p);
 #endif

 

+#define max(a, b) (((a) > (b)) ? (a) : (b))

+#define min(a, b) (((a) < (b)) ? (a) : (b))

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

+

 inline void sleep(int sec)

 {

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

@@ -40,6 +44,11 @@ inline void usleep(int usec)
 inline float distance2d(float x1, float y1, float x2, float y2)

 {

 	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));

+}

+

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

+{

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

 }
 
 #endif