summaryrefslogtreecommitdiffstats
path: root/source
diff options
context:
space:
mode:
authorsicarius <devnull@localhost>2007-02-20 22:01:02 +0100
committersicarius <devnull@localhost>2007-02-20 22:01:02 +0100
commit22433e52a7b0f2e5c548edb915ea11a38a8077ad (patch)
tree1d7add120a7fe0630dc798eaaf4a3277f2bb1e23 /source
parentec2a18e931cdaa6c62a8843c29dc93c4a823a2cd (diff)
downloadrc2007-soccer-22433e52a7b0f2e5c548edb915ea11a38a8077ad.tar
rc2007-soccer-22433e52a7b0f2e5c548edb915ea11a38a8077ad.zip
+++ Codework, Navigator&Positiontracker done (buggy?)
Diffstat (limited to 'source')
-rw-r--r--source/Concept/Framework/RoboCode.aps2
-rw-r--r--source/Concept/Framework/defines.h3
-rwxr-xr-xsource/Concept/Framework/main.c58
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c44
-rwxr-xr-xsource/Concept/Framework/modules/input/mouse_sensor.h22
-rwxr-xr-xsource/Concept/Framework/modules/interpreter/position_tracker.c62
-rwxr-xr-xsource/Concept/Framework/modules/interpreter/position_tracker.h6
-rw-r--r--source/Concept/Framework/tools.h17
8 files changed, 168 insertions, 46 deletions
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
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-21 20:59:53 +0100