From 3c3c628b617dc53f0b7b59285c7d67888074c33d Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Sun, 18 Feb 2007 00:14:00 +0000
Subject: +++ Additional Codework

---
 source/Concept/Framework/RoboCode.aps              |   2 +-
 source/Concept/Framework/atmega128io.c             | 585 +++++++++++++++++++++
 source/Concept/Framework/atmega128io.cpp           | 585 ---------------------
 source/Concept/Framework/default/Makefile          |  46 +-
 source/Concept/Framework/defines.h                 |  19 +-
 source/Concept/Framework/display.cpp               |   1 -
 source/Concept/Framework/distance_sensor.cpp       |  26 -
 source/Concept/Framework/dribbler.cpp              |   1 -
 source/Concept/Framework/engine.cpp                |   1 -
 source/Concept/Framework/io_module.cpp             |   1 -
 source/Concept/Framework/ir_sensor.cpp             |   9 -
 source/Concept/Framework/keyboard.cpp              |   1 -
 source/Concept/Framework/kicker.cpp                |   1 -
 source/Concept/Framework/led.cpp                   |   1 -
 source/Concept/Framework/main.c                    | 189 +++++++
 source/Concept/Framework/main.cpp                  | 158 ------
 .../Concept/Framework/modules/executor/navigator.c | 199 +++++++
 .../Concept/Framework/modules/executor/navigator.h |  53 ++
 .../Framework/modules/input/distance_sensor.c      |  32 ++
 .../Framework/modules/input/distance_sensor.h      |  71 +++
 source/Concept/Framework/modules/input/ir_sensor.c |   9 +
 source/Concept/Framework/modules/input/ir_sensor.h |  62 +++
 source/Concept/Framework/modules/input/keyboard.c  |   1 +
 source/Concept/Framework/modules/input/keyboard.h  |  87 +++
 .../Concept/Framework/modules/input/mouse_sensor.c |   1 +
 .../Concept/Framework/modules/input/mouse_sensor.h | 199 +++++++
 source/Concept/Framework/modules/input/sensor.c    |   1 +
 source/Concept/Framework/modules/input/sensor.h    |  27 +
 .../Framework/modules/interpreter/ball_tracker.c   | 125 +++++
 .../Framework/modules/interpreter/ball_tracker.h   |  35 ++
 .../modules/interpreter/position_tracker.c         |   7 +
 .../modules/interpreter/position_tracker.h         |  45 ++
 source/Concept/Framework/modules/io_module.c       |   1 +
 source/Concept/Framework/modules/io_module.h       |  45 ++
 source/Concept/Framework/modules/output/display.c  |   1 +
 source/Concept/Framework/modules/output/display.h  | 198 +++++++
 source/Concept/Framework/modules/output/dribbler.c |   1 +
 source/Concept/Framework/modules/output/dribbler.h | 118 +++++
 source/Concept/Framework/modules/output/engine.c   |   1 +
 source/Concept/Framework/modules/output/engine.h   | 124 +++++
 source/Concept/Framework/modules/output/kicker.c   |   1 +
 source/Concept/Framework/modules/output/kicker.h   |  84 +++
 source/Concept/Framework/modules/output/led.c      |   1 +
 source/Concept/Framework/modules/output/led.h      |  65 +++
 source/Concept/Framework/mouse_sensor.cpp          |   1 -
 source/Concept/Framework/position_tracker.cpp      |   7 -
 source/Concept/Framework/robot.c                   | 135 +++++
 source/Concept/Framework/robot.cpp                 | 129 -----
 source/Concept/Framework/sensor.cpp                |   1 -
 source/Concept/Framework/stdafx.h                  |   2 +
 source/Concept/Framework/tools.c                   |  17 +
 source/Concept/Framework/tools.cpp                 |  17 -
 source/Concept/Framework/tools.h                   |  10 +-
 53 files changed, 2578 insertions(+), 961 deletions(-)
 create mode 100755 source/Concept/Framework/atmega128io.c
 delete mode 100644 source/Concept/Framework/atmega128io.cpp
 delete mode 100644 source/Concept/Framework/display.cpp
 delete mode 100644 source/Concept/Framework/distance_sensor.cpp
 delete mode 100644 source/Concept/Framework/dribbler.cpp
 delete mode 100644 source/Concept/Framework/engine.cpp
 delete mode 100644 source/Concept/Framework/io_module.cpp
 delete mode 100644 source/Concept/Framework/ir_sensor.cpp
 delete mode 100644 source/Concept/Framework/keyboard.cpp
 delete mode 100644 source/Concept/Framework/kicker.cpp
 delete mode 100644 source/Concept/Framework/led.cpp
 create mode 100755 source/Concept/Framework/main.c
 delete mode 100644 source/Concept/Framework/main.cpp
 create mode 100755 source/Concept/Framework/modules/executor/navigator.c
 create mode 100755 source/Concept/Framework/modules/executor/navigator.h
 create mode 100755 source/Concept/Framework/modules/input/distance_sensor.c
 create mode 100755 source/Concept/Framework/modules/input/distance_sensor.h
 create mode 100755 source/Concept/Framework/modules/input/ir_sensor.c
 create mode 100755 source/Concept/Framework/modules/input/ir_sensor.h
 create mode 100755 source/Concept/Framework/modules/input/keyboard.c
 create mode 100755 source/Concept/Framework/modules/input/keyboard.h
 create mode 100755 source/Concept/Framework/modules/input/mouse_sensor.c
 create mode 100755 source/Concept/Framework/modules/input/mouse_sensor.h
 create mode 100755 source/Concept/Framework/modules/input/sensor.c
 create mode 100755 source/Concept/Framework/modules/input/sensor.h
 create mode 100755 source/Concept/Framework/modules/interpreter/ball_tracker.c
 create mode 100755 source/Concept/Framework/modules/interpreter/ball_tracker.h
 create mode 100755 source/Concept/Framework/modules/interpreter/position_tracker.c
 create mode 100755 source/Concept/Framework/modules/interpreter/position_tracker.h
 create mode 100755 source/Concept/Framework/modules/io_module.c
 create mode 100755 source/Concept/Framework/modules/io_module.h
 create mode 100755 source/Concept/Framework/modules/output/display.c
 create mode 100755 source/Concept/Framework/modules/output/display.h
 create mode 100755 source/Concept/Framework/modules/output/dribbler.c
 create mode 100755 source/Concept/Framework/modules/output/dribbler.h
 create mode 100755 source/Concept/Framework/modules/output/engine.c
 create mode 100755 source/Concept/Framework/modules/output/engine.h
 create mode 100755 source/Concept/Framework/modules/output/kicker.c
 create mode 100755 source/Concept/Framework/modules/output/kicker.h
 create mode 100755 source/Concept/Framework/modules/output/led.c
 create mode 100755 source/Concept/Framework/modules/output/led.h
 delete mode 100644 source/Concept/Framework/mouse_sensor.cpp
 delete mode 100644 source/Concept/Framework/position_tracker.cpp
 create mode 100755 source/Concept/Framework/robot.c
 delete mode 100644 source/Concept/Framework/robot.cpp
 delete mode 100644 source/Concept/Framework/sensor.cpp
 create mode 100755 source/Concept/Framework/tools.c
 delete mode 100644 source/Concept/Framework/tools.cpp

(limited to 'source/Concept')

diff --git a/source/Concept/Framework/RoboCode.aps b/source/Concept/Framework/RoboCode.aps
index dc61dc5..4822e36 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 14:21:10</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>main.cpp</SOURCEFILE><SOURCEFILE>sensor.cpp</SOURCEFILE><SOURCEFILE>tools.cpp</SOURCEFILE><SOURCEFILE>atmega128io.cpp</SOURCEFILE><SOURCEFILE>distance_sensor.cpp</SOURCEFILE><SOURCEFILE>engine.cpp</SOURCEFILE><SOURCEFILE>io_module.cpp</SOURCEFILE><SOURCEFILE>ir_sensor.cpp</SOURCEFILE><SOURCEFILE>kicker.cpp</SOURCEFILE><SOURCEFILE>led.cpp</SOURCEFILE><SOURCEFILE>robot.cpp</SOURCEFILE><SOURCEFILE>dribbler.cpp</SOURCEFILE><SOURCEFILE>display.cpp</SOURCEFILE><SOURCEFILE>mouse_sensor.cpp</SOURCEFILE><SOURCEFILE>keyboard.cpp</SOURCEFILE><SOURCEFILE>position_tracker.cpp</SOURCEFILE><HEADERFILE>sensor.h</HEADERFILE><HEADERFILE>stdafx.h</HEADERFILE><HEADERFILE>tools.h</HEADERFILE><HEADERFILE>atmega128io.h</HEADERFILE><HEADERFILE>defines.h</HEADERFILE><HEADERFILE>distance_sensor.h</HEADERFILE><HEADERFILE>engine.h</HEADERFILE><HEADERFILE>io_module.h</HEADERFILE><HEADERFILE>ir_sensor.h</HEADERFILE><HEADERFILE>kicker.h</HEADERFILE><HEADERFILE>led.h</HEADERFILE><HEADERFILE>robot.h</HEADERFILE><HEADERFILE>dribbler.h</HEADERFILE><HEADERFILE>display.h</HEADERFILE><HEADERFILE>mouse_sensor.h</HEADERFILE><HEADERFILE>keyboard.h</HEADERFILE><HEADERFILE>position_tracker.h</HEADERFILE><OTHERFILE>default\Makefile</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>YES</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE>default\Makefile</EXTERNALMAKEFILE><PART>atmega128</PART><HEX>1</HEX><LIST>0</LIST><MAP>0</MAP><OUTPUTFILENAME>RoboCode.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>1</ISDIRTY><OPTIONS/><INCDIRS/><LIBDIRS/><LIBS/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2  -O0 -fsigned-char</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\WinAVR\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\WinAVR\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>Y:\Concept\Framework\sensor.h</Name><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\distance_sensor.h</Name><Name>Y:\Concept\Framework\engine.h</Name><Name>Y:\Concept\Framework\io_module.h</Name><Name>Y:\Concept\Framework\ir_sensor.h</Name><Name>Y:\Concept\Framework\kicker.h</Name><Name>Y:\Concept\Framework\led.h</Name><Name>Y:\Concept\Framework\robot.h</Name><Name>Y:\Concept\Framework\dribbler.h</Name><Name>Y:\Concept\Framework\display.h</Name><Name>Y:\Concept\Framework\mouse_sensor.h</Name><Name>Y:\Concept\Framework\keyboard.h</Name><Name>Y:\Concept\Framework\main.cpp</Name><Name>Y:\Concept\Framework\sensor.cpp</Name><Name>Y:\Concept\Framework\tools.cpp</Name><Name>Y:\Concept\Framework\atmega128io.cpp</Name><Name>Y:\Concept\Framework\distance_sensor.cpp</Name><Name>Y:\Concept\Framework\engine.cpp</Name><Name>Y:\Concept\Framework\io_module.cpp</Name><Name>Y:\Concept\Framework\ir_sensor.cpp</Name><Name>Y:\Concept\Framework\kicker.cpp</Name><Name>Y:\Concept\Framework\led.cpp</Name><Name>Y:\Concept\Framework\robot.cpp</Name><Name>Y:\Concept\Framework\dribbler.cpp</Name><Name>Y:\Concept\Framework\display.cpp</Name><Name>Y:\Concept\Framework\mouse_sensor.cpp</Name><Name>Y:\Concept\Framework\keyboard.cpp</Name></Files></ProjectFiles><Files><File00000><FileId>00000</FileId><FileName>stdafx.h</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>engine.h</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>defines.h</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>io_module.h</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>robot.h</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>robot.cpp</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>atmega128io.h</FileName><Status>1</Status></File00006><File00007><FileId>00007</FileId><FileName>main.cpp</FileName><Status>1</Status></File00007><File00008><FileId>00008</FileId><FileName>tools.cpp</FileName><Status>1</Status></File00008><File00009><FileId>00009</FileId><FileName>tools.h</FileName><Status>1</Status></File00009><File00010><FileId>00010</FileId><FileName>distance_sensor.h</FileName><Status>1</Status></File00010><File00011><FileId>00011</FileId><FileName>ir_sensor.h</FileName><Status>1</Status></File00011><File00012><FileId>00012</FileId><FileName>kicker.h</FileName><Status>1</Status></File00012><File00013><FileId>00013</FileId><FileName>led.h</FileName><Status>1</Status></File00013><File00014><FileId>00014</FileId><FileName>sensor.h</FileName><Status>1</Status></File00014><File00015><FileId>00015</FileId><FileName>dribbler.h</FileName><Status>1</Status></File00015><File00016><FileId>00016</FileId><FileName>kicker.cpp</FileName><Status>1</Status></File00016><File00017><FileId>00017</FileId><FileName>dribbler.cpp</FileName><Status>1</Status></File00017><File00018><FileId>00018</FileId><FileName>ir_sensor.cpp</FileName><Status>1</Status></File00018><File00019><FileId>00019</FileId><FileName>io_module.cpp</FileName><Status>1</Status></File00019><File00020><FileId>00020</FileId><FileName>engine.cpp</FileName><Status>1</Status></File00020><File00021><FileId>00021</FileId><FileName>distance_sensor.cpp</FileName><Status>1</Status></File00021><File00022><FileId>00022</FileId><FileName>atmega128io.cpp</FileName><Status>1</Status></File00022><File00023><FileId>00023</FileId><FileName>led.cpp</FileName><Status>1</Status></File00023><File00024><FileId>00024</FileId><FileName>sensor.cpp</FileName><Status>1</Status></File00024><File00025><FileId>00025</FileId><FileName>display.h</FileName><Status>1</Status></File00025><File00026><FileId>00026</FileId><FileName>display.cpp</FileName><Status>1</Status></File00026><File00027><FileId>00027</FileId><FileName>mouse_sensor.h</FileName><Status>1</Status></File00027><File00028><FileId>00028</FileId><FileName>mouse_sensor.cpp</FileName><Status>1</Status></File00028><File00029><FileId>00029</FileId><FileName>keyboard.h</FileName><Status>1</Status></File00029><File00030><FileId>00030</FileId><FileName>keyboard.cpp</FileName><Status>1</Status></File00030><File00031><FileId>00031</FileId><FileName>position_tracker.h</FileName><Status>1</Status></File00031><File00032><FileId>00032</FileId><FileName>position_tracker.cpp</FileName><Status>1</Status></File00032></Files><Workspace><File00000><Position>299 74 1025 529</Position><LineCol>20 29</LineCol></File00000><File00001><Position>344 135 944 381</Position><LineCol>111 2</LineCol></File00001><File00002><Position>366 157 966 403</Position><LineCol>148 6</LineCol></File00002><File00003><Position>388 179 988 425</Position><LineCol>16 0</LineCol></File00003><File00004><Position>410 201 1010 447</Position><LineCol>19 0</LineCol></File00004><File00005><Position>300 91 900 337</Position><LineCol>97 44</LineCol></File00005><File00006><Position>344 135 944 381</Position><LineCol>18 0</LineCol></File00006><File00007><Position>296 70 1022 525</Position><LineCol>149 41</LineCol></File00007><File00008><Position>388 179 988 425</Position><LineCol>2 0</LineCol></File00008><File00009><Position>410 201 1010 447</Position><LineCol>32 17</LineCol></File00009><File00010><Position>297 98 927 403</Position><LineCol>19 33</LineCol></File00010><File00011><Position>319 127 949 432</Position><LineCol>57 7</LineCol></File00011><File00012><Position>341 156 971 461</Position><LineCol>29 0</LineCol></File00012><File00013><Position>363 185 993 490</Position><LineCol>58 0</LineCol></File00013><File00014><Position>385 214 1015 519</Position><LineCol>17 22</LineCol></File00014><File00015><Position>297 98 927 403</Position><LineCol>58 16</LineCol></File00015><File00016><Position>341 156 971 461</Position><LineCol>1 0</LineCol></File00016><File00017><Position>363 185 993 490</Position><LineCol>0 0</LineCol></File00017><File00018><Position>385 214 1015 519</Position><LineCol>9 0</LineCol></File00018><File00019><Position>297 98 927 403</Position><LineCol>1 0</LineCol></File00019><File00020><Position>319 127 949 432</Position><LineCol>1 0</LineCol></File00020><File00021><Position>341 156 971 461</Position><LineCol>0 0</LineCol></File00021><File00022><Position>363 185 993 490</Position><LineCol>511 0</LineCol></File00022><File00023><Position>385 214 1015 519</Position><LineCol>1 0</LineCol></File00023><File00024><Position>297 98 927 403</Position><LineCol>0 0</LineCol></File00024><File00025><Position>300 100 930 405</Position><LineCol>170 2</LineCol></File00025><File00026><Position>344 158 974 463</Position><LineCol>0 0</LineCol></File00026><File00027><Position>347 160 977 465</Position><LineCol>198 38</LineCol></File00027><File00028><Position>391 218 1021 523</Position><LineCol>1 0</LineCol></File00028><File00029><Position>303 102 933 407</Position><LineCol>34 10</LineCol></File00029><File00030><Position>347 160 977 465</Position><LineCol>1 0</LineCol></File00030><File00031><Position>299 72 1025 527</Position><LineCol>17 28</LineCol><State>Maximized</State></File00031><File00032><Position>303 102 933 407</Position><LineCol>1 0</LineCol></File00032></Workspace><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
+<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>
diff --git a/source/Concept/Framework/atmega128io.c b/source/Concept/Framework/atmega128io.c
new file mode 100755
index 0000000..b1d6f4d
--- /dev/null
+++ b/source/Concept/Framework/atmega128io.c
@@ -0,0 +1,585 @@
+#include "atmega128io.h"
+
+/*************************************************************************
+Title:    Interrupt UART library with receive/transmit circular buffers
+Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury
+File:     $Id: uart.c,v 1.5.2.10 2005/11/15 19:49:12 peter Exp $
+Software: AVR-GCC 3.3 
+Hardware: any AVR with built-in UART, 
+          tested on AT90S8515 at 4 Mhz and ATmega at 1Mhz
+
+DESCRIPTION:
+    An interrupt is generated when the UART has finished transmitting or
+    receiving a byte. The interrupt handling routines use circular buffers
+    for buffering received and transmitted data.
+    
+    The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define
+    the buffer size in bytes. Note that these variables must be a 
+    power of 2.
+    
+USAGE:
+    Refere to the header file uart.h for a description of the routines. 
+    See also example test_uart.c.
+
+NOTES:
+    Based on Atmel Application Note AVR306
+                    
+*************************************************************************/
+#include <avr/io.h>
+#include <avr/interrupt.h>
+//#include <avr/signal.h>
+#include <avr/pgmspace.h>
+
+/*
+ *  constants and macros
+ */
+
+/* size of RX/TX buffers */
+#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1)
+#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1)
+
+#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK )
+#error RX buffer size is not a power of 2
+#endif
+#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK )
+#error TX buffer size is not a power of 2
+#endif
+
+#if defined(__AVR_AT90S2313__) \
+ || defined(__AVR_AT90S4414__) || defined(__AVR_AT90S4434__) \
+ || defined(__AVR_AT90S8515__) || defined(__AVR_AT90S8535__) \
+ || defined(__AVR_ATmega103__)
+ /* old AVR classic or ATmega103 with one UART */
+ #define AT90_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   USR
+ #define UART0_CONTROL  UCR
+ #define UART0_DATA     UDR  
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_AT90S2333__) || defined(__AVR_AT90S4433__)
+ /* old AVR classic with one UART */
+ #define AT90_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR 
+ #define UART0_UDRIE    UDRIE
+#elif  defined(__AVR_ATmega8__)  || defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) \
+  || defined(__AVR_ATmega8515__) || defined(__AVR_ATmega8535__) \
+  || defined(__AVR_ATmega323__)
+  /* ATmega with one USART */
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega163__) 
+  /* ATmega163 with one UART */
+ #define ATMEGA_UART
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega162__)
+ /* ATmega with two USART */
+ #define ATMEGA_USART0
+ #define ATMEGA_USART1
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RECV
+ #define UART1_RECEIVE_INTERRUPT   SIG_USART1_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_DATA
+ #define UART1_TRANSMIT_INTERRUPT  SIG_USART1_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+ #define UART1_STATUS   UCSR1A
+ #define UART1_CONTROL  UCSR1B
+ #define UART1_DATA     UDR1
+ #define UART1_UDRIE    UDRIE1
+#elif defined(__AVR_ATmega64__) || defined(__AVR_ATmega128__) 
+ /* ATmega with two USART */
+ #define ATMEGA_USART0
+ #define ATMEGA_USART1
+ #define UART0_RECEIVE_INTERRUPT   SIG_UART0_RECV
+ #define UART1_RECEIVE_INTERRUPT   SIG_UART1_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_UART0_DATA
+ #define UART1_TRANSMIT_INTERRUPT  SIG_UART1_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+ #define UART1_STATUS   UCSR1A
+ #define UART1_CONTROL  UCSR1B
+ #define UART1_DATA     UDR1
+ #define UART1_UDRIE    UDRIE1
+#elif defined(__AVR_ATmega161__)
+ /* ATmega with UART */
+ #error "AVR ATmega161 currently not supported by this libaray !"
+#elif defined(__AVR_ATmega169__) 
+ /* ATmega with one USART */
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#elif defined(__AVR_ATmega48__) ||defined(__AVR_ATmega88__) || defined(__AVR_ATmega168__)
+ #define ATMEGA_USART0
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
+ #define UART0_STATUS   UCSR0A
+ #define UART0_CONTROL  UCSR0B
+ #define UART0_DATA     UDR0
+ #define UART0_UDRIE    UDRIE0
+#elif defined(__AVR_ATtiny2313__)
+ #define ATMEGA_USART
+ #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RX 
+ #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_UDRE
+ #define UART0_STATUS   UCSRA
+ #define UART0_CONTROL  UCSRB
+ #define UART0_DATA     UDR
+ #define UART0_UDRIE    UDRIE
+#else
+ #error "no UART definition for MCU available"
+#endif
+
+
+/*
+ *  module global variables
+ */
+static volatile unsigned char UART_TxBuf[UART_TX_BUFFER_SIZE];
+static volatile unsigned char UART_RxBuf[UART_RX_BUFFER_SIZE];
+static volatile unsigned char UART_TxHead;
+static volatile unsigned char UART_TxTail;
+static volatile unsigned char UART_RxHead;
+static volatile unsigned char UART_RxTail;
+static volatile unsigned char UART_LastRxError;
+
+#if defined( ATMEGA_USART1 )
+static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE];
+static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE];
+static volatile unsigned char UART1_TxHead;
+static volatile unsigned char UART1_TxTail;
+static volatile unsigned char UART1_RxHead;
+static volatile unsigned char UART1_RxTail;
+static volatile unsigned char UART1_LastRxError;
+#endif
+
+
+
+SIGNAL(UART0_RECEIVE_INTERRUPT)
+/*************************************************************************
+Function: UART Receive Complete interrupt
+Purpose:  called when the UART has received a character
+**************************************************************************/
+{
+    unsigned char tmphead;
+    unsigned char data;
+    unsigned char usr;
+    unsigned char lastRxError;
+ 
+ 
+    /* read UART status register and UART data register */ 
+    usr  = UART0_STATUS;
+    data = UART0_DATA;
+    
+    /* */
+#if defined( AT90_UART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#elif defined( ATMEGA_USART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#elif defined( ATMEGA_USART0 )
+    lastRxError = (usr & (_BV(FE0)|_BV(DOR0)) );
+#elif defined ( ATMEGA_UART )
+    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
+#endif
+        
+    /* calculate buffer index */ 
+    tmphead = ( UART_RxHead + 1) & UART_RX_BUFFER_MASK;
+    
+    if ( tmphead == UART_RxTail ) {
+        /* error: receive buffer overflow */
+        lastRxError = UART_BUFFER_OVERFLOW >> 8;
+    }else{
+        /* store new index */
+        UART_RxHead = tmphead;
+        /* store received data in buffer */
+        UART_RxBuf[tmphead] = data;
+    }
+    UART_LastRxError = lastRxError;   
+}
+
+
+SIGNAL(UART0_TRANSMIT_INTERRUPT)
+/*************************************************************************
+Function: UART Data Register Empty interrupt
+Purpose:  called when the UART is ready to transmit the next byte
+**************************************************************************/
+{
+    unsigned char tmptail;
+
+    
+    if ( UART_TxHead != UART_TxTail) {
+        /* calculate and store new buffer index */
+        tmptail = (UART_TxTail + 1) & UART_TX_BUFFER_MASK;
+        UART_TxTail = tmptail;
+        /* get one byte from buffer and write it to UART */
+        UART0_DATA = UART_TxBuf[tmptail];  /* start transmission */
+    }else{
+        /* tx buffer empty, disable UDRE interrupt */
+        UART0_CONTROL &= ~_BV(UART0_UDRIE);
+    }
+}
+
+
+/*************************************************************************
+Function: uart_init()
+Purpose:  initialize UART and set baudrate
+Input:    baudrate using macro UART_BAUD_SELECT()
+Returns:  none
+**************************************************************************/
+void uart_init(unsigned int baudrate)
+{
+    UART_TxHead = 0;
+    UART_TxTail = 0;
+    UART_RxHead = 0;
+    UART_RxTail = 0;
+    
+#if defined( AT90_UART )
+    /* set baud rate */
+    UBRR = (unsigned char)baudrate; 
+
+    /* enable UART receiver and transmmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|_BV(RXEN)|_BV(TXEN);
+
+#elif defined (ATMEGA_USART)
+    /* Set baud rate */
+    if ( baudrate & 0x8000 )
+    {
+    	 UART0_STATUS = (1<<U2X);  //Enable 2x speed 
+    	 baudrate &= ~0x8000;
+    }
+    UBRRH = (unsigned char)(baudrate>>8);
+    UBRRL = (unsigned char) baudrate;
+   
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
+    #ifdef URSEL
+    UCSRC = (1<<URSEL)|(3<<UCSZ0);
+    #else
+    UCSRC = (3<<UCSZ0);
+    #endif 
+    
+#elif defined (ATMEGA_USART0 )
+    /* Set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+   		UART0_STATUS = (1<<U2X0);  //Enable 2x speed 
+   		baudrate &= ~0x8000;
+   	}
+    UBRR0H = (unsigned char)(baudrate>>8);
+    UBRR0L = (unsigned char) baudrate;
+
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE0)|(1<<RXEN0)|(1<<TXEN0);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
+    #ifdef URSEL0
+    UCSR0C = (1<<URSEL0)|(3<<UCSZ00);
+    #else
+    UCSR0C = (3<<UCSZ00);
+    #endif 
+
+#elif defined ( ATMEGA_UART )
+    /* set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+    	UART0_STATUS = (1<<U2X);  //Enable 2x speed 
+    	baudrate &= ~0x8000;
+    }
+    UBRRHI = (unsigned char)(baudrate>>8);
+    UBRR   = (unsigned char) baudrate;
+
+    /* Enable UART receiver and transmitter and receive complete interrupt */
+    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
+
+#endif
+
+}/* uart_init */
+
+
+/*************************************************************************
+Function: uart_getc()
+Purpose:  return byte from ringbuffer  
+Returns:  lower byte:  received byte from ringbuffer
+          higher byte: last receive error
+**************************************************************************/
+unsigned int uart_getc(void)
+{    
+    unsigned char tmptail;
+    unsigned char data;
+
+
+    if ( UART_RxHead == UART_RxTail ) {
+        return UART_NO_DATA;   /* no data available */
+    }
+    
+    /* calculate /store buffer index */
+    tmptail = (UART_RxTail + 1) & UART_RX_BUFFER_MASK;
+    UART_RxTail = tmptail; 
+    
+    /* get data from receive buffer */
+    data = UART_RxBuf[tmptail];
+    
+    return (UART_LastRxError << 8) + data;
+
+}/* uart_getc */
+
+
+/*************************************************************************
+Function: uart_putc()
+Purpose:  write byte to ringbuffer for transmitting via UART
+Input:    byte to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart_putc(unsigned char data)
+{
+    unsigned char tmphead;
+
+    
+    tmphead  = (UART_TxHead + 1) & UART_TX_BUFFER_MASK;
+    
+    while ( tmphead == UART_TxTail ){
+        ;/* wait for free space in buffer */
+    }
+    
+    UART_TxBuf[tmphead] = data;
+    UART_TxHead = tmphead;
+
+    /* enable UDRE interrupt */
+    UART0_CONTROL    |= _BV(UART0_UDRIE);
+
+}/* uart_putc */
+
+
+/*************************************************************************
+Function: uart_puts()
+Purpose:  transmit string to UART
+Input:    string to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart_puts(const char *s )
+{
+    while (*s) 
+      uart_putc(*s++);
+
+}/* uart_puts */
+
+
+/*************************************************************************
+Function: uart_puts_p()
+Purpose:  transmit string from program memory to UART
+Input:    program memory string to be transmitted
+Returns:  none
+**************************************************************************/
+void uart_puts_p(const char *progmem_s )
+{
+    register char c;
+    
+    while ( (c = pgm_read_byte(progmem_s++)) ) 
+      uart_putc(c);
+
+}/* uart_puts_p */
+
+
+/*
+ * these functions are only for ATmegas with two USART
+ */
+#if defined( ATMEGA_USART1 )
+
+SIGNAL(UART1_RECEIVE_INTERRUPT)
+/*************************************************************************
+Function: UART1 Receive Complete interrupt
+Purpose:  called when the UART1 has received a character
+**************************************************************************/
+{
+    unsigned char tmphead;
+    unsigned char data;
+    unsigned char usr;
+    unsigned char lastRxError;
+ 
+ 
+    /* read UART status register and UART data register */ 
+    usr  = UART1_STATUS;
+    data = UART1_DATA;
+    
+    /* */
+    lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) );
+        
+    /* calculate buffer index */ 
+    tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK;
+    
+    if ( tmphead == UART1_RxTail ) {
+        /* error: receive buffer overflow */
+        lastRxError = UART_BUFFER_OVERFLOW >> 8;
+    }else{
+        /* store new index */
+        UART1_RxHead = tmphead;
+        /* store received data in buffer */
+        UART1_RxBuf[tmphead] = data;
+    }
+    UART1_LastRxError = lastRxError;   
+}
+
+
+SIGNAL(UART1_TRANSMIT_INTERRUPT)
+/*************************************************************************
+Function: UART1 Data Register Empty interrupt
+Purpose:  called when the UART1 is ready to transmit the next byte
+**************************************************************************/
+{
+    unsigned char tmptail;
+
+    
+    if ( UART1_TxHead != UART1_TxTail) {
+        /* calculate and store new buffer index */
+        tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK;
+        UART1_TxTail = tmptail;
+        /* get one byte from buffer and write it to UART */
+        UART1_DATA = UART1_TxBuf[tmptail];  /* start transmission */
+    }else{
+        /* tx buffer empty, disable UDRE interrupt */
+        UART1_CONTROL &= ~_BV(UART1_UDRIE);
+    }
+}
+
+
+/*************************************************************************
+Function: uart1_init()
+Purpose:  initialize UART1 and set baudrate
+Input:    baudrate using macro UART_BAUD_SELECT()
+Returns:  none
+**************************************************************************/
+void uart1_init(unsigned int baudrate)
+{
+    UART1_TxHead = 0;
+    UART1_TxTail = 0;
+    UART1_RxHead = 0;
+    UART1_RxTail = 0;
+    
+
+    /* Set baud rate */
+    if ( baudrate & 0x8000 ) 
+    {
+    	UART1_STATUS = (1<<U2X1);  //Enable 2x speed 
+      baudrate &= ~0x8000;
+    }
+    UBRR1H = (unsigned char)(baudrate>>8);
+    UBRR1L = (unsigned char) baudrate;
+
+    /* Enable USART receiver and transmitter and receive complete interrupt */
+    UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1);
+    
+    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */   
+    #ifdef URSEL1
+    UCSR1C = (1<<URSEL1)|(3<<UCSZ10);
+    #else
+    UCSR1C = (3<<UCSZ10);
+    #endif 
+}/* uart_init */
+
+
+/*************************************************************************
+Function: uart1_getc()
+Purpose:  return byte from ringbuffer  
+Returns:  lower byte:  received byte from ringbuffer
+          higher byte: last receive error
+**************************************************************************/
+unsigned int uart1_getc(void)
+{    
+    unsigned char tmptail;
+    unsigned char data;
+
+
+    if ( UART1_RxHead == UART1_RxTail ) {
+        return UART_NO_DATA;   /* no data available */
+    }
+    
+    /* calculate /store buffer index */
+    tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK;
+    UART1_RxTail = tmptail; 
+    
+    /* get data from receive buffer */
+    data = UART1_RxBuf[tmptail];
+    
+    return (UART1_LastRxError << 8) + data;
+
+}/* uart1_getc */
+
+
+/*************************************************************************
+Function: uart1_putc()
+Purpose:  write byte to ringbuffer for transmitting via UART
+Input:    byte to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart1_putc(unsigned char data)
+{
+    unsigned char tmphead;
+
+    
+    tmphead  = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK;
+    
+    while ( tmphead == UART1_TxTail ){
+        ;/* wait for free space in buffer */
+    }
+    
+    UART1_TxBuf[tmphead] = data;
+    UART1_TxHead = tmphead;
+
+    /* enable UDRE interrupt */
+    UART1_CONTROL    |= _BV(UART1_UDRIE);
+
+}/* uart1_putc */
+
+
+/*************************************************************************
+Function: uart1_puts()
+Purpose:  transmit string to UART1
+Input:    string to be transmitted
+Returns:  none          
+**************************************************************************/
+void uart1_puts(const char *s )
+{
+    while (*s) 
+      uart1_putc(*s++);
+
+}/* uart1_puts */
+
+
+/*************************************************************************
+Function: uart1_puts_p()
+Purpose:  transmit string from program memory to UART1
+Input:    program memory string to be transmitted
+Returns:  none
+**************************************************************************/
+void uart1_puts_p(const char *progmem_s )
+{
+    register char c;
+    
+    while ( (c = pgm_read_byte(progmem_s++)) ) 
+      uart1_putc(c);
+
+}/* uart1_puts_p */
+
+
+#endif
diff --git a/source/Concept/Framework/atmega128io.cpp b/source/Concept/Framework/atmega128io.cpp
deleted file mode 100644
index b1d6f4d..0000000
--- a/source/Concept/Framework/atmega128io.cpp
+++ /dev/null
@@ -1,585 +0,0 @@
-#include "atmega128io.h"
-
-/*************************************************************************
-Title:    Interrupt UART library with receive/transmit circular buffers
-Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury
-File:     $Id: uart.c,v 1.5.2.10 2005/11/15 19:49:12 peter Exp $
-Software: AVR-GCC 3.3 
-Hardware: any AVR with built-in UART, 
-          tested on AT90S8515 at 4 Mhz and ATmega at 1Mhz
-
-DESCRIPTION:
-    An interrupt is generated when the UART has finished transmitting or
-    receiving a byte. The interrupt handling routines use circular buffers
-    for buffering received and transmitted data.
-    
-    The UART_RX_BUFFER_SIZE and UART_TX_BUFFER_SIZE variables define
-    the buffer size in bytes. Note that these variables must be a 
-    power of 2.
-    
-USAGE:
-    Refere to the header file uart.h for a description of the routines. 
-    See also example test_uart.c.
-
-NOTES:
-    Based on Atmel Application Note AVR306
-                    
-*************************************************************************/
-#include <avr/io.h>
-#include <avr/interrupt.h>
-//#include <avr/signal.h>
-#include <avr/pgmspace.h>
-
-/*
- *  constants and macros
- */
-
-/* size of RX/TX buffers */
-#define UART_RX_BUFFER_MASK ( UART_RX_BUFFER_SIZE - 1)
-#define UART_TX_BUFFER_MASK ( UART_TX_BUFFER_SIZE - 1)
-
-#if ( UART_RX_BUFFER_SIZE & UART_RX_BUFFER_MASK )
-#error RX buffer size is not a power of 2
-#endif
-#if ( UART_TX_BUFFER_SIZE & UART_TX_BUFFER_MASK )
-#error TX buffer size is not a power of 2
-#endif
-
-#if defined(__AVR_AT90S2313__) \
- || defined(__AVR_AT90S4414__) || defined(__AVR_AT90S4434__) \
- || defined(__AVR_AT90S8515__) || defined(__AVR_AT90S8535__) \
- || defined(__AVR_ATmega103__)
- /* old AVR classic or ATmega103 with one UART */
- #define AT90_UART
- #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
- #define UART0_STATUS   USR
- #define UART0_CONTROL  UCR
- #define UART0_DATA     UDR  
- #define UART0_UDRIE    UDRIE
-#elif defined(__AVR_AT90S2333__) || defined(__AVR_AT90S4433__)
- /* old AVR classic with one UART */
- #define AT90_UART
- #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
- #define UART0_STATUS   UCSRA
- #define UART0_CONTROL  UCSRB
- #define UART0_DATA     UDR 
- #define UART0_UDRIE    UDRIE
-#elif  defined(__AVR_ATmega8__)  || defined(__AVR_ATmega16__) || defined(__AVR_ATmega32__) \
-  || defined(__AVR_ATmega8515__) || defined(__AVR_ATmega8535__) \
-  || defined(__AVR_ATmega323__)
-  /* ATmega with one USART */
- #define ATMEGA_USART
- #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
- #define UART0_STATUS   UCSRA
- #define UART0_CONTROL  UCSRB
- #define UART0_DATA     UDR
- #define UART0_UDRIE    UDRIE
-#elif defined(__AVR_ATmega163__) 
-  /* ATmega163 with one UART */
- #define ATMEGA_UART
- #define UART0_RECEIVE_INTERRUPT   SIG_UART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_UART_DATA
- #define UART0_STATUS   UCSRA
- #define UART0_CONTROL  UCSRB
- #define UART0_DATA     UDR
- #define UART0_UDRIE    UDRIE
-#elif defined(__AVR_ATmega162__)
- /* ATmega with two USART */
- #define ATMEGA_USART0
- #define ATMEGA_USART1
- #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RECV
- #define UART1_RECEIVE_INTERRUPT   SIG_USART1_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_DATA
- #define UART1_TRANSMIT_INTERRUPT  SIG_USART1_DATA
- #define UART0_STATUS   UCSR0A
- #define UART0_CONTROL  UCSR0B
- #define UART0_DATA     UDR0
- #define UART0_UDRIE    UDRIE0
- #define UART1_STATUS   UCSR1A
- #define UART1_CONTROL  UCSR1B
- #define UART1_DATA     UDR1
- #define UART1_UDRIE    UDRIE1
-#elif defined(__AVR_ATmega64__) || defined(__AVR_ATmega128__) 
- /* ATmega with two USART */
- #define ATMEGA_USART0
- #define ATMEGA_USART1
- #define UART0_RECEIVE_INTERRUPT   SIG_UART0_RECV
- #define UART1_RECEIVE_INTERRUPT   SIG_UART1_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_UART0_DATA
- #define UART1_TRANSMIT_INTERRUPT  SIG_UART1_DATA
- #define UART0_STATUS   UCSR0A
- #define UART0_CONTROL  UCSR0B
- #define UART0_DATA     UDR0
- #define UART0_UDRIE    UDRIE0
- #define UART1_STATUS   UCSR1A
- #define UART1_CONTROL  UCSR1B
- #define UART1_DATA     UDR1
- #define UART1_UDRIE    UDRIE1
-#elif defined(__AVR_ATmega161__)
- /* ATmega with UART */
- #error "AVR ATmega161 currently not supported by this libaray !"
-#elif defined(__AVR_ATmega169__) 
- /* ATmega with one USART */
- #define ATMEGA_USART
- #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
- #define UART0_STATUS   UCSRA
- #define UART0_CONTROL  UCSRB
- #define UART0_DATA     UDR
- #define UART0_UDRIE    UDRIE
-#elif defined(__AVR_ATmega48__) ||defined(__AVR_ATmega88__) || defined(__AVR_ATmega168__)
- #define ATMEGA_USART0
- #define UART0_RECEIVE_INTERRUPT   SIG_USART_RECV
- #define UART0_TRANSMIT_INTERRUPT  SIG_USART_DATA
- #define UART0_STATUS   UCSR0A
- #define UART0_CONTROL  UCSR0B
- #define UART0_DATA     UDR0
- #define UART0_UDRIE    UDRIE0
-#elif defined(__AVR_ATtiny2313__)
- #define ATMEGA_USART
- #define UART0_RECEIVE_INTERRUPT   SIG_USART0_RX 
- #define UART0_TRANSMIT_INTERRUPT  SIG_USART0_UDRE
- #define UART0_STATUS   UCSRA
- #define UART0_CONTROL  UCSRB
- #define UART0_DATA     UDR
- #define UART0_UDRIE    UDRIE
-#else
- #error "no UART definition for MCU available"
-#endif
-
-
-/*
- *  module global variables
- */
-static volatile unsigned char UART_TxBuf[UART_TX_BUFFER_SIZE];
-static volatile unsigned char UART_RxBuf[UART_RX_BUFFER_SIZE];
-static volatile unsigned char UART_TxHead;
-static volatile unsigned char UART_TxTail;
-static volatile unsigned char UART_RxHead;
-static volatile unsigned char UART_RxTail;
-static volatile unsigned char UART_LastRxError;
-
-#if defined( ATMEGA_USART1 )
-static volatile unsigned char UART1_TxBuf[UART_TX_BUFFER_SIZE];
-static volatile unsigned char UART1_RxBuf[UART_RX_BUFFER_SIZE];
-static volatile unsigned char UART1_TxHead;
-static volatile unsigned char UART1_TxTail;
-static volatile unsigned char UART1_RxHead;
-static volatile unsigned char UART1_RxTail;
-static volatile unsigned char UART1_LastRxError;
-#endif
-
-
-
-SIGNAL(UART0_RECEIVE_INTERRUPT)
-/*************************************************************************
-Function: UART Receive Complete interrupt
-Purpose:  called when the UART has received a character
-**************************************************************************/
-{
-    unsigned char tmphead;
-    unsigned char data;
-    unsigned char usr;
-    unsigned char lastRxError;
- 
- 
-    /* read UART status register and UART data register */ 
-    usr  = UART0_STATUS;
-    data = UART0_DATA;
-    
-    /* */
-#if defined( AT90_UART )
-    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
-#elif defined( ATMEGA_USART )
-    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
-#elif defined( ATMEGA_USART0 )
-    lastRxError = (usr & (_BV(FE0)|_BV(DOR0)) );
-#elif defined ( ATMEGA_UART )
-    lastRxError = (usr & (_BV(FE)|_BV(DOR)) );
-#endif
-        
-    /* calculate buffer index */ 
-    tmphead = ( UART_RxHead + 1) & UART_RX_BUFFER_MASK;
-    
-    if ( tmphead == UART_RxTail ) {
-        /* error: receive buffer overflow */
-        lastRxError = UART_BUFFER_OVERFLOW >> 8;
-    }else{
-        /* store new index */
-        UART_RxHead = tmphead;
-        /* store received data in buffer */
-        UART_RxBuf[tmphead] = data;
-    }
-    UART_LastRxError = lastRxError;   
-}
-
-
-SIGNAL(UART0_TRANSMIT_INTERRUPT)
-/*************************************************************************
-Function: UART Data Register Empty interrupt
-Purpose:  called when the UART is ready to transmit the next byte
-**************************************************************************/
-{
-    unsigned char tmptail;
-
-    
-    if ( UART_TxHead != UART_TxTail) {
-        /* calculate and store new buffer index */
-        tmptail = (UART_TxTail + 1) & UART_TX_BUFFER_MASK;
-        UART_TxTail = tmptail;
-        /* get one byte from buffer and write it to UART */
-        UART0_DATA = UART_TxBuf[tmptail];  /* start transmission */
-    }else{
-        /* tx buffer empty, disable UDRE interrupt */
-        UART0_CONTROL &= ~_BV(UART0_UDRIE);
-    }
-}
-
-
-/*************************************************************************
-Function: uart_init()
-Purpose:  initialize UART and set baudrate
-Input:    baudrate using macro UART_BAUD_SELECT()
-Returns:  none
-**************************************************************************/
-void uart_init(unsigned int baudrate)
-{
-    UART_TxHead = 0;
-    UART_TxTail = 0;
-    UART_RxHead = 0;
-    UART_RxTail = 0;
-    
-#if defined( AT90_UART )
-    /* set baud rate */
-    UBRR = (unsigned char)baudrate; 
-
-    /* enable UART receiver and transmmitter and receive complete interrupt */
-    UART0_CONTROL = _BV(RXCIE)|_BV(RXEN)|_BV(TXEN);
-
-#elif defined (ATMEGA_USART)
-    /* Set baud rate */
-    if ( baudrate & 0x8000 )
-    {
-    	 UART0_STATUS = (1<<U2X);  //Enable 2x speed 
-    	 baudrate &= ~0x8000;
-    }
-    UBRRH = (unsigned char)(baudrate>>8);
-    UBRRL = (unsigned char) baudrate;
-   
-    /* Enable USART receiver and transmitter and receive complete interrupt */
-    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
-    
-    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
-    #ifdef URSEL
-    UCSRC = (1<<URSEL)|(3<<UCSZ0);
-    #else
-    UCSRC = (3<<UCSZ0);
-    #endif 
-    
-#elif defined (ATMEGA_USART0 )
-    /* Set baud rate */
-    if ( baudrate & 0x8000 ) 
-    {
-   		UART0_STATUS = (1<<U2X0);  //Enable 2x speed 
-   		baudrate &= ~0x8000;
-   	}
-    UBRR0H = (unsigned char)(baudrate>>8);
-    UBRR0L = (unsigned char) baudrate;
-
-    /* Enable USART receiver and transmitter and receive complete interrupt */
-    UART0_CONTROL = _BV(RXCIE0)|(1<<RXEN0)|(1<<TXEN0);
-    
-    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */
-    #ifdef URSEL0
-    UCSR0C = (1<<URSEL0)|(3<<UCSZ00);
-    #else
-    UCSR0C = (3<<UCSZ00);
-    #endif 
-
-#elif defined ( ATMEGA_UART )
-    /* set baud rate */
-    if ( baudrate & 0x8000 ) 
-    {
-    	UART0_STATUS = (1<<U2X);  //Enable 2x speed 
-    	baudrate &= ~0x8000;
-    }
-    UBRRHI = (unsigned char)(baudrate>>8);
-    UBRR   = (unsigned char) baudrate;
-
-    /* Enable UART receiver and transmitter and receive complete interrupt */
-    UART0_CONTROL = _BV(RXCIE)|(1<<RXEN)|(1<<TXEN);
-
-#endif
-
-}/* uart_init */
-
-
-/*************************************************************************
-Function: uart_getc()
-Purpose:  return byte from ringbuffer  
-Returns:  lower byte:  received byte from ringbuffer
-          higher byte: last receive error
-**************************************************************************/
-unsigned int uart_getc(void)
-{    
-    unsigned char tmptail;
-    unsigned char data;
-
-
-    if ( UART_RxHead == UART_RxTail ) {
-        return UART_NO_DATA;   /* no data available */
-    }
-    
-    /* calculate /store buffer index */
-    tmptail = (UART_RxTail + 1) & UART_RX_BUFFER_MASK;
-    UART_RxTail = tmptail; 
-    
-    /* get data from receive buffer */
-    data = UART_RxBuf[tmptail];
-    
-    return (UART_LastRxError << 8) + data;
-
-}/* uart_getc */
-
-
-/*************************************************************************
-Function: uart_putc()
-Purpose:  write byte to ringbuffer for transmitting via UART
-Input:    byte to be transmitted
-Returns:  none          
-**************************************************************************/
-void uart_putc(unsigned char data)
-{
-    unsigned char tmphead;
-
-    
-    tmphead  = (UART_TxHead + 1) & UART_TX_BUFFER_MASK;
-    
-    while ( tmphead == UART_TxTail ){
-        ;/* wait for free space in buffer */
-    }
-    
-    UART_TxBuf[tmphead] = data;
-    UART_TxHead = tmphead;
-
-    /* enable UDRE interrupt */
-    UART0_CONTROL    |= _BV(UART0_UDRIE);
-
-}/* uart_putc */
-
-
-/*************************************************************************
-Function: uart_puts()
-Purpose:  transmit string to UART
-Input:    string to be transmitted
-Returns:  none          
-**************************************************************************/
-void uart_puts(const char *s )
-{
-    while (*s) 
-      uart_putc(*s++);
-
-}/* uart_puts */
-
-
-/*************************************************************************
-Function: uart_puts_p()
-Purpose:  transmit string from program memory to UART
-Input:    program memory string to be transmitted
-Returns:  none
-**************************************************************************/
-void uart_puts_p(const char *progmem_s )
-{
-    register char c;
-    
-    while ( (c = pgm_read_byte(progmem_s++)) ) 
-      uart_putc(c);
-
-}/* uart_puts_p */
-
-
-/*
- * these functions are only for ATmegas with two USART
- */
-#if defined( ATMEGA_USART1 )
-
-SIGNAL(UART1_RECEIVE_INTERRUPT)
-/*************************************************************************
-Function: UART1 Receive Complete interrupt
-Purpose:  called when the UART1 has received a character
-**************************************************************************/
-{
-    unsigned char tmphead;
-    unsigned char data;
-    unsigned char usr;
-    unsigned char lastRxError;
- 
- 
-    /* read UART status register and UART data register */ 
-    usr  = UART1_STATUS;
-    data = UART1_DATA;
-    
-    /* */
-    lastRxError = (usr & (_BV(FE1)|_BV(DOR1)) );
-        
-    /* calculate buffer index */ 
-    tmphead = ( UART1_RxHead + 1) & UART_RX_BUFFER_MASK;
-    
-    if ( tmphead == UART1_RxTail ) {
-        /* error: receive buffer overflow */
-        lastRxError = UART_BUFFER_OVERFLOW >> 8;
-    }else{
-        /* store new index */
-        UART1_RxHead = tmphead;
-        /* store received data in buffer */
-        UART1_RxBuf[tmphead] = data;
-    }
-    UART1_LastRxError = lastRxError;   
-}
-
-
-SIGNAL(UART1_TRANSMIT_INTERRUPT)
-/*************************************************************************
-Function: UART1 Data Register Empty interrupt
-Purpose:  called when the UART1 is ready to transmit the next byte
-**************************************************************************/
-{
-    unsigned char tmptail;
-
-    
-    if ( UART1_TxHead != UART1_TxTail) {
-        /* calculate and store new buffer index */
-        tmptail = (UART1_TxTail + 1) & UART_TX_BUFFER_MASK;
-        UART1_TxTail = tmptail;
-        /* get one byte from buffer and write it to UART */
-        UART1_DATA = UART1_TxBuf[tmptail];  /* start transmission */
-    }else{
-        /* tx buffer empty, disable UDRE interrupt */
-        UART1_CONTROL &= ~_BV(UART1_UDRIE);
-    }
-}
-
-
-/*************************************************************************
-Function: uart1_init()
-Purpose:  initialize UART1 and set baudrate
-Input:    baudrate using macro UART_BAUD_SELECT()
-Returns:  none
-**************************************************************************/
-void uart1_init(unsigned int baudrate)
-{
-    UART1_TxHead = 0;
-    UART1_TxTail = 0;
-    UART1_RxHead = 0;
-    UART1_RxTail = 0;
-    
-
-    /* Set baud rate */
-    if ( baudrate & 0x8000 ) 
-    {
-    	UART1_STATUS = (1<<U2X1);  //Enable 2x speed 
-      baudrate &= ~0x8000;
-    }
-    UBRR1H = (unsigned char)(baudrate>>8);
-    UBRR1L = (unsigned char) baudrate;
-
-    /* Enable USART receiver and transmitter and receive complete interrupt */
-    UART1_CONTROL = _BV(RXCIE1)|(1<<RXEN1)|(1<<TXEN1);
-    
-    /* Set frame format: asynchronous, 8data, no parity, 1stop bit */   
-    #ifdef URSEL1
-    UCSR1C = (1<<URSEL1)|(3<<UCSZ10);
-    #else
-    UCSR1C = (3<<UCSZ10);
-    #endif 
-}/* uart_init */
-
-
-/*************************************************************************
-Function: uart1_getc()
-Purpose:  return byte from ringbuffer  
-Returns:  lower byte:  received byte from ringbuffer
-          higher byte: last receive error
-**************************************************************************/
-unsigned int uart1_getc(void)
-{    
-    unsigned char tmptail;
-    unsigned char data;
-
-
-    if ( UART1_RxHead == UART1_RxTail ) {
-        return UART_NO_DATA;   /* no data available */
-    }
-    
-    /* calculate /store buffer index */
-    tmptail = (UART1_RxTail + 1) & UART_RX_BUFFER_MASK;
-    UART1_RxTail = tmptail; 
-    
-    /* get data from receive buffer */
-    data = UART1_RxBuf[tmptail];
-    
-    return (UART1_LastRxError << 8) + data;
-
-}/* uart1_getc */
-
-
-/*************************************************************************
-Function: uart1_putc()
-Purpose:  write byte to ringbuffer for transmitting via UART
-Input:    byte to be transmitted
-Returns:  none          
-**************************************************************************/
-void uart1_putc(unsigned char data)
-{
-    unsigned char tmphead;
-
-    
-    tmphead  = (UART1_TxHead + 1) & UART_TX_BUFFER_MASK;
-    
-    while ( tmphead == UART1_TxTail ){
-        ;/* wait for free space in buffer */
-    }
-    
-    UART1_TxBuf[tmphead] = data;
-    UART1_TxHead = tmphead;
-
-    /* enable UDRE interrupt */
-    UART1_CONTROL    |= _BV(UART1_UDRIE);
-
-}/* uart1_putc */
-
-
-/*************************************************************************
-Function: uart1_puts()
-Purpose:  transmit string to UART1
-Input:    string to be transmitted
-Returns:  none          
-**************************************************************************/
-void uart1_puts(const char *s )
-{
-    while (*s) 
-      uart1_putc(*s++);
-
-}/* uart1_puts */
-
-
-/*************************************************************************
-Function: uart1_puts_p()
-Purpose:  transmit string from program memory to UART1
-Input:    program memory string to be transmitted
-Returns:  none
-**************************************************************************/
-void uart1_puts_p(const char *progmem_s )
-{
-    register char c;
-    
-    while ( (c = pgm_read_byte(progmem_s++)) ) 
-      uart1_putc(c);
-
-}/* uart1_puts_p */
-
-
-#endif
diff --git a/source/Concept/Framework/default/Makefile b/source/Concept/Framework/default/Makefile
index e8aa72d..d0fbfb0 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     -DF_CPU=16000000ULUL  -O3 -fsigned-char
+CFLAGS += -Wall -gdwarf-2 -O3 -fsigned-char 
 CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d 
 
 ## Assembly specific flags
@@ -33,8 +33,11 @@ HEX_EEPROM_FLAGS += --set-section-flags=.eeprom="alloc,load"
 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" 
+
 ## Objects that must be built in order to link
-OBJECTS = main.o sensor.o tools.o atmega128io.o display.o keyboard.o distance_sensor.o mouse_sensor.o engine.o dribbler.o io_module.o ir_sensor.o kicker.o led.o robot.o position_tracker.o 
+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 
 
 ## Objects explicitly added by the user
 LINKONLYOBJECTS = 
@@ -43,49 +46,58 @@ LINKONLYOBJECTS =
 all: $(TARGET) RoboCode.hex RoboCode.eep size
 
 ## Compile
-main.o: ../main.cpp
+io_module.o: ../modules/io_module.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+atmega128io.o: ../atmega128io.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+main.o: ../main.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+robot.o: ../robot.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-tools.o: ../tools.cpp
+tools.o: ../tools.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-io_module.o: ../io_module.cpp
+distance_sensor.o: ../modules/input/distance_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-robot.o: ../robot.cpp
+ir_sensor.o: ../modules/input/ir_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-sensor.o: ../sensor.cpp
+keyboard.o: ../modules/input/keyboard.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-engine.o: ../engine.cpp
+mouse_sensor.o: ../modules/input/mouse_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-led.o: ../led.cpp
+sensor.o: ../modules/input/sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-display.o: ../display.cpp
+position_tracker.o: ../modules/interpreter/position_tracker.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-keyboard.o: ../keyboard.cpp
+display.o: ../modules/output/display.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-ir_sensor.o: ../ir_sensor.cpp
+dribbler.o: ../modules/output/dribbler.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-distance_sensor.o: ../distance_sensor.cpp
+engine.o: ../modules/output/engine.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-mouse_sensor.o: ../mouse_sensor.cpp
+kicker.o: ../modules/output/kicker.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-kicker.o: ../kicker.cpp
+led.o: ../modules/output/led.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-dribbler.o: ../dribbler.cpp
+ball_tracker.o: ../modules/interpreter/ball_tracker.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-atmega128io.o: ../atmega128io.cpp
+navigator.o: ../modules/executor/navigator.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
 ##Link
diff --git a/source/Concept/Framework/defines.h b/source/Concept/Framework/defines.h
index 2bc59d9..73505e2 100644
--- a/source/Concept/Framework/defines.h
+++ b/source/Concept/Framework/defines.h
@@ -58,6 +58,7 @@
 //Constants
 #define SPEED_PER_PWM 1
 #define DISTANCE_PER_VALUE 1
+#define PI 3.14159265f
 
 //IO Module Names
 enum IOModuleNames
@@ -142,8 +143,24 @@ enum IOModuleNames
 
 	IO_POSITION_TRACKER_END,
 
+	//Position Tracker
+
+	IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END,
+
+	IO_BALL_TRACKER_MAIN,
+
+	IO_BALL_TRACKER_END,
+
+	//Navigators
+
+	IO_NAVIGATOR_START = IO_BALL_TRACKER_END,
+
+	IO_NAVIGATOR_MAIN,
+
+	IO_NAVIGATOR_END,
+
 	//General
-	IO_END = IO_POSITION_TRACKER_END,
+	IO_END = IO_NAVIGATOR_END,
 };
 
 #endif
diff --git a/source/Concept/Framework/display.cpp b/source/Concept/Framework/display.cpp
deleted file mode 100644
index 82fbb41..0000000
--- a/source/Concept/Framework/display.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "display.h"
diff --git a/source/Concept/Framework/distance_sensor.cpp b/source/Concept/Framework/distance_sensor.cpp
deleted file mode 100644
index c6c0582..0000000
--- a/source/Concept/Framework/distance_sensor.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#include "distance_sensor.h"
-
-//-----------------------------------------------------------------------------
-float Distance_Sensor::GetDistance()
-{
-	uint32 result = 0;
-
-	//Generate pulse
-	*hardwareDDR |= pin;//Set pin output
-	*hardwarePort |= pin;//Activate port
-	usleep(10);//Wait for 10µs
-	*hardwarePort &= ~pin;//Deactivate port
-	*hardwareDDR &= ~pin;//Set pin input
-
-	//Wait for response
-	while(!(*hardwarePin & pin));
-	
-	//Calculate duration of response
-	while(*hardwarePin & pin) 
-	{
-		result++;
-		asm volatile("nop");
-	}
-
-	return (float(result) * DISTANCE_PER_VALUE);
-}
diff --git a/source/Concept/Framework/dribbler.cpp b/source/Concept/Framework/dribbler.cpp
deleted file mode 100644
index 20557e4..0000000
--- a/source/Concept/Framework/dribbler.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "dribbler.h"
diff --git a/source/Concept/Framework/engine.cpp b/source/Concept/Framework/engine.cpp
deleted file mode 100644
index 5c14c17..0000000
--- a/source/Concept/Framework/engine.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "engine.h"
diff --git a/source/Concept/Framework/io_module.cpp b/source/Concept/Framework/io_module.cpp
deleted file mode 100644
index 3549af5..0000000
--- a/source/Concept/Framework/io_module.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "io_module.h"
diff --git a/source/Concept/Framework/ir_sensor.cpp b/source/Concept/Framework/ir_sensor.cpp
deleted file mode 100644
index c34feed..0000000
--- a/source/Concept/Framework/ir_sensor.cpp
+++ /dev/null
@@ -1,9 +0,0 @@
-#include "ir_sensor.h"
-
-//-----------------------------------------------------------------------------
-uint16 IR_Sensor::GetIRIntensity()
-{
-	if(!parent) return 0;
-
-	return parent->GetADCValue(channel);
-}
diff --git a/source/Concept/Framework/keyboard.cpp b/source/Concept/Framework/keyboard.cpp
deleted file mode 100644
index 65fc7cf..0000000
--- a/source/Concept/Framework/keyboard.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "keyboard.h"
diff --git a/source/Concept/Framework/kicker.cpp b/source/Concept/Framework/kicker.cpp
deleted file mode 100644
index 6670efb..0000000
--- a/source/Concept/Framework/kicker.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "kicker.h"
diff --git a/source/Concept/Framework/led.cpp b/source/Concept/Framework/led.cpp
deleted file mode 100644
index 687b2db..0000000
--- a/source/Concept/Framework/led.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "led.h"
diff --git a/source/Concept/Framework/main.c b/source/Concept/Framework/main.c
new file mode 100755
index 0000000..fa6d718
--- /dev/null
+++ b/source/Concept/Framework/main.c
@@ -0,0 +1,189 @@
+#include "stdafx.h"
+
+int main()
+{
+	//Init our robot
+	Robot* localRobot = new Robot();
+
+	//Init Engines
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		Engine* newEngine = new Engine(i);
+		localRobot->AddModule(newEngine);
+		newEngine = NULL;
+	}
+
+	//Init Dribbler
+	for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
+	{
+		Dribbler* newDribbler = new Dribbler(i);
+		localRobot->AddModule(newDribbler);
+		newDribbler = NULL;
+	}
+
+	//Init Kicker
+	for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
+	{
+		Kicker* newKicker = new Kicker(i);
+		localRobot->AddModule(newKicker);
+		newKicker = NULL;
+	}
+
+	//Init Sensors
+	for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
+	{
+		switch(i)
+		{
+			//Create correct type of sensor
+			case IO_SENSOR_IR_0_DEG:
+			case IO_SENSOR_IR_30_DEG:
+			case IO_SENSOR_IR_60_DEG:
+			case IO_SENSOR_IR_100_DEG:
+			case IO_SENSOR_IR_180_DEG:
+			case IO_SENSOR_IR_260_DEG:
+			case IO_SENSOR_IR_300_DEG:
+			case IO_SENSOR_IR_330_DEG:
+			{
+				IR_Sensor* newSensor = new IR_Sensor(i);
+				localRobot->AddModule(newSensor);
+				newSensor = NULL;
+				break;
+			}
+			case IO_SENSOR_DISTANCE_0_DEG:
+			case IO_SENSOR_DISTANCE_90_DEG:
+			case IO_SENSOR_DISTANCE_180_DEG:
+			case IO_SENSOR_DISTANCE_270_DEG:
+			{
+				Distance_Sensor* newSensor = new Distance_Sensor(i);
+				localRobot->AddModule(newSensor);
+				newSensor = NULL;
+				break;
+			}
+			case IO_SENSOR_MOUSE_LEFT:
+			case IO_SENSOR_MOUSE_RIGHT:
+			{
+				Mouse_Sensor* newSensor = new Mouse_Sensor(i);
+				localRobot->AddModule(newSensor);
+				newSensor = NULL;
+				break;
+			}
+			//Other cases
+			default:
+			{
+				Sensor* newSensor = new Sensor(i);
+				localRobot->AddModule(newSensor);
+				newSensor = NULL;
+				break;
+			}
+		}
+	}
+
+	//Init Leds
+	for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
+	{
+		Led* newLed = new Led(i);
+		localRobot->AddModule(newLed);
+		newLed = NULL;
+	}
+
+	//Init Displays
+	for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
+	{
+		Display* newDisplay = new Display(i);
+		localRobot->AddModule(newDisplay);
+		newDisplay = NULL;
+	}
+
+	//Init Keyboards
+	for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
+	{
+		Keyboard* newKeyboard = new Keyboard(i);
+		localRobot->AddModule(newKeyboard);
+		newKeyboard = NULL;
+	}
+
+	//Init Position Tracker
+	for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
+	{
+		Position_Tracker* newPositionTracker = new Position_Tracker(i);
+		localRobot->AddModule(newPositionTracker);
+		newPositionTracker = NULL;
+	}
+
+	//Init Ball Tracker
+	for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
+	{
+		Ball_Tracker* newBallTracker = new Ball_Tracker(i);
+		localRobot->AddModule(newBallTracker);
+		newBallTracker = NULL;
+	}
+
+	//Init Navigators
+	for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
+	{
+		Navigator* newNavigator = new Navigator(i);
+		localRobot->AddModule(newNavigator);
+		newNavigator = NULL;
+	}
+
+	
+
+	IR_Sensor* ourSensor = NULL;
+	uint16 value = 0;
+	int8 value2 = 0;
+	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
+	uint32 i = 1;
+	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+
+	//Run
+	while(true)
+	{
+		msleep(500);
+		ourDisplay->Clear();
+		ourDisplay->Print(i++);
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value, 1, 2);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value, 1, 3);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value);
+		ourDisplay->Print(";");
+		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
+		value = ourSensor->GetIRIntensity();
+		ourDisplay->Print(value);
+		ourDisplay->Print(";");
+		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);
+
+		localRobot->Update();
+
+		ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
+	}
+
+	//Cleanup
+	delete localRobot;
+	localRobot = NULL;
+}
diff --git a/source/Concept/Framework/main.cpp b/source/Concept/Framework/main.cpp
deleted file mode 100644
index f689d88..0000000
--- a/source/Concept/Framework/main.cpp
+++ /dev/null
@@ -1,158 +0,0 @@
-#include "stdafx.h"
-
-int main()
-{
-	//Init our robot
-	Robot* localRobot = new Robot();
-
-	//Init Engines
-	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
-	{
-		Engine* newEngine = new Engine(i);
-		localRobot->AddModule(newEngine);
-		newEngine = NULL;
-	}
-
-	//Init Dribbler
-	for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
-	{
-		Dribbler* newDribbler = new Dribbler(i);
-		localRobot->AddModule(newDribbler);
-		newDribbler = NULL;
-	}
-
-	//Init Kicker
-	for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
-	{
-		Kicker* newKicker = new Kicker(i);
-		localRobot->AddModule(newKicker);
-		newKicker = NULL;
-	}
-
-	//Init Sensors
-	for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
-	{
-		switch(i)
-		{
-			//Create correct type of sensor
-			case IO_SENSOR_IR_0_DEG:
-			case IO_SENSOR_IR_30_DEG:
-			case IO_SENSOR_IR_60_DEG:
-			case IO_SENSOR_IR_100_DEG:
-			case IO_SENSOR_IR_180_DEG:
-			case IO_SENSOR_IR_260_DEG:
-			case IO_SENSOR_IR_300_DEG:
-			case IO_SENSOR_IR_330_DEG:
-			{
-				IR_Sensor* newSensor = new IR_Sensor(i);
-				localRobot->AddModule(newSensor);
-				newSensor = NULL;
-				break;
-			}
-			case IO_SENSOR_DISTANCE_0_DEG:
-			case IO_SENSOR_DISTANCE_90_DEG:
-			case IO_SENSOR_DISTANCE_180_DEG:
-			case IO_SENSOR_DISTANCE_270_DEG:
-			{
-				Distance_Sensor* newSensor = new Distance_Sensor(i);
-				localRobot->AddModule(newSensor);
-				newSensor = NULL;
-				break;
-			}
-			case IO_SENSOR_MOUSE_LEFT:
-			case IO_SENSOR_MOUSE_RIGHT:
-			{
-				Mouse_Sensor* newSensor = new Mouse_Sensor(i);
-				localRobot->AddModule(newSensor);
-				newSensor = NULL;
-				break;
-			}
-			//Other cases
-			default:
-			{
-				Sensor* newSensor = new Sensor(i);
-				localRobot->AddModule(newSensor);
-				newSensor = NULL;
-				break;
-			}
-		}
-	}
-
-	//Init Leds
-	for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
-	{
-		Led* newLed = new Led(i);
-		localRobot->AddModule(newLed);
-		newLed = NULL;
-	}
-
-	//Init Displays
-	for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
-	{
-		Display* newDisplay = new Display(i);
-		localRobot->AddModule(newDisplay);
-		newDisplay = NULL;
-	}
-
-	//Init Keyboards
-	for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
-	{
-		Keyboard* newKeyboard = new Keyboard(i);
-		localRobot->AddModule(newKeyboard);
-		newKeyboard = NULL;
-	}
-
-	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
-	IR_Sensor* ourSensor = NULL;
-	uint16 value = 0;
-	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
-	uint32 i = 1;
-
-	//Run
-	while(true)
-	{
-		msleep(500);
-		ourDisplay->Clear();
-		ourDisplay->Print(i++);
-		ourDisplay->NewLine();
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value, 1, 2);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value, 1, 3);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourDisplay->Print(ourKeyboard->GetInput(), 1, 4);
-
-		localRobot->Update();
-	}
-
-	//Cleanup
-	delete localRobot;
-	localRobot = NULL;
-}
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
new file mode 100755
index 0000000..459d4c1
--- /dev/null
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -0,0 +1,199 @@
+#include "navigator.h"
+
+//-----------------------------------------------------------------------------
+void Navigator::Stop()
+{	
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+	this->rotationSpeed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(0);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Rotate(float rotationSpeed)
+{
+	this->rotationSpeed = rotationSpeed;
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	bool hasDistanceToDrive = true;
+	bool hasOrientationToAdjust = true;
+
+	//Check for distance to drive
+	if((targetX >= 0) != (targetY >= 0))
+	{
+		targetX = -1.0f;
+		targetY = -1.0f;
+
+		hasDistanceToDrive = false;
+	}
+	else if(targetX >= 0 && targetY >= 0)
+	{
+		if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+		{
+			targetX = -1.0f;
+			targetY = -1.0f;
+
+			hasDistanceToDrive = false;
+		}
+		else
+		{
+			hasDistanceToDrive = true;
+		}
+	}
+	else
+	{
+		if(direction >= 0)
+		{
+			hasDistanceToDrive = true;
+		}
+		else
+		{
+			hasDistanceToDrive = false;
+		}
+	}
+
+	//Check for orientation to adjust
+	if(targetAngle >= 0)
+	{
+		if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
+		{
+			hasOrientationToAdjust = false;
+		}
+		else
+		{
+			hasOrientationToAdjust = true;
+		}
+	}
+	else
+	{
+		if(rotationSpeed != 0)
+		{
+			hasOrientationToAdjust = true;
+		}
+		else
+		{
+			hasOrientationToAdjust = false;
+		}
+	}
+
+	//Calculate directional/rotational engine speed
+	if(hasDistanceToDrive)
+	{
+
+
+		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
+
+		if(speed > maxRobotSpeed)
+		{
+			speed = maxRobotSpeed;
+		}
+
+		maxMotorSpeed = speed / (sqrt(2) / 2);
+
+
+
+		relAngel = direction - orientation
+		
+		robotSpeed = sin(45) * maxMotorSpeed
+		maxMotorSpeed = robotSpeed / sin(45)
+
+		if(relAngel > 45)
+		{
+			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+
+			back = speed / sin(relAngel);
+		}
+		else
+		{
+			
+		}
+
+		left = 
+		back = sin(relAngel) * speed
+
+
+		direction = 0:
+		orientation = 0:
+		left = speed
+		right = -speed
+		back = 0
+
+		direction = 0:
+		orientation = 90:
+		left = speed
+		right = speed
+		back = (sinVcos)(45) * speed
+
+		direction = 0:
+		orientation = 45:
+		left = speed
+		right = 0
+		back = -speed
+
+		direction = 0:
+		orientation = 180:
+		left = -speed
+		right = speed
+		back = 0
+
+	}
+	else if(!hasOrientationToAdjust)
+	{
+		Stop();
+	}
+	else
+	{
+	}
+}
+
+// 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
new file mode 100755
index 0000000..ed8b3c8
--- /dev/null
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -0,0 +1,53 @@
+#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+	Navigator()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+	Navigator(uint32 navigatorId)
+	{
+		this->parent = NULL;
+		this->moduleId = navigatorId;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+protected:
+	float direction; 
+	float targetAngle;
+	float targetX;
+	float targetY;
+	float speed;
+	float rotationSpeed;
+
+public:
+	void Update();	
+
+	void Stop();
+
+	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
+
+	void DriveTo(float newX, float newY, float newAngle);
+
+	void Rotate(float rotationSpeed);
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/input/distance_sensor.c b/source/Concept/Framework/modules/input/distance_sensor.c
new file mode 100755
index 0000000..977784f
--- /dev/null
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@@ -0,0 +1,32 @@
+#include "distance_sensor.h"
+
+//-----------------------------------------------------------------------------
+float Distance_Sensor::GetDistance()
+{
+	uint32 result = 0;
+
+	//Generate pulse
+	*hardwareDDR |= pin;//Set pin output
+	*hardwarePort |= pin;//Activate port
+	usleep(10);//Wait for 10µs
+	//*hardwarePort &= ~pin;//Deactivate port
+	*hardwareDDR &= ~pin;//Set pin input
+
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
+
+	//Wait for response
+	while(!(PINC & pin)){asm volatile("nop");}
+
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
+	
+	//Calculate duration of response
+	while(*hardwarePin & pin) 
+	{
+		result++;
+		asm volatile("nop");
+	}
+
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
+
+	return (float(result) * DISTANCE_PER_VALUE);
+}
diff --git a/source/Concept/Framework/modules/input/distance_sensor.h b/source/Concept/Framework/modules/input/distance_sensor.h
new file mode 100755
index 0000000..8e6aa21
--- /dev/null
+++ b/source/Concept/Framework/modules/input/distance_sensor.h
@@ -0,0 +1,71 @@
+#ifndef _DISTANCE_SENSOR_H
+#define _DISTANCE_SENSOR_H
+
+#include "../../stdafx.h"
+#include "sensor.h"
+
+class Distance_Sensor : public Sensor
+{
+public:
+	Distance_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->hardwareDDR = NULL;
+		this->hardwarePin = NULL;
+		this->pin = 0;
+	}
+
+	Distance_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_DISTANCE_0_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 0);
+				break;
+			case IO_SENSOR_DISTANCE_90_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 1);
+				break;
+			case IO_SENSOR_DISTANCE_180_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 2);
+				break;
+			case IO_SENSOR_DISTANCE_270_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 3);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->hardwareDDR = NULL;
+				this->hardwarePin = NULL;
+				this->pin = 0;
+				break;
+		}
+	}
+
+protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* hardwareDDR;
+	volatile uint8* hardwarePin;
+	uint8 pin;
+
+public:
+	float GetDistance();
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/input/ir_sensor.c b/source/Concept/Framework/modules/input/ir_sensor.c
new file mode 100755
index 0000000..c34feed
--- /dev/null
+++ b/source/Concept/Framework/modules/input/ir_sensor.c
@@ -0,0 +1,9 @@
+#include "ir_sensor.h"
+
+//-----------------------------------------------------------------------------
+uint16 IR_Sensor::GetIRIntensity()
+{
+	if(!parent) return 0;
+
+	return parent->GetADCValue(channel);
+}
diff --git a/source/Concept/Framework/modules/input/ir_sensor.h b/source/Concept/Framework/modules/input/ir_sensor.h
new file mode 100755
index 0000000..30e6ea4
--- /dev/null
+++ b/source/Concept/Framework/modules/input/ir_sensor.h
@@ -0,0 +1,62 @@
+#ifndef _IR_SENSOR_H
+#define _IR_SENSOR_H
+
+#include "../../defines.h"
+#include "../../robot.h"
+#include "sensor.h"
+
+class IR_Sensor : public Sensor
+{
+public:
+	IR_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	IR_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_IR_0_DEG:
+				this->channel = 0;
+				break;
+			case IO_SENSOR_IR_30_DEG:
+				this->channel = 1;
+				break;
+			case IO_SENSOR_IR_60_DEG:
+				this->channel = 2;
+				break;
+			case IO_SENSOR_IR_100_DEG:
+				this->channel = 3;
+				break;
+			case IO_SENSOR_IR_180_DEG:
+				this->channel = 4;
+				break;
+			case IO_SENSOR_IR_260_DEG:
+				this->channel = 5;
+				break;
+			case IO_SENSOR_IR_300_DEG:
+				this->channel = 6;
+				break;
+			case IO_SENSOR_IR_330_DEG:
+				this->channel = 7;
+				break;
+			default:
+				this->channel = 8;
+				break;
+		}
+	}
+
+protected:
+	//Hardware
+	uint8 channel;
+
+public:
+	uint16 GetIRIntensity();
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/input/keyboard.c b/source/Concept/Framework/modules/input/keyboard.c
new file mode 100755
index 0000000..65fc7cf
--- /dev/null
+++ b/source/Concept/Framework/modules/input/keyboard.c
@@ -0,0 +1 @@
+#include "keyboard.h"
diff --git a/source/Concept/Framework/modules/input/keyboard.h b/source/Concept/Framework/modules/input/keyboard.h
new file mode 100755
index 0000000..243bb6c
--- /dev/null
+++ b/source/Concept/Framework/modules/input/keyboard.h
@@ -0,0 +1,87 @@
+#ifndef _KEYBOARD_H
+#define _KEYBOARD_H
+
+#include "../../stdafx.h"
+
+class Keyboard : public IO_Module
+{
+public:
+	Keyboard()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->commandSetting = 0;
+		this->settingClearBuffer = 0;
+	}
+
+	Keyboard(uint32 keyboardId)
+	{
+		this->parent = NULL;
+		this->moduleId = keyboardId;
+		
+		switch(keyboardId)
+		{
+			case IO_KEYBOARD_MAIN:
+				this->commandSetting = 27;
+				this->settingClearBuffer = 123;
+				break;
+			default:
+				this->commandSetting = 0;
+				this->settingClearBuffer = 0;
+				break;
+		}
+	}
+
+protected:
+	//Commands
+	uint8 commandSetting;
+	//Settings
+	uint8 settingClearBuffer;
+
+	void SendCommand(uint8 newCommand)
+	{
+		switch(moduleId)
+		{
+			case IO_KEYBOARD_MAIN:
+				uart1_putc(newCommand);
+				break;
+			default:
+				break;
+		}
+	}
+
+public:
+	uint8 GetInput()
+	{
+		uint16 input = uart1_getc();
+
+		if(input == 0x100)//no data
+		{
+			return 0xEE;//empty
+		}
+		else if(input >= '0' && input <= '9')
+		{
+			return (uint8)(input - '0');
+		}
+		else if(input == '*')
+		{
+			return 10;
+		}
+		else if(input == '#')
+		{
+			return 11;
+		}
+		else
+		{
+			return 0xFF;//unknown
+		}
+	}
+
+	void ClearKeyBuffer()
+	{
+		SendCommand(commandSetting);
+		SendCommand(settingClearBuffer);
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/input/mouse_sensor.c b/source/Concept/Framework/modules/input/mouse_sensor.c
new file mode 100755
index 0000000..214a13e
--- /dev/null
+++ b/source/Concept/Framework/modules/input/mouse_sensor.c
@@ -0,0 +1 @@
+#include "mouse_sensor.h"
diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h
new file mode 100755
index 0000000..30f9d53
--- /dev/null
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@@ -0,0 +1,199 @@
+#ifndef _MOUSE_SENSOR_H
+#define _MOUSE_SENSOR_H
+
+#include "../../stdafx.h"
+#include "sensor.h"
+
+class Mouse_Sensor : public Sensor
+{
+public:
+	Mouse_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->hardwareDDR = NULL;
+		this->hardwarePin = NULL;
+		this->pinSDA = 0;
+		this->pinSCK = 0;
+		this->registerConfig = 0;
+		this->registerPixelData = 0;
+		this->registerSqual = 0;
+		this->registerDeltaX = 0;
+		this->registerDeltaY = 0;
+		this->configReset = 0;
+		this->configAwake = 0;
+		this->newImage = false;
+	}
+
+	Mouse_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+		this->newImage = false;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_MOUSE_LEFT:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pinSDA = (1 << 4);
+				this->pinSCK = (1 << 6);
+				this->registerConfig = 0x00;
+				this->registerPixelData = 0x08;
+				this->registerSqual = 0x4;
+				this->registerDeltaX = 0x3;
+				this->registerDeltaY = 0x2;
+				this->configReset = 0x80;
+				this->configAwake = 0x01;
+				break;
+			case IO_SENSOR_MOUSE_RIGHT:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pinSDA = (1 << 5);
+				this->pinSCK = (1 << 7);
+				this->registerConfig = 0x00;
+				this->registerPixelData = 0x08;
+				this->registerSqual = 0x4;
+				this->registerDeltaX = 0x3;
+				this->registerDeltaY = 0x2;
+				this->configReset = 0x80;
+				this->configAwake = 0x01;
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->hardwareDDR = NULL;
+				this->hardwarePin = NULL;
+				this->pinSDA = 0;
+				this->pinSCK = 0;
+				this->registerConfig = 0;
+				this->registerPixelData = 0;
+				this->registerSqual = 0;
+				this->registerDeltaX = 0;
+				this->registerDeltaY = 0;
+				this->configReset = 0;
+				this->configAwake = 0;
+				break;
+		}
+
+		*hardwareDDR  |= pinSCK;
+		*hardwarePort &= ~pinSCK;
+	
+		Write(registerConfig, configReset);
+		Write(registerConfig, configAwake);
+	}
+
+protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* hardwareDDR;
+	volatile uint8* hardwarePin;
+	uint8 pinSDA;
+	uint8 pinSCK;
+	bool newImage;
+	//Registers and Settings
+	uint8 registerConfig;
+	uint8 registerPixelData;
+	uint8 registerSqual;
+	uint8 registerDeltaX;
+	uint8 registerDeltaY;
+	uint8 configReset;
+	uint8 configAwake;
+
+public:
+	void WriteByte(uint8 newByte)
+	{
+		*hardwareDDR |= pinSDA;//Set SDA output
+	
+		for(uint8 i = 0; i < 8; i++)
+		{
+			*hardwarePort &= ~pinSCK;//prepare SCK
+		
+			//write data
+			*hardwarePort = (*hardwarePort & (~(*hardwarePin))) | 
+				((newByte >> 7) * pinSDA);
+
+			newByte = newByte << 1;//prepare next byte
+			asm volatile("nop");
+		
+			*hardwarePort |= pinSCK;
+		}
+	}
+
+	void Write(int8 adr, uint8 data)
+	{	
+		WriteByte(adr | 0x80);
+		WriteByte(data);
+		usleep(100);
+	}
+
+	uint8 ReadByte()
+	{
+		uint8 data=0;
+
+		*hardwareDDR  &= ~pinSDA;//Set SDA input
+
+		for(uint8 i = 0; i < 8; i++)
+		{
+			*hardwarePort &= ~pinSCK;//Prepare data
+			data = data << 1;
+
+			asm volatile("nop");
+			*hardwarePort |= pinSCK;//Prepare for reading
+
+			data |= (*hardwarePin & pinSDA) / pinSDA;
+		}
+
+		return data;
+	}
+
+	uint8 Read(uint8 adr)
+	{
+		WriteByte(adr);
+		usleep(100);
+	
+		return ReadByte();
+	}
+
+	void ImagePrepare()
+	{
+		Write(registerConfig, configAwake);
+		Write(registerPixelData, 0x00);
+
+		newImage = true;
+	}
+
+	uint8 ImageRead()
+	{
+		uint8 pixel = Read(registerPixelData);
+		if(newImage)
+		{
+			while (!(pixel & 0x80))//0x80 indicates first pixel
+			{
+				pixel=Read(registerPixelData);
+			}
+			newImage = false;
+		}
+	
+		return pixel;
+	}
+
+	uint8 GetSqual()
+	{
+		return Read(registerSqual);
+	}
+
+	int8 GetXMovement()
+	{
+		return (int8)(Read(registerDeltaX));
+	}
+
+	int8 GetYMovement()
+	{
+		return (int8)(Read(registerDeltaY));
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/input/sensor.c b/source/Concept/Framework/modules/input/sensor.c
new file mode 100755
index 0000000..3036159
--- /dev/null
+++ b/source/Concept/Framework/modules/input/sensor.c
@@ -0,0 +1 @@
+#include "sensor.h"
diff --git a/source/Concept/Framework/modules/input/sensor.h b/source/Concept/Framework/modules/input/sensor.h
new file mode 100755
index 0000000..e44c739
--- /dev/null
+++ b/source/Concept/Framework/modules/input/sensor.h
@@ -0,0 +1,27 @@
+#ifndef _SENSOR_H
+#define _SENSOR_H
+
+#include "../../defines.h"
+#include "../io_module.h"
+
+class Sensor : public IO_Module
+{
+public:
+	Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+	}
+
+protected:
+
+public:
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c
new file mode 100755
index 0000000..6679d3e
--- /dev/null
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c
@@ -0,0 +1,125 @@
+#include "ball_tracker.h"
+
+//-----------------------------------------------------------------------------
+void Ball_Tracker::Update()
+{
+	uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
+	uint16 intensity[sensorCount];
+	uint8 greatestIntensity = 0;
+	for(uint8 i = 0; i < sensorCount; i++)
+	{
+		IR_Sensor* currentSensor = parent->GetModule<IR_Sensor>(i + IO_SENSOR_IR_0_DEG);
+
+		intensity[i] = 1023 - currentSensor->GetIRIntensity();
+
+		if(intensity[i] < 24)
+		{
+			intensity[i] = 0;
+		}
+		
+		if(intensity[i] > intensity[greatestIntensity])
+		{
+			greatestIntensity = i;
+		}
+	}
+
+	if(intensity[greatestIntensity])
+	{
+		uint8 secondIntensity = 0xFF;
+		uint8 leftSensor = (greatestIntensity + 1) % sensorCount;
+		uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount;
+
+		if(intensity[leftSensor])
+		{
+			secondIntensity = leftSensor;
+		}
+
+		if(intensity[rightSensor] > intensity[leftSensor])
+		{
+			secondIntensity = rightSensor;
+		}
+
+		float mainDirection;
+		
+		switch(greatestIntensity + IO_SENSOR_IR_0_DEG)
+		{
+			case IO_SENSOR_IR_0_DEG:
+				mainDirection = 0;
+				break;
+			case IO_SENSOR_IR_30_DEG:
+				mainDirection = 1.0f * PI / 6.0f;
+				break;
+			case IO_SENSOR_IR_60_DEG:
+				mainDirection = 1.0f * PI / 3.0f;
+				break;
+			case IO_SENSOR_IR_100_DEG:
+				mainDirection = 5.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_180_DEG:
+				mainDirection = PI;
+				break;
+			case IO_SENSOR_IR_260_DEG:
+				mainDirection = 13.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_300_DEG:
+				mainDirection = 15.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_330_DEG:
+				mainDirection = 33.0f * PI / 18.0f;
+				break;
+			default:
+				mainDirection = -1.0f;
+				return;
+				break;
+		}
+
+		if(secondIntensity != 0xFF)
+		{
+			float secondDirection;
+
+			switch(secondIntensity + IO_SENSOR_IR_0_DEG)
+			{
+				case IO_SENSOR_IR_0_DEG:
+					secondDirection = 0;
+					break;
+				case IO_SENSOR_IR_30_DEG:
+					secondDirection = 1.0f * PI / 6.0f;
+					break;
+				case IO_SENSOR_IR_60_DEG:
+					secondDirection = 1.0f * PI / 3.0f;
+					break;
+				case IO_SENSOR_IR_100_DEG:
+					secondDirection = 5.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_180_DEG:
+					secondDirection = PI;
+					break;
+				case IO_SENSOR_IR_260_DEG:
+					secondDirection = 13.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_300_DEG:
+					secondDirection = 15.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_330_DEG:
+					secondDirection = 33.0f * PI / 18.0f;
+					break;
+				default:
+					secondDirection = -1.0f;
+					return;
+					break;
+			}
+
+			direction = (intensity[greatestIntensity] * mainDirection + 
+									intensity[secondIntensity] * secondDirection) / 
+									(intensity[greatestIntensity] + intensity[secondIntensity]);
+		}
+		else
+		{
+			direction = mainDirection;
+		}
+	}
+	else
+	{
+		direction = -1.0f;
+	}
+}
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h
new file mode 100755
index 0000000..ed8801f
--- /dev/null
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h
@@ -0,0 +1,35 @@
+#ifndef _BALL_TRACKER_H
+#define _BALL_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Ball_Tracker : public IO_Module
+{
+public:
+	Ball_Tracker()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->direction = -1.0f;
+	}
+
+	Ball_Tracker(uint32 trackerId)
+	{
+		this->parent = NULL;
+		this->moduleId = trackerId;
+		this->direction = -1.0f;
+	}
+
+protected:
+	float direction;
+
+public:
+	void Update();
+
+	float GetBallDirection()
+	{
+		return direction;
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
new file mode 100755
index 0000000..262069a
--- /dev/null
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@@ -0,0 +1,7 @@
+#include "position_tracker.h"
+
+//-----------------------------------------------------------------------------
+void Position_Tracker::Update()
+{
+	//insert code here
+}
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h
new file mode 100755
index 0000000..b31dd8f
--- /dev/null
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@@ -0,0 +1,45 @@
+#ifndef _POSITION_TRACKER_H
+#define _POSITION_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Position_Tracker : public IO_Module
+{
+public:
+	Position_Tracker()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Position_Tracker(uint32 trackerId)
+	{
+		this->parent = NULL;
+		this->moduleId = trackerId;
+	}
+
+protected:
+	float positionX;
+	float positionY;
+	float orientation;
+
+public:
+	void Update();
+
+	float GetPositionX()
+	{
+		return positionX;
+	}
+
+	float GetPositionY()
+	{
+		return positionY;
+	}
+
+	float GetOrientation()
+	{
+		return orientation;
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/io_module.c b/source/Concept/Framework/modules/io_module.c
new file mode 100755
index 0000000..3549af5
--- /dev/null
+++ b/source/Concept/Framework/modules/io_module.c
@@ -0,0 +1 @@
+#include "io_module.h"
diff --git a/source/Concept/Framework/modules/io_module.h b/source/Concept/Framework/modules/io_module.h
new file mode 100755
index 0000000..383eefb
--- /dev/null
+++ b/source/Concept/Framework/modules/io_module.h
@@ -0,0 +1,45 @@
+#ifndef _MODULE_H
+#define _MODULE_H
+
+#include "../defines.h"
+#include "../tools.h"
+
+class Robot;
+
+class IO_Module
+{
+public:
+	IO_Module()
+	{
+		this->parent = NULL; 
+		this->moduleId = 0;
+	}
+
+	IO_Module(uint32 moduleId)
+	{
+		this->parent = NULL;
+		this->moduleId = moduleId;
+	}
+
+protected:
+	Robot* parent;
+	uint32 moduleId;
+
+public:
+	Robot* GetParent()
+	{
+		return parent;
+	}
+
+	void SetParent(Robot* newParent)
+	{
+		parent = newParent;
+	}
+
+	uint32 GetId()
+	{
+		return moduleId;
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/output/display.c b/source/Concept/Framework/modules/output/display.c
new file mode 100755
index 0000000..82fbb41
--- /dev/null
+++ b/source/Concept/Framework/modules/output/display.c
@@ -0,0 +1 @@
+#include "display.h"
diff --git a/source/Concept/Framework/modules/output/display.h b/source/Concept/Framework/modules/output/display.h
new file mode 100755
index 0000000..e221120
--- /dev/null
+++ b/source/Concept/Framework/modules/output/display.h
@@ -0,0 +1,198 @@
+#ifndef _DISPLAY_H
+#define _DISPLAY_H
+
+#include "../../stdafx.h"
+
+class Display : public IO_Module
+{
+public:
+	Display()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->cursorVisible = false;
+		this->illuminationEnabled = true;
+		this->commandClear = 0;
+		this->commandReturnCursor = 0;
+		this->commandNewLine = 0;
+		this->commandSetting = 0;
+		this->settingCursorVisible = 0;
+		this->settingIllumination = 0;
+		this->settingCursorPosition = 0;
+	}
+
+	Display(uint32 displayId)
+	{
+		this->parent = NULL;
+		this->moduleId = displayId;
+		this->cursorVisible = false;
+		this->illuminationEnabled = true;
+		
+		switch(displayId)
+		{
+			case IO_DISPLAY_MAIN:
+				this->commandClear = 12;
+				this->commandReturnCursor = 13;
+				this->commandNewLine = 10;
+				this->commandSetting = 27;
+				this->settingCursorVisible = 67;
+				this->settingIllumination = 76;
+				this->settingCursorPosition = 79;
+				msleep(100);
+				uart1_init(103);//9600 BAUD at 16MHz Atmel
+				msleep(100);
+				break;
+			default:
+				this->commandClear = 0;
+				this->commandReturnCursor = 0;
+				this->commandNewLine = 0;
+				this->commandSetting = 0;
+				this->settingCursorVisible = 0;
+				this->settingIllumination = 0;
+				this->settingCursorPosition = 0;
+				break;
+		}
+	}
+
+protected:
+	bool cursorVisible;
+	bool illuminationEnabled;
+	//Commands
+	uint8 commandClear;
+	uint8 commandReturnCursor;
+	uint8 commandNewLine;
+	uint8 commandSetting;
+	//Settings
+	uint8 settingCursorVisible;
+	uint8 settingIllumination;
+	uint8 settingCursorPosition;
+
+	void SendCommand(uint8 newCommand)
+	{
+		switch(moduleId)
+		{
+			case IO_DISPLAY_MAIN:
+				uart1_putc(newCommand);
+				break;
+			default:
+				break;
+		}
+	}
+
+public:
+	void Print(char* newString)
+	{
+		switch(moduleId)
+		{
+			case IO_DISPLAY_MAIN:
+				uart1_puts(newString);
+				break;
+			default:
+				break;
+		}
+	}
+
+	void Print(int32 newInteger)
+	{
+		char buffer[12];
+		ltoa(newInteger, buffer, 10);
+		Print(buffer);
+	}
+
+	void PrintFloat(float newFloat)
+	{
+		Print((int32)(newFloat));
+		Print(".");
+		Print(abs((uint32)(newFloat - float((int32)(newFloat)) * 100000)));
+	}
+
+	void Print(char* newString, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		Print(newString);
+	}
+
+	void Print(int32 newInteger, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		Print(newInteger);
+	}
+
+	void PrintFloat(float newFloat, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		PrintFloat(newFloat);
+	}
+
+	void Clear()
+	{
+		SendCommand(commandClear);
+	}
+
+	void ReturnCursor()
+	{
+		SendCommand(commandReturnCursor);
+	}
+
+	void NewLine()
+	{
+		SendCommand(commandNewLine);
+	}
+
+	bool GetCursorVisible()
+	{
+		return cursorVisible;
+	}
+
+	void SetCursorVisible(bool newStatus)
+	{
+		cursorVisible = newStatus;
+
+		SendCommand(commandSetting);
+		SendCommand(settingCursorVisible);
+
+		if(cursorVisible)
+		{
+			SendCommand(1);
+		}
+		else
+		{
+			SendCommand(0);
+		}
+	}
+
+	bool GetLightingEnabled()
+	{
+		return illuminationEnabled;
+	}
+
+	void SetLightingEnabled(bool newStatus)
+	{
+		illuminationEnabled = newStatus;
+
+		SendCommand(commandSetting);
+		SendCommand(settingIllumination);
+
+		if(illuminationEnabled)
+		{
+			SendCommand(1);
+		}
+		else
+		{
+			SendCommand(0);
+		}
+	}
+
+	void SetCursorPosition(uint8 newX, uint8 newY)
+	{
+		if(!newX || newX > 20) return;
+		if(!newY || newY > 4) return;
+
+		SendCommand(commandSetting);
+		SendCommand(settingCursorPosition);
+		SendCommand(newX);
+		SendCommand(newY);
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/output/dribbler.c b/source/Concept/Framework/modules/output/dribbler.c
new file mode 100755
index 0000000..20557e4
--- /dev/null
+++ b/source/Concept/Framework/modules/output/dribbler.c
@@ -0,0 +1 @@
+#include "dribbler.h"
diff --git a/source/Concept/Framework/modules/output/dribbler.h b/source/Concept/Framework/modules/output/dribbler.h
new file mode 100755
index 0000000..b11af69
--- /dev/null
+++ b/source/Concept/Framework/modules/output/dribbler.h
@@ -0,0 +1,118 @@
+#ifndef _DRIBBLER_H
+#define _DRIBBLER_H
+
+#include "../../stdafx.h"
+
+class Dribbler : public IO_Module
+{
+public:
+	Dribbler()
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->portPower = NULL;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+		this->pinPower = 0;
+	}
+
+	Dribbler(uint32 dribblerId)
+	{
+		this->enabled = false;
+		this->curSpeed = 1.0f;
+		this->parent = NULL;
+		this->moduleId = dribblerId;
+		
+		switch(dribblerId)
+		{
+			case IO_DRIBBLER_MAIN:
+				this->hardwarePort = &PORTD;
+				this->portPower = &PORTA;
+				this->pinForward = (1 << 6);
+				this->pinReverse = (1 << 7);
+				this->pinPower = (1 << 5);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->portPower = NULL;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				this->pinPower = 0;
+				break;
+		}
+
+		UpdateDirection();
+	}
+
+protected:
+	bool enabled;
+	float curSpeed;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* portPower;
+	uint8 pinForward;
+	uint8 pinReverse;
+	uint8 pinPower;
+
+	void UpdateDirection()
+	{
+		if(enabled)
+		{
+			if(curSpeed > 0)
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort &= ~pinReverse;
+			}
+			else if(curSpeed < 0)
+			{
+				*hardwarePort |= pinReverse;
+				*hardwarePort &= ~pinForward;
+			}
+			else
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort |= pinReverse;
+			}
+
+			*portPower |= pinPower;
+		}
+		else
+		{
+			*hardwarePort &= ~pinForward;
+			*hardwarePort &= ~pinReverse;
+
+			*portPower &= ~pinPower;
+		}
+	}
+
+public:
+	float GetSpeed()
+	{
+		return curSpeed;
+	}
+
+	void SetSpeed(float newSpeed)
+	{
+		curSpeed = newSpeed;
+
+		UpdateDirection();
+	}
+
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		UpdateDirection();
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/output/engine.c b/source/Concept/Framework/modules/output/engine.c
new file mode 100755
index 0000000..5c14c17
--- /dev/null
+++ b/source/Concept/Framework/modules/output/engine.c
@@ -0,0 +1 @@
+#include "engine.h"
diff --git a/source/Concept/Framework/modules/output/engine.h b/source/Concept/Framework/modules/output/engine.h
new file mode 100755
index 0000000..27b9905
--- /dev/null
+++ b/source/Concept/Framework/modules/output/engine.h
@@ -0,0 +1,124 @@
+#ifndef _ENGINE_H
+#define _ENGINE_H
+
+#include "../../stdafx.h"
+
+class Engine : public IO_Module
+{
+public:
+	Engine()
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->pwmSpeed = NULL;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+	}
+
+	Engine(uint32 engineId)
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = engineId;
+		
+		switch(engineId)
+		{
+			case IO_ENGINE_DRIVE_LEFT:
+				this->hardwarePort = &PORTB;
+				this->pwmSpeed = &OCR1A;
+				this->pinForward = (1 << 0);
+				this->pinReverse = (1 << 1);
+				break;
+			case IO_ENGINE_DRIVE_BACK:
+				this->hardwarePort = &PORTB;
+				this->pwmSpeed = &OCR1B;
+				this->pinForward = (1 << 2);
+				this->pinReverse = (1 << 3);
+				break;
+			case IO_ENGINE_DRIVE_RIGHT:
+				this->hardwarePort = &PORTD;
+				this->pwmSpeed = &OCR3A;
+				this->pinForward = (1 << 5);
+				this->pinReverse = (1 << 4);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->pwmSpeed = NULL;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				break;
+		}
+
+		*this->pwmSpeed = 0;
+	}
+
+protected:
+	bool enabled;
+	float curSpeed;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint16* pwmSpeed;
+	uint8 pinForward;
+	uint8 pinReverse;
+
+	void UpdateDirection()
+	{
+		if(enabled)
+		{
+			if(curSpeed > 0)
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort &= ~pinReverse;
+			}
+			else if(curSpeed < 0)
+			{
+				*hardwarePort |= pinReverse;
+				*hardwarePort &= ~pinForward;
+			}
+			else
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort |= pinReverse;
+			}
+		}
+		else
+		{
+			*hardwarePort &= ~pinForward;
+			*hardwarePort &= ~pinReverse;
+		}
+	}
+
+public:
+	float GetSpeed()
+	{
+		return curSpeed;
+	}
+
+	void SetSpeed(float newSpeed)
+	{
+		curSpeed = newSpeed;
+
+		*pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM)));
+
+		UpdateDirection();
+	}
+
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		UpdateDirection();
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/output/kicker.c b/source/Concept/Framework/modules/output/kicker.c
new file mode 100755
index 0000000..6670efb
--- /dev/null
+++ b/source/Concept/Framework/modules/output/kicker.c
@@ -0,0 +1 @@
+#include "kicker.h"
diff --git a/source/Concept/Framework/modules/output/kicker.h b/source/Concept/Framework/modules/output/kicker.h
new file mode 100755
index 0000000..080666a
--- /dev/null
+++ b/source/Concept/Framework/modules/output/kicker.h
@@ -0,0 +1,84 @@
+#ifndef _KICKER_H
+#define _KICKER_H
+
+#include "../../stdafx.h"
+
+class Kicker : public IO_Module
+{
+public:
+	Kicker()
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->portPower = NULL;
+		this->portForward = NULL;
+		this->portReverse = NULL;
+		this->pinPower = 0;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+	}
+
+	Kicker(uint32 kickerId)
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = kickerId;
+
+		switch(kickerId)
+		{
+			case IO_KICKER_MAIN:
+				this->portPower = &PORTG;
+				this->portForward = &PORTA;
+				this->portReverse = &PORTE;
+				this->pinPower = (1 << 3);
+				this->pinForward = (1 << 2);
+				this->pinReverse = (1 << 6);
+				break;
+			default:
+				this->portPower = NULL;
+				this->portForward = NULL;
+				this->portReverse = NULL;
+				this->pinPower = 0;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				break;
+		}
+
+		*this->portForward |= this->pinForward;
+		*this->portReverse &= ~this->pinReverse;
+	}
+
+protected:
+	bool enabled;
+
+	//Hardware
+	volatile uint8* portPower;
+	volatile uint8* portForward;
+	volatile uint8* portReverse;
+	uint8 pinPower;
+	uint8 pinForward;
+	uint8 pinReverse;
+
+public:
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		if(enabled)
+		{
+			*portPower |= pinPower;
+		}
+		else
+		{
+			*portPower &= ~pinPower;
+		}
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/modules/output/led.c b/source/Concept/Framework/modules/output/led.c
new file mode 100755
index 0000000..687b2db
--- /dev/null
+++ b/source/Concept/Framework/modules/output/led.c
@@ -0,0 +1 @@
+#include "led.h"
diff --git a/source/Concept/Framework/modules/output/led.h b/source/Concept/Framework/modules/output/led.h
new file mode 100755
index 0000000..08e7466
--- /dev/null
+++ b/source/Concept/Framework/modules/output/led.h
@@ -0,0 +1,65 @@
+#ifndef _LED_H
+#define _LED_H
+
+#include "../../stdafx.h"
+
+class Led : public IO_Module
+{
+public:
+	Led()
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->pinPower = 0;
+	}
+
+	Led(uint32 ledId)
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = ledId;
+
+		switch(ledId)
+		{
+			case IO_LED_MAIN:
+				this->hardwarePort = &PORTB;
+				this->pinPower = (1 << 1);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->pinPower = 0;
+				break;
+		}
+	}
+
+protected:
+	bool enabled;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	uint8 pinPower;
+
+public:
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		if(enabled)
+		{
+			*hardwarePort &= ~pinPower;
+		}
+		else
+		{
+			*hardwarePort |= pinPower;
+		}
+	}
+};
+
+#endif
diff --git a/source/Concept/Framework/mouse_sensor.cpp b/source/Concept/Framework/mouse_sensor.cpp
deleted file mode 100644
index 214a13e..0000000
--- a/source/Concept/Framework/mouse_sensor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "mouse_sensor.h"
diff --git a/source/Concept/Framework/position_tracker.cpp b/source/Concept/Framework/position_tracker.cpp
deleted file mode 100644
index 262069a..0000000
--- a/source/Concept/Framework/position_tracker.cpp
+++ /dev/null
@@ -1,7 +0,0 @@
-#include "position_tracker.h"
-
-//-----------------------------------------------------------------------------
-void Position_Tracker::Update()
-{
-	//insert code here
-}
diff --git a/source/Concept/Framework/robot.c b/source/Concept/Framework/robot.c
new file mode 100755
index 0000000..d8ea1c2
--- /dev/null
+++ b/source/Concept/Framework/robot.c
@@ -0,0 +1,135 @@
+#include "robot.h"
+
+//-----------------------------------------------------------------------------
+Robot::Robot()
+{
+	//Hardware
+
+	//Set pin 1-6 output, 0 and 7 input
+	DDRA = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6);
+	PORTA = 0;
+
+	//All output
+	DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
+	PORTB = (1 << 1);
+
+	//All output except PC4/PC5 (mousesensor SDA)
+	DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 6)  | (1 << 7);
+	PORTC = 0;
+
+	//All output except PD0+1(I2C) + 2+3(RS232)
+	DDRD = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
+	PORTD = (1 << 0) | (1 << 1);//Activate pullup at PD0+1
+
+	//PE5 for input
+	DDRE = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 6)  | (1 << 7);
+	PORTE = 0;
+
+	//All input with pullup
+	DDRF = 0;
+	PORTF = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
+
+	//All input
+	DDRG = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4); 
+	PORTG = (1 << 0) | (1 << 1);
+
+	// activate channel A+B on PWM1 at 8Bit
+	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
+	TCCR1B = (1<<ICNC1) | (1<<CS11); // set clock/prescaler 1/8 -> enable counter
+
+	// activate Kanal A+B on PWM3 at 8Bit
+	TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
+	TCCR3B =  (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter
+
+	//Activate interrupt
+	sei();
+
+	//Interface
+	for(uint8 i = IO_START; i < IO_END; i++)
+	{
+		modules[i] = NULL;
+	}
+}
+
+//-----------------------------------------------------------------------------
+Robot::~Robot()
+{
+	for(uint8 i = 0; i < IO_END; i++)
+	{
+		if(!modules[i]) continue;
+
+		delete modules[i];
+		modules[i] = NULL;
+	}
+}
+
+//-----------------------------------------------------------------------------
+bool Robot::AddModule(IO_Module* newModule)
+{
+	if(modules[newModule->GetId()])
+		return false;
+
+	newModule->SetParent(this);
+
+	modules[newModule->GetId()] = newModule;
+
+	return true;
+}
+
+//-----------------------------------------------------------------------------
+bool Robot::RemoveModule(IO_Module* oldModule)
+{
+	return RemoveModule(oldModule->GetId());
+}
+
+//-----------------------------------------------------------------------------
+void Robot::Update()
+{
+	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
+
+	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
+
+	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
+
+	//insert code here
+}
+
+//-----------------------------------------------------------------------------
+uint16 Robot::GetADCValue(uint8 channel)
+{
+	if(channel > 7) return 0;
+
+	uint32 result = 0;
+	
+	//Activate ADC and set division factor to 8
+	ADCSRA = (1 << ADEN) | (1 << ADPS1) | (1 << ADPS0);
+
+	//Set multiplexer channel
+	ADMUX = channel;
+	//Use internal referencevoltage (2,56 V for atmega32)
+	ADMUX |= (1 << REFS1) | (1 << REFS0);
+	
+	//Initialise ADC and start dummyreadout
+	ADCSRA |= (1 << ADSC);
+	while(ADCSRA & (1 << ADSC));
+	
+	//Get voltage three times and calculate average value
+	for(uint8 i = 0; i < 3; i++)
+	{
+		// Eine Wandlung
+		ADCSRA |= (1 << ADSC);
+		// Auf Ergebnis warten...
+		while(ADCSRA & (1 << ADSC));
+		
+		result += ADCW;
+	}
+	
+	//Disable ADC
+	ADCSRA &= ~(1 << ADEN);
+	
+	result /= 3;
+	
+	return (uint16)(result);
+}
+
+//--- EOF ---
diff --git a/source/Concept/Framework/robot.cpp b/source/Concept/Framework/robot.cpp
deleted file mode 100644
index 5c673cb..0000000
--- a/source/Concept/Framework/robot.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-#include "robot.h"
-
-//-----------------------------------------------------------------------------
-Robot::Robot()
-{
-	//Hardware
-
-	//Set pin 1-6 output, 0 and 7 input
-	DDRA = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6);
-	PORTA = 0;
-
-	//All output
-	DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
-	PORTB = (1 << 1);
-
-	//All output except PC4/PC5 (mousesensor SDA)
-	DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 6)  | (1 << 7);
-	PORTC = 0;
-
-	//All output except PD0+1(I2C) + 2+3(RS232)
-	DDRD = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
-	PORTD = (1 << 0) | (1 << 1);//Activate pullup at PD0+1
-
-	//PE5 for input
-	DDRE = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 6)  | (1 << 7);
-	PORTE = 0;
-
-	//All input with pullup
-	DDRF = 0;
-	PORTF = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6)  | (1 << 7);
-
-	//All input
-	DDRG = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4); 
-	PORTG = (1 << 0) | (1 << 1);
-
-	// activate channel A+B on PWM1 at 8Bit
-	TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
-	TCCR1B = (1<<ICNC1) | (1<<CS11); // set clock/prescaler 1/8 -> enable counter
-
-	// activate Kanal A+B on PWM3 at 8Bit
-	TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
-	TCCR3B =  (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter
-
-	//Activate interrupt
-	sei();
-
-	//Interface
-	for(uint8 i = 0; i < IO_END; i++)
-	{
-		modules[i] = NULL;
-	}
-}
-
-//-----------------------------------------------------------------------------
-Robot::~Robot()
-{
-	for(uint8 i = 0; i < IO_END; i++)
-	{
-		if(!modules[i]) continue;
-
-		delete modules[i];
-		modules[i] = NULL;
-	}
-}
-
-//-----------------------------------------------------------------------------
-bool Robot::AddModule(IO_Module* newModule)
-{
-	if(modules[newModule->GetId()])
-		return false;
-
-	newModule->SetParent(this);
-
-	modules[newModule->GetId()] = newModule;
-
-	return true;
-}
-
-//-----------------------------------------------------------------------------
-bool Robot::RemoveModule(IO_Module* oldModule)
-{
-	return RemoveModule(oldModule->GetId());
-}
-
-//-----------------------------------------------------------------------------
-void Robot::Update()
-{
-	//insert code here
-}
-
-//-----------------------------------------------------------------------------
-uint16 Robot::GetADCValue(uint8 channel)
-{
-	if(channel > 7) return 0;
-
-	uint32 result = 0;
-	
-	//Activate ADC and set division factor to 8
-	ADCSRA = (1 << ADEN) | (1 << ADPS1) | (1 << ADPS0);
-
-	//Set multiplexer channel
-	ADMUX = channel;
-	//Use internal referencevoltage (2,56 V for atmega32)
-	ADMUX |= (1 << REFS1) | (1 << REFS0);
-	
-	//Initialise ADC and start dummyreadout
-	ADCSRA |= (1 << ADSC);
-	while(ADCSRA & (1 << ADSC));
-	
-	//Get voltage three times and calculate average value
-	for(uint8 i = 0; i < 3; i++)
-	{
-		// Eine Wandlung
-		ADCSRA |= (1 << ADSC);
-		// Auf Ergebnis warten...
-		while(ADCSRA & (1 << ADSC));
-		
-		result += ADCW;
-	}
-	
-	//Disable ADC
-	ADCSRA &= ~(1 << ADEN);
-	
-	result /= 3;
-	
-	return (uint16)(result);
-}
-
-//--- EOF ---
diff --git a/source/Concept/Framework/sensor.cpp b/source/Concept/Framework/sensor.cpp
deleted file mode 100644
index b9b4936..0000000
--- a/source/Concept/Framework/sensor.cpp
+++ /dev/null
@@ -1 +0,0 @@
-#include "sensor.h"
\ No newline at end of file
diff --git a/source/Concept/Framework/stdafx.h b/source/Concept/Framework/stdafx.h
index 151d749..af60c6e 100644
--- a/source/Concept/Framework/stdafx.h
+++ b/source/Concept/Framework/stdafx.h
@@ -19,4 +19,6 @@
 #include "ir_sensor.h"
 #include "mouse_sensor.h"
 #include "position_tracker.h"
+#include "ball_tracker.h"
+#include "navigator.h"
 #include "robot.h"
diff --git a/source/Concept/Framework/tools.c b/source/Concept/Framework/tools.c
new file mode 100755
index 0000000..6fd107c
--- /dev/null
+++ b/source/Concept/Framework/tools.c
@@ -0,0 +1,17 @@
+#include "tools.h"
+
+#ifndef new
+
+//-----------------------------------------------------------------------------
+void* operator new(size_t sz)
+{
+	return malloc(sz);
+}
+
+//-----------------------------------------------------------------------------
+void operator delete(void* p)
+{
+	free(p);
+}
+
+#endif
diff --git a/source/Concept/Framework/tools.cpp b/source/Concept/Framework/tools.cpp
deleted file mode 100644
index 6fd107c..0000000
--- a/source/Concept/Framework/tools.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "tools.h"
-
-#ifndef new
-
-//-----------------------------------------------------------------------------
-void* operator new(size_t sz)
-{
-	return malloc(sz);
-}
-
-//-----------------------------------------------------------------------------
-void operator delete(void* p)
-{
-	free(p);
-}
-
-#endif
diff --git a/source/Concept/Framework/tools.h b/source/Concept/Framework/tools.h
index 6519a1b..0ca4dda 100644
--- a/source/Concept/Framework/tools.h
+++ b/source/Concept/Framework/tools.h
@@ -1,7 +1,8 @@
 #ifndef _TOOLS_H
 #define _TOOLS_H
 
-#include <stdlib.h>
+#include <stdlib.h>
+#include <math.h>
 
 #ifndef new
 	void* operator new(size_t sz);
@@ -34,6 +35,11 @@ inline void usleep(int usec)
       asm volatile("nop"); 
     }
   }
-};
+};
+
+inline float distance2d(float x1, float y1, float x2, float y2)
+{
+	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
+}
 
 #endif
-- 
cgit v1.2.3