diff options
| author | masterm <devnull@localhost> | 2007-02-19 18:15:02 +0100 |
|---|---|---|
| committer | masterm <devnull@localhost> | 2007-02-19 18:15:02 +0100 |
| commit | 354ef5f0d0ad5487288e8d25fc9b1fcecf730eba (patch) | |
| tree | d761d61760999f007a95687948446d633a8e1c17 | |
| parent | c3ce4e70692504c2179219f0dc9c04732353aa54 (diff) | |
| download | rc2007-soccer-354ef5f0d0ad5487288e8d25fc9b1fcecf730eba.tar rc2007-soccer-354ef5f0d0ad5487288e8d25fc9b1fcecf730eba.zip | |
+++ improved navigator
| -rw-r--r-- | source/Concept/Framework/Concept.vcproj | 162 | ||||
| -rw-r--r-- | source/Concept/Framework/RoboCode.aps | 2 | ||||
| -rw-r--r-- | source/Concept/Framework/defines.h | 5 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 159 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.h | 28 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/interpreter/ball_tracker.c | 6 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/interpreter/ball_tracker.h | 4 |
7 files changed, 247 insertions, 119 deletions
diff --git a/source/Concept/Framework/Concept.vcproj b/source/Concept/Framework/Concept.vcproj index 1348191..5de195f 100644 --- a/source/Concept/Framework/Concept.vcproj +++ b/source/Concept/Framework/Concept.vcproj @@ -107,7 +107,7 @@ Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}">
<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:
|