diff --git a/Firmware/carteHRI.uvoptx b/Firmware/carteHRI.uvoptx
index 06161cb..fc80484 100644
--- a/Firmware/carteHRI.uvoptx
+++ b/Firmware/carteHRI.uvoptx
@@ -1,749 +1,762 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no" ?>
 <ProjectOpt xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_optx.xsd">
 
   <SchemaVersion>1.0</SchemaVersion>
 
   <Header>### uVision Project, (C) Keil Software</Header>
 
   <Extensions>
     <cExt>*.c</cExt>
     <aExt>*.s*; *.src; *.a*</aExt>
     <oExt>*.obj</oExt>
     <lExt>*.lib</lExt>
     <tExt>*.txt; *.h; *.inc</tExt>
     <pExt>*.plm</pExt>
     <CppX>*.cpp</CppX>
     <nMigrate>0</nMigrate>
   </Extensions>
 
   <DaveTm>
     <dwLowDateTime>0</dwLowDateTime>
     <dwHighDateTime>0</dwHighDateTime>
   </DaveTm>
 
   <Target>
     <TargetName>Target 1</TargetName>
     <ToolsetNumber>0x4</ToolsetNumber>
     <ToolsetName>ARM-ADS</ToolsetName>
     <TargetOption>
       <CLKADS>168000000</CLKADS>
       <OPTTT>
         <gFlags>1</gFlags>
         <BeepAtEnd>1</BeepAtEnd>
         <RunSim>0</RunSim>
         <RunTarget>1</RunTarget>
         <RunAbUc>0</RunAbUc>
       </OPTTT>
       <OPTHX>
         <HexSelection>1</HexSelection>
         <FlashByte>65535</FlashByte>
         <HexRangeLowAddress>0</HexRangeLowAddress>
         <HexRangeHighAddress>0</HexRangeHighAddress>
         <HexOffset>0</HexOffset>
       </OPTHX>
       <OPTLEX>
         <PageWidth>79</PageWidth>
         <PageLength>66</PageLength>
         <TabStop>8</TabStop>
         <ListingPath>.\lst\</ListingPath>
       </OPTLEX>
       <ListingPage>
         <CreateCListing>1</CreateCListing>
         <CreateAListing>1</CreateAListing>
         <CreateLListing>1</CreateLListing>
         <CreateIListing>0</CreateIListing>
         <AsmCond>1</AsmCond>
         <AsmSymb>1</AsmSymb>
         <AsmXref>0</AsmXref>
         <CCond>1</CCond>
         <CCode>0</CCode>
         <CListInc>0</CListInc>
         <CSymb>0</CSymb>
         <LinkerCodeListing>0</LinkerCodeListing>
       </ListingPage>
       <OPTXL>
         <LMap>1</LMap>
         <LComments>1</LComments>
         <LGenerateSymbols>1</LGenerateSymbols>
         <LLibSym>1</LLibSym>
         <LLines>1</LLines>
         <LLocSym>1</LLocSym>
         <LPubSym>1</LPubSym>
         <LXref>0</LXref>
         <LExpSel>0</LExpSel>
       </OPTXL>
       <OPTFL>
         <tvExp>1</tvExp>
         <tvExpOptDlg>0</tvExpOptDlg>
         <IsCurrentTarget>1</IsCurrentTarget>
       </OPTFL>
       <CpuCode>18</CpuCode>
       <Books>
         <Book>
           <Number>0</Number>
           <Title>User Manual (MCBSTM32F400)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\Keil\MCBSTM32F400\Documentation\mcbstm32f200.chm</Path>
         </Book>
         <Book>
           <Number>1</Number>
           <Title>Schematics (MCBSTM32F400)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\Keil\MCBSTM32F400\Documentation\mcbstm32f400-schematics.pdf</Path>
         </Book>
         <Book>
           <Number>2</Number>
           <Title>Getting Started (STM32F4-Discovery)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\DM00037368.pdf</Path>
         </Book>
         <Book>
           <Number>3</Number>
           <Title>User Manual (STM32F4-Discovery)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\DM00039084.pdf</Path>
         </Book>
         <Book>
           <Number>4</Number>
           <Title>Bill of Materials (STM32F4-Discovery)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_bom.zip</Path>
         </Book>
         <Book>
           <Number>5</Number>
           <Title>Gerber Files (STM32F4-Discovery)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_gerber.zip</Path>
         </Book>
         <Book>
           <Number>6</Number>
           <Title>Schematics (STM32F4-Discovery)</Title>
           <Path>C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_sch.zip</Path>
         </Book>
         <Book>
           <Number>7</Number>
           <Title>MCBSTM32F400 Evaluation Board Web Page (MCBSTM32F400)</Title>
           <Path>http://www.keil.com/mcbstm32f400/</Path>
         </Book>
         <Book>
           <Number>8</Number>
           <Title>STM32F4-Discovery Web Page (STM32F4-Discovery)</Title>
           <Path>http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1199/PF252419</Path>
         </Book>
       </Books>
       <DebugOpt>
         <uSim>0</uSim>
         <uTrg>1</uTrg>
         <sLdApp>1</sLdApp>
         <sGomain>1</sGomain>
         <sRbreak>1</sRbreak>
         <sRwatch>1</sRwatch>
         <sRmem>1</sRmem>
         <sRfunc>1</sRfunc>
         <sRbox>1</sRbox>
         <tLdApp>1</tLdApp>
         <tGomain>0</tGomain>
         <tRbreak>1</tRbreak>
         <tRwatch>1</tRwatch>
         <tRmem>1</tRmem>
         <tRfunc>0</tRfunc>
         <tRbox>1</tRbox>
         <tRtrace>1</tRtrace>
         <sRSysVw>1</sRSysVw>
         <tRSysVw>1</tRSysVw>
         <sRunDeb>0</sRunDeb>
         <sLrtime>0</sLrtime>
         <nTsel>11</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
         <sDlgPa></sDlgPa>
         <sIfile></sIfile>
         <tDll></tDll>
         <tDllPa></tDllPa>
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
         <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMRTXEVENTFLAGS</Key>
           <Name>-L50 -Z18 -C0 -M0 -T0</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGDARM</Key>
           <Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGTARM</Key>
           <Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMDBGFLAGS</Key>
           <Name>-T0</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
           <Name>(105=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ST-LINKIII-KEIL_SWO</Key>
-          <Name>-U303030303030303030303031 -I0 -O8398 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO19 -TC168000000 -TP21 -TDS8053 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+          <Name>-U303030303030303030303031 -O8398 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO19 -TC168000000 -TP21 -TDS8053 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>UL2CM3</Key>
           <Name>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$Flash\STM32F4xx_1024.FLM))</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
       <WatchWindow1>
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>statusReg,0x0A</ItemText>
+          <ItemText>uart_txBuffer[0]</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ctrl_currentPid,0x0A</ItemText>
+          <ItemText>uart_txBuffer[1]</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>adc_currentSensOffset,0x0A</ItemText>
+          <ItemText>uart_currentTxBufferToWriteTo</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ctrl_motorTorque_Nm_c</ItemText>
+          <ItemText>testVar</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>ctrl_positionPid</ItemText>
+          <ItemText>uart_txBufferIndex</ItemText>
         </Ww>
       </WatchWindow1>
       <WatchWindow2>
         <Ww>
           <count>0</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrentDebug</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorTargetCurrentDebug</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>2</WinNumber>
           <ItemText>ctrl_motorTorque_mNm_c</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorTorque_mNm_dbg</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorTorque_mNm</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>2</WinNumber>
           <ItemText>nCycles</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorPosCod,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>2</WinNumber>
           <ItemText>CurrentSensOffset,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrent_mA_mean</ItemText>
         </Ww>
         <Ww>
           <count>9</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrent_mA</ItemText>
         </Ww>
         <Ww>
           <count>10</count>
           <WinNumber>2</WinNumber>
           <ItemText>ADCValuesBuffer,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>11</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrent_mA</ItemText>
         </Ww>
         <Ww>
           <count>12</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrentSum</ItemText>
         </Ww>
         <Ww>
           <count>13</count>
           <WinNumber>2</WinNumber>
           <ItemText>debugVar01,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>14</count>
           <WinNumber>2</WinNumber>
           <ItemText>adcValues1,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>2</WinNumber>
           <ItemText>motorCurrent_mA</ItemText>
         </Ww>
       </WatchWindow2>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
         <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>
         <aPa>0</aPa>
         <viewmode>1</viewmode>
         <vrSel>0</vrSel>
         <aSym>0</aSym>
         <aTbox>0</aTbox>
         <AscS1>0</AscS1>
         <AscS2>0</AscS2>
         <AscS3>0</AscS3>
         <aSer3>0</aSer3>
         <eProf>0</eProf>
         <aLa>0</aLa>
         <aPa1>0</aPa1>
         <AscS4>0</AscS4>
         <aSer4>0</aSer4>
         <StkLoc>0</StkLoc>
         <TrcWin>0</TrcWin>
         <newCpu>0</newCpu>
         <uProt>0</uProt>
       </DebugFlag>
       <LintExecutable></LintExecutable>
       <LintConfigFile></LintConfigFile>
       <bLintAuto>0</bLintAuto>
     </TargetOption>
   </Target>
 
   <Group>
     <GroupName>STM32_files</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>1</GroupNumber>
       <FileNumber>1</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\system_stm32f4xx.c</PathWithFileName>
       <FilenameWithoutPath>system_stm32f4xx.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>1</GroupNumber>
       <FileNumber>2</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\startup_stm32f4xx.s</PathWithFileName>
       <FilenameWithoutPath>startup_stm32f4xx.s</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
   </Group>
 
   <Group>
     <GroupName>SRC</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>2</GroupNumber>
       <FileNumber>3</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\main.c</PathWithFileName>
       <FilenameWithoutPath>main.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
       <FileNumber>4</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\communication.c</PathWithFileName>
       <FilenameWithoutPath>communication.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
       <FileNumber>5</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\controller.c</PathWithFileName>
       <FilenameWithoutPath>controller.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
   </Group>
 
   <Group>
     <GroupName>STM32_libs</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>6</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_gpio.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_gpio.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>7</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rcc.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_rcc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>8</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_tim.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_tim.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>9</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\misc.c</PathWithFileName>
       <FilenameWithoutPath>misc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>10</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_exti.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_exti.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>11</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_syscfg.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_syscfg.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>12</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_adc.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_adc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>13</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dac.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_dac.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>14</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dma.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_dma.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>15</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_usart.c</PathWithFileName>
       <FilenameWithoutPath>stm32f4xx_usart.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>16</FileNumber>
       <FileType>4</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\Lib\ARM\arm_cortexM4lf_math.lib</PathWithFileName>
       <FilenameWithoutPath>arm_cortexM4lf_math.lib</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
   </Group>
 
   <Group>
     <GroupName>SRC_drivers</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>17</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\adc.c</PathWithFileName>
       <FilenameWithoutPath>adc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\callback_timers.c</PathWithFileName>
       <FilenameWithoutPath>callback_timers.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\dac.c</PathWithFileName>
       <FilenameWithoutPath>dac.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\h_bridge.c</PathWithFileName>
       <FilenameWithoutPath>h_bridge.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\incr_encoder.c</PathWithFileName>
       <FilenameWithoutPath>incr_encoder.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\uart.c</PathWithFileName>
       <FilenameWithoutPath>uart.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\hall.c</PathWithFileName>
       <FilenameWithoutPath>hall.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\strain_gauge.c</PathWithFileName>
       <FilenameWithoutPath>strain_gauge.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
       <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\tachometer.c</PathWithFileName>
       <FilenameWithoutPath>tachometer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
   </Group>
 
   <Group>
     <GroupName>SRC_lib</GroupName>
     <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
       <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\lib\basic_filter.c</PathWithFileName>
       <FilenameWithoutPath>basic_filter.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
       <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\lib\pid.c</PathWithFileName>
       <FilenameWithoutPath>pid.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
       <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\lib\utils.c</PathWithFileName>
       <FilenameWithoutPath>utils.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>5</GroupNumber>
+      <FileNumber>29</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <Focus>0</Focus>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>.\src\lib\circular_buffer.c</PathWithFileName>
+      <FilenameWithoutPath>circular_buffer.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
 </ProjectOpt>
diff --git a/Firmware/carteHRI.uvprojx b/Firmware/carteHRI.uvprojx
index 3b37fa6..a59f0aa 100644
--- a/Firmware/carteHRI.uvprojx
+++ b/Firmware/carteHRI.uvprojx
@@ -1,581 +1,586 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no" ?>
 <Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
 
   <SchemaVersion>2.1</SchemaVersion>
 
   <Header>### uVision Project, (C) Keil Software</Header>
 
   <Targets>
     <Target>
       <TargetName>Target 1</TargetName>
       <ToolsetNumber>0x4</ToolsetNumber>
       <ToolsetName>ARM-ADS</ToolsetName>
       <TargetOption>
         <TargetCommonOption>
           <Device>STM32F407VG</Device>
           <Vendor>STMicroelectronics</Vendor>
           <PackID>Keil.STM32F4xx_DFP.2.5.0</PackID>
           <PackURL>http://www.keil.com/pack</PackURL>
           <Cpu>IROM(0x08000000,0x100000) IRAM(0x20000000,0x20000) IRAM2(0x10000000,0x10000) CPUTYPE("Cortex-M4") FPU2 CLOCK(168000000) ELITTLE</Cpu>
           <FlashUtilSpec></FlashUtilSpec>
           <StartupFile></StartupFile>
           <FlashDriverDll>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$Flash\STM32F4xx_1024.FLM))</FlashDriverDll>
           <DeviceId>6103</DeviceId>
           <RegisterFile>$$Device:STM32F407VG$Device\Include\stm32f4xx.h</RegisterFile>
           <MemoryEnv></MemoryEnv>
           <Cmp></Cmp>
           <Asm></Asm>
           <Linker></Linker>
           <OHString></OHString>
           <InfinionOptionDll></InfinionOptionDll>
           <SLE66CMisc></SLE66CMisc>
           <SLE66AMisc></SLE66AMisc>
           <SLE66LinkerMisc></SLE66LinkerMisc>
           <SFDFile>$$Device:STM32F407VG$SVD\STM32F40x.svd</SFDFile>
           <bCustSvd>0</bCustSvd>
           <UseEnv>0</UseEnv>
           <BinPath></BinPath>
           <IncludePath></IncludePath>
           <LibPath></LibPath>
           <RegisterFilePath></RegisterFilePath>
           <DBRegisterFilePath></DBRegisterFilePath>
           <TargetStatus>
             <Error>0</Error>
             <ExitCodeStop>0</ExitCodeStop>
             <ButtonStop>0</ButtonStop>
             <NotGenerated>0</NotGenerated>
             <InvalidFlash>1</InvalidFlash>
           </TargetStatus>
           <OutputDirectory>.\output\</OutputDirectory>
           <OutputName>motorControl</OutputName>
           <CreateExecutable>1</CreateExecutable>
           <CreateLib>0</CreateLib>
           <CreateHexFile>1</CreateHexFile>
           <DebugInformation>1</DebugInformation>
           <BrowseInformation>1</BrowseInformation>
           <ListingPath>.\lst\</ListingPath>
           <HexFormatSelection>1</HexFormatSelection>
           <Merge32K>0</Merge32K>
           <CreateBatchFile>0</CreateBatchFile>
           <BeforeCompile>
             <RunUserProg1>0</RunUserProg1>
             <RunUserProg2>0</RunUserProg2>
             <UserProg1Name></UserProg1Name>
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopU1X>0</nStopU1X>
             <nStopU2X>0</nStopU2X>
           </BeforeCompile>
           <BeforeMake>
             <RunUserProg1>0</RunUserProg1>
             <RunUserProg2>0</RunUserProg2>
             <UserProg1Name></UserProg1Name>
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopB1X>0</nStopB1X>
             <nStopB2X>0</nStopB2X>
           </BeforeMake>
           <AfterMake>
             <RunUserProg1>0</RunUserProg1>
             <RunUserProg2>0</RunUserProg2>
             <UserProg1Name></UserProg1Name>
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopA1X>0</nStopA1X>
             <nStopA2X>0</nStopA2X>
           </AfterMake>
           <SelectedForBatchBuild>0</SelectedForBatchBuild>
           <SVCSIdString></SVCSIdString>
         </TargetCommonOption>
         <CommonProperty>
           <UseCPPCompiler>0</UseCPPCompiler>
           <RVCTCodeConst>0</RVCTCodeConst>
           <RVCTZI>0</RVCTZI>
           <RVCTOtherData>0</RVCTOtherData>
           <ModuleSelection>0</ModuleSelection>
           <IncludeInBuild>1</IncludeInBuild>
           <AlwaysBuild>0</AlwaysBuild>
           <GenerateAssemblyFile>0</GenerateAssemblyFile>
           <AssembleAssemblyFile>0</AssembleAssemblyFile>
           <PublicsOnly>0</PublicsOnly>
           <StopOnExitCode>3</StopOnExitCode>
           <CustomArgument></CustomArgument>
           <IncludeLibraryModules></IncludeLibraryModules>
           <ComprImg>1</ComprImg>
         </CommonProperty>
         <DllOption>
           <SimDllName>SARMCM3.DLL</SimDllName>
           <SimDllArguments>-REMAP -MPU</SimDllArguments>
           <SimDlgDll>DCM.DLL</SimDlgDll>
           <SimDlgDllArguments>-pCM4</SimDlgDllArguments>
           <TargetDllName>SARMCM3.DLL</TargetDllName>
           <TargetDllArguments>-REMAP -MPU</TargetDllArguments>
           <TargetDlgDll>TCM.DLL</TargetDlgDll>
           <TargetDlgDllArguments>-pCM4</TargetDlgDllArguments>
         </DllOption>
         <DebugOption>
           <OPTHX>
             <HexSelection>1</HexSelection>
             <HexRangeLowAddress>0</HexRangeLowAddress>
             <HexRangeHighAddress>0</HexRangeHighAddress>
             <HexOffset>0</HexOffset>
             <Oh166RecLen>16</Oh166RecLen>
           </OPTHX>
           <Simulator>
             <UseSimulator>0</UseSimulator>
             <LoadApplicationAtStartup>1</LoadApplicationAtStartup>
             <RunToMain>1</RunToMain>
             <RestoreBreakpoints>1</RestoreBreakpoints>
             <RestoreWatchpoints>1</RestoreWatchpoints>
             <RestoreMemoryDisplay>1</RestoreMemoryDisplay>
             <RestoreFunctions>1</RestoreFunctions>
             <RestoreToolbox>1</RestoreToolbox>
             <LimitSpeedToRealTime>0</LimitSpeedToRealTime>
             <RestoreSysVw>1</RestoreSysVw>
           </Simulator>
           <Target>
             <UseTarget>1</UseTarget>
             <LoadApplicationAtStartup>1</LoadApplicationAtStartup>
             <RunToMain>0</RunToMain>
             <RestoreBreakpoints>1</RestoreBreakpoints>
             <RestoreWatchpoints>1</RestoreWatchpoints>
             <RestoreMemoryDisplay>1</RestoreMemoryDisplay>
             <RestoreFunctions>0</RestoreFunctions>
             <RestoreToolbox>1</RestoreToolbox>
             <RestoreTracepoints>1</RestoreTracepoints>
             <RestoreSysVw>1</RestoreSysVw>
           </Target>
           <RunDebugAfterBuild>0</RunDebugAfterBuild>
           <TargetSelection>11</TargetSelection>
           <SimDlls>
             <CpuDll></CpuDll>
             <CpuDllArguments></CpuDllArguments>
             <PeripheralDll></PeripheralDll>
             <PeripheralDllArguments></PeripheralDllArguments>
             <InitializationFile></InitializationFile>
           </SimDlls>
           <TargetDlls>
             <CpuDll></CpuDll>
             <CpuDllArguments></CpuDllArguments>
             <PeripheralDll></PeripheralDll>
             <PeripheralDllArguments></PeripheralDllArguments>
             <InitializationFile></InitializationFile>
             <Driver>STLink\ST-LINKIII-KEIL_SWO.dll</Driver>
           </TargetDlls>
         </DebugOption>
         <Utilities>
           <Flash1>
             <UseTargetDll>1</UseTargetDll>
             <UseExternalTool>0</UseExternalTool>
             <RunIndependent>0</RunIndependent>
             <UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
             <Capability>1</Capability>
             <DriverSelection>4096</DriverSelection>
           </Flash1>
           <bUseTDR>1</bUseTDR>
           <Flash2>BIN\UL2CM3.DLL</Flash2>
           <Flash3>"" ()</Flash3>
           <Flash4></Flash4>
           <pFcarmOut></pFcarmOut>
           <pFcarmGrp></pFcarmGrp>
           <pFcArmRoot></pFcArmRoot>
           <FcArmLst>0</FcArmLst>
         </Utilities>
         <TargetArmAds>
           <ArmAdsMisc>
             <GenerateListings>0</GenerateListings>
             <asHll>1</asHll>
             <asAsm>1</asAsm>
             <asMacX>1</asMacX>
             <asSyms>1</asSyms>
             <asFals>1</asFals>
             <asDbgD>1</asDbgD>
             <asForm>1</asForm>
             <ldLst>0</ldLst>
             <ldmm>1</ldmm>
             <ldXref>1</ldXref>
             <BigEnd>0</BigEnd>
             <AdsALst>1</AdsALst>
             <AdsACrf>1</AdsACrf>
             <AdsANop>0</AdsANop>
             <AdsANot>0</AdsANot>
             <AdsLLst>1</AdsLLst>
             <AdsLmap>1</AdsLmap>
             <AdsLcgr>1</AdsLcgr>
             <AdsLsym>1</AdsLsym>
             <AdsLszi>1</AdsLszi>
             <AdsLtoi>1</AdsLtoi>
             <AdsLsun>1</AdsLsun>
             <AdsLven>1</AdsLven>
             <AdsLsxf>1</AdsLsxf>
             <RvctClst>0</RvctClst>
             <GenPPlst>0</GenPPlst>
             <AdsCpuType>"Cortex-M4"</AdsCpuType>
             <RvctDeviceName></RvctDeviceName>
             <mOS>0</mOS>
             <uocRom>0</uocRom>
             <uocRam>0</uocRam>
             <hadIROM>1</hadIROM>
             <hadIRAM>1</hadIRAM>
             <hadXRAM>0</hadXRAM>
             <uocXRam>0</uocXRam>
             <RvdsVP>2</RvdsVP>
             <hadIRAM2>1</hadIRAM2>
             <hadIROM2>0</hadIROM2>
             <StupSel>8</StupSel>
             <useUlib>0</useUlib>
             <EndSel>0</EndSel>
             <uLtcg>0</uLtcg>
             <RoSelD>3</RoSelD>
             <RwSelD>3</RwSelD>
             <CodeSel>0</CodeSel>
             <OptFeed>0</OptFeed>
             <NoZi1>0</NoZi1>
             <NoZi2>0</NoZi2>
             <NoZi3>0</NoZi3>
             <NoZi4>0</NoZi4>
             <NoZi5>0</NoZi5>
             <Ro1Chk>0</Ro1Chk>
             <Ro2Chk>0</Ro2Chk>
             <Ro3Chk>0</Ro3Chk>
             <Ir1Chk>1</Ir1Chk>
             <Ir2Chk>0</Ir2Chk>
             <Ra1Chk>0</Ra1Chk>
             <Ra2Chk>0</Ra2Chk>
             <Ra3Chk>0</Ra3Chk>
             <Im1Chk>1</Im1Chk>
             <Im2Chk>0</Im2Chk>
             <OnChipMemories>
               <Ocm1>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm1>
               <Ocm2>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm2>
               <Ocm3>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm3>
               <Ocm4>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm4>
               <Ocm5>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm5>
               <Ocm6>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </Ocm6>
               <IRAM>
                 <Type>0</Type>
                 <StartAddress>0x20000000</StartAddress>
                 <Size>0x20000</Size>
               </IRAM>
               <IROM>
                 <Type>1</Type>
                 <StartAddress>0x8000000</StartAddress>
                 <Size>0x100000</Size>
               </IROM>
               <XRAM>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </XRAM>
               <OCR_RVCT1>
                 <Type>1</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT1>
               <OCR_RVCT2>
                 <Type>1</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT2>
               <OCR_RVCT3>
                 <Type>1</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT3>
               <OCR_RVCT4>
                 <Type>1</Type>
                 <StartAddress>0x8000000</StartAddress>
                 <Size>0x100000</Size>
               </OCR_RVCT4>
               <OCR_RVCT5>
                 <Type>1</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT5>
               <OCR_RVCT6>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT6>
               <OCR_RVCT7>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT7>
               <OCR_RVCT8>
                 <Type>0</Type>
                 <StartAddress>0x0</StartAddress>
                 <Size>0x0</Size>
               </OCR_RVCT8>
               <OCR_RVCT9>
                 <Type>0</Type>
                 <StartAddress>0x20000000</StartAddress>
                 <Size>0x20000</Size>
               </OCR_RVCT9>
               <OCR_RVCT10>
                 <Type>0</Type>
                 <StartAddress>0x10000000</StartAddress>
                 <Size>0x10000</Size>
               </OCR_RVCT10>
             </OnChipMemories>
             <RvctStartVector></RvctStartVector>
           </ArmAdsMisc>
           <Cads>
             <interw>1</interw>
             <Optim>4</Optim>
             <oTime>1</oTime>
             <SplitLS>0</SplitLS>
             <OneElfS>0</OneElfS>
             <Strict>0</Strict>
             <EnumInt>0</EnumInt>
             <PlainCh>0</PlainCh>
             <Ropi>0</Ropi>
             <Rwpi>0</Rwpi>
             <wLevel>0</wLevel>
             <uThumb>0</uThumb>
             <uSurpInc>0</uSurpInc>
             <uC99>0</uC99>
             <useXO>0</useXO>
             <v6Lang>0</v6Lang>
             <v6LangP>0</v6LangP>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define>USE_STDPERIPH_DRIVER STM32F4XX ARM_MATH_CM4</Define>
               <Undefine></Undefine>
               <IncludePath>..\CodeUC;.\inc;.\src;C:\STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\ST\STM32F4xx\Include;C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\inc;C:\STM32F4-Discovery_FW_V1.1.0\Utilities\STM32F4-Discovery;C:\STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\Include</IncludePath>
             </VariousControls>
           </Cads>
           <Aads>
             <interw>1</interw>
             <Ropi>0</Ropi>
             <Rwpi>0</Rwpi>
             <thumb>0</thumb>
             <SplitLS>0</SplitLS>
             <SwStkChk>0</SwStkChk>
             <NoWarn>0</NoWarn>
             <uSurpInc>0</uSurpInc>
             <useXO>0</useXO>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define></Define>
               <Undefine></Undefine>
               <IncludePath></IncludePath>
             </VariousControls>
           </Aads>
           <LDads>
             <umfTarg>0</umfTarg>
             <Ropi>0</Ropi>
             <Rwpi>0</Rwpi>
             <noStLib>0</noStLib>
             <RepFail>1</RepFail>
             <useFile>0</useFile>
             <TextAddressRange>0x08000000</TextAddressRange>
             <DataAddressRange>0x20000000</DataAddressRange>
             <pXoBase></pXoBase>
             <ScatterFile></ScatterFile>
             <IncludeLibs></IncludeLibs>
             <IncludeLibsPath></IncludeLibsPath>
             <Misc></Misc>
             <LinkerInputFile></LinkerInputFile>
             <DisabledWarnings></DisabledWarnings>
           </LDads>
         </TargetArmAds>
       </TargetOption>
       <Groups>
         <Group>
           <GroupName>STM32_files</GroupName>
           <Files>
             <File>
               <FileName>system_stm32f4xx.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\system_stm32f4xx.c</FilePath>
             </File>
             <File>
               <FileName>startup_stm32f4xx.s</FileName>
               <FileType>2</FileType>
               <FilePath>.\startup_stm32f4xx.s</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
           <GroupName>SRC</GroupName>
           <Files>
             <File>
               <FileName>main.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\main.c</FilePath>
             </File>
             <File>
               <FileName>communication.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\communication.c</FilePath>
             </File>
             <File>
               <FileName>controller.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\controller.c</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
           <GroupName>STM32_libs</GroupName>
           <Files>
             <File>
               <FileName>stm32f4xx_gpio.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_gpio.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_rcc.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rcc.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_tim.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_tim.c</FilePath>
             </File>
             <File>
               <FileName>misc.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\misc.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_exti.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_exti.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_syscfg.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_syscfg.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_adc.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_adc.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_dac.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dac.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_dma.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dma.c</FilePath>
             </File>
             <File>
               <FileName>stm32f4xx_usart.c</FileName>
               <FileType>1</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_usart.c</FilePath>
             </File>
             <File>
               <FileName>arm_cortexM4lf_math.lib</FileName>
               <FileType>4</FileType>
               <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\Lib\ARM\arm_cortexM4lf_math.lib</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
           <GroupName>SRC_drivers</GroupName>
           <Files>
             <File>
               <FileName>adc.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\adc.c</FilePath>
             </File>
             <File>
               <FileName>callback_timers.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\callback_timers.c</FilePath>
             </File>
             <File>
               <FileName>dac.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\dac.c</FilePath>
             </File>
             <File>
               <FileName>h_bridge.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\h_bridge.c</FilePath>
             </File>
             <File>
               <FileName>incr_encoder.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\incr_encoder.c</FilePath>
             </File>
             <File>
               <FileName>uart.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\uart.c</FilePath>
             </File>
             <File>
               <FileName>hall.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\hall.c</FilePath>
             </File>
             <File>
               <FileName>strain_gauge.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\strain_gauge.c</FilePath>
             </File>
             <File>
               <FileName>tachometer.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\tachometer.c</FilePath>
             </File>
           </Files>
         </Group>
         <Group>
           <GroupName>SRC_lib</GroupName>
           <Files>
             <File>
               <FileName>basic_filter.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\lib\basic_filter.c</FilePath>
             </File>
             <File>
               <FileName>pid.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\lib\pid.c</FilePath>
             </File>
             <File>
               <FileName>utils.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\lib\utils.c</FilePath>
             </File>
+            <File>
+              <FileName>circular_buffer.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\lib\circular_buffer.c</FilePath>
+            </File>
           </Files>
         </Group>
       </Groups>
     </Target>
   </Targets>
 
 </Project>
diff --git a/Firmware/src/communication.c b/Firmware/src/communication.c
index 4ad4031..a3d2b8a 100644
--- a/Firmware/src/communication.c
+++ b/Firmware/src/communication.c
@@ -1,361 +1,378 @@
 #include "communication.h"
 #include "controller.h"
 #include "drivers/callback_timers.h"
 #include "drivers/adc.h"
 #include "drivers/dac.h"
 #include "drivers/incr_encoder.h"
 #include "drivers/hall.h"
 #include "drivers/strain_gauge.h"
 #include "drivers/tachometer.h"
 #include "drivers/uart.h"
 #include "lib/basic_filter.h"
 #include "lib/pid.h"
 #include "lib/utils.h"
 
-#include <stdarg.h>
 #include <stdio.h>
 
 extern uint32_t statusReg, ctrl_timestamp, ctrl_currentFilterType;
 extern float32_t ctrl_motorPosCod, ctrl_motorPosCod_c;
 extern float32_t ctrl_motorTorque_Nm_c, ctrl_motorPosCodFilt, ctrl_firstOrderFilterTau;
 extern float32_t hall_angle, tac_speed, sg_force;
 extern pid_Pid ctrl_positionPid;
 extern bfilt_BasicFilter ctrl_encoderFilter;
 extern float32_t ctrl_sineAmplitude, ctrl_sinePeriod;
 
 uint32_t selectedVariablesToStream; // Bitfield that indicates for each variable if it should be streamed or not.
 void* variablesArray[N_VARIABLES]; // Pointers to variables to stream.
 uint8_t txBuffer[1024];
 
+#define COMM_BUFFER_SIZE 1024
 #define DEBUG_MESSAGE_BUFFER_SIZE 1024
+
+uint8_t comm_packetTxBuffer[COMM_BUFFER_SIZE];
 char comm_debugMessageBuffer[DEBUG_MESSAGE_BUFFER_SIZE];
 
+cb_CircularBuffer *comm_rxQueue;
 uint8_t rxCurrentMessageType = PC_MESSAGE_DO_NOTHING; // Current message type for RX bytes.
 uint32_t rxBytesCount; // Number of received bytes for the current message.
 uint8_t firstHalfByte; // First half of the data byte to receive.
 uint8_t rxDataBytesBuffer[32]; // Data bytes received (ready to use, bytes already merged).
 
 void comm_SendPacket(uint8_t messageType, uint8_t *data, uint16_t dataLength);
 void comm_HandleByte(uint8_t rxData);
 void comm_Stream(void);
 uint32_t comm_GetVar(uint8_t variableIndex);
 
 /**
   * @brief Init the communication manager.
   */
 void comm_Init(void)
 {
     // Initialize the array with all the pointers on variables.
     // Note that this array contains pointers only to uint32_t, even if the
     // variables are actually float32_t. This does not matter, since we only use
     // these pointers to perform byte-to-byte copies.
     // Do not modify this by hand, copy-paste code generated by the excel sheet.
     variablesArray[VAR_TIMESTAMP] = (uint32_t*) &ctrl_timestamp;
     variablesArray[VAR_POS_REG_PID_P] = (uint32_t*) &ctrl_positionPid.kp;
     variablesArray[VAR_POS_REG_PID_I] = (uint32_t*) &ctrl_positionPid.ki;
     variablesArray[VAR_POS_REG_PID_D] = (uint32_t*) &ctrl_positionPid.kd;
     variablesArray[VAR_POS_REG_PID_ARW] = (uint32_t*) &ctrl_positionPid.arw;
     variablesArray[VAR_POS_REG_PID_INTEGRATOR] = (uint32_t*) &ctrl_positionPid.integrator;
     variablesArray[VAR_POS_REG_PID_OUTPUT] = (uint32_t*) &ctrl_motorTorque_Nm_c;
     variablesArray[VAR_POS_REG_SAMPLING_PERIOD] = NULL;
     variablesArray[VAR_WALL_POSITION] = (uint32_t*) &ctrl_motorPosCod_c;
     variablesArray[VAR_MEASURED_ANGLE_RAW] = NULL;
     variablesArray[VAR_MEASURED_ANGLE_FILTERED] = (uint32_t*) &ctrl_motorPosCodFilt;
     variablesArray[VAR_COMM_TX_PERIOD] = NULL;
     variablesArray[VAR_ACTIVE_FILTER] = (uint32_t*) &ctrl_currentFilterType;
     variablesArray[VAR_1ST_FILT_TAU] = (uint32_t*) &ctrl_encoderFilter.tau;
     variablesArray[VAR_DAC1_VOLTAGE] = NULL;
     variablesArray[VAR_DAC2_VOLTAGE] = NULL;
     variablesArray[VAR_ADC1_VOLTAGE] = NULL;
     variablesArray[VAR_ADC2_VOLTAGE] = NULL;
     variablesArray[VAR_HALL_ANGLE] = NULL;
     variablesArray[VAR_TACHO_SPEED] = NULL;
     variablesArray[VAR_STRAIN_GAUGE_FORCE] = NULL;
     variablesArray[VAR_USER_VAR_1] = NULL;
     variablesArray[VAR_USER_VAR_2] = NULL;
     variablesArray[VAR_USER_VAR_3] = NULL;
     variablesArray[VAR_USER_VAR_4] = NULL;
     variablesArray[VAR_USER_VAR_5] = NULL;
 
  
     // Setup the UART peripheral, and specify the function that will be called
     // each time a byte is received.
-    uart_Init(comm_HandleByte);
+    uart_Init();
+    comm_rxQueue = uart_GetRxQueue();
+    rxCurrentMessageType = PC_MESSAGE_DO_NOTHING;
 
     // Make the streaming function periodically called by the timer 7.
     cbt_SetCommLoopTimer(comm_Stream, TE_DATA_LOOP_DEFAULT_VAL);
 }
 
+void comm_Step(void)
+{
+    // Send the bytes in the TX queue, even if it is not full, to avoid latency.
+    uart_FlushTx();
+
+    // Interpret the bytes in the RX queue.
+    uart_Step();
+
+    while(!cb_IsEmpty(comm_rxQueue))
+        comm_HandleByte(cb_Pull(comm_rxQueue));
+}
+
 /**
   * @brief  UART data streaming "loop" function.
   */
 void comm_Stream()
 {
     // If the data streaming is enabled, send a stream packet to the PC.
     if(statusReg & DATA_UPSTREAM_ENB)
     {
         uint32_t i;
         int nDataBytesToSend = 0;
 
         for(i=0; i<N_VARIABLES; i++)
         {
             if(selectedVariablesToStream & (1<<i)) // If the variable has been selected for streaming...
             {
                 uint32_t variableValue;
                 variableValue = comm_GetVar(i);
                 memcpy(&txBuffer[nDataBytesToSend], &variableValue, 4);
                 nDataBytesToSend += 4;
             }
         }
 
         comm_SendPacket(STM_MESSAGE_STREAMING_PACKET, txBuffer, nDataBytesToSend);
     }
 }
 
 /**
   * @brief Get the four bytes of a monitored variable, from its index.
   * @param variableIndex: index of the variable to get.
   * @return the variable bytes as a uint32_t (even if actually float32_t).
   * @note This function returns 0 if the variableIndex does not correspond to a
   * valid variable to read.
   */
 uint32_t comm_GetVar(uint8_t variableIndex)
 {
     float32_t tmp;
     uint32_t variableValue;
                     
     // Note that some variable cannot be read directly, and are
     // treated as exceptions, because a function call is necessary.
     switch(variableIndex)
     {
     // Special case, function call needed.
     case VAR_POS_REG_SAMPLING_PERIOD:
         variableValue = cbt_GetPositionLoopPeriod();
         break;  
     case VAR_COMM_TX_PERIOD:
         variableValue = cbt_GetCommLoopPeriod();
         break;
     case VAR_DAC1_VOLTAGE:
         tmp = dac_GetVoltage1();
         variableValue = *(uint32_t*)&tmp;
         break;
     case VAR_DAC2_VOLTAGE:
         tmp = dac_GetVoltage2();
         variableValue = *(uint32_t*)&tmp;
         break;
     case VAR_ADC1_VOLTAGE:
         tmp = adc_GetChannelVoltage(ANIN1, ADC_SCALE_10_V);
         variableValue = *(uint32_t*)&tmp;
         break;
     case VAR_ADC2_VOLTAGE:
         tmp = adc_GetChannelVoltage(ANIN2, ADC_SCALE_10_V);
         variableValue = *(uint32_t*)&tmp;
         break;
     case VAR_MEASURED_ANGLE_RAW:
         tmp = enc_GetPosition();
         variableValue = *(uint32_t*)&tmp;
         break;
         
     // All the others are normal 32-bits variables.
     default:
         if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
             variableValue = *((uint32_t*)variablesArray[variableIndex]);
         else
             variableValue = 0;
         break;
     }
     
     return variableValue;
 }
 
 /**
   * @brief Set the four bytes of a monitored variable, from its index.
   * @param variableIndex: index of the variable to get.
   * @param newValue: new value for the variable.
   * @note This function does nothing if the variableIndex does not correspond to
   * a valid variable to write.
   */
 void comm_SetVar(uint8_t variableIndex, uint32_t newValue)
 {
     // Copy the new value to the selected 32-bits variable.
     // Note that some variable cannot be changed directly, and
     // are treated as exceptions, because a function call is necessary.
     switch(variableIndex)
     {
     // Special case, function call needed.
     case VAR_POS_REG_SAMPLING_PERIOD:
         cbt_SetPositionLoopPeriod(newValue);
         break;
     case VAR_COMM_TX_PERIOD:
         cbt_SetCommLoopPeriod(newValue);
         break;
     case VAR_DAC1_VOLTAGE:
         dac_SetVoltage1(*(float32_t*)&newValue);
         break;
     case VAR_DAC2_VOLTAGE:
         dac_SetVoltage2(*(float32_t*)&newValue);
         break;
     case VAR_MEASURED_ANGLE_RAW:
         enc_SetPosition(*(float32_t*)&newValue);
         break;
         
     // All the others are normal 32-bits variables.
     default: 
         if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
             memcpy(variablesArray[variableIndex], &newValue, 4);
         break;
     }
 }
 
 /**
-  * @brief  Send Packet with a header and data bytes.
-  * @param  messageType: message type (see the protocol doc).
+  * @brief  Sends Packet with a header and data bytes.
+  * @param  type: message type ("header" of the message).
   * @param  data: array of data bytes to be sent ("content" of the message).
   * @param  dataLength: number of data bytes to be sent.
   */
-void comm_SendPacket(uint8_t messageType, uint8_t *data, uint16_t dataLength)
+void comm_SendPacket(uint8_t type, uint8_t *data, uint16_t dataLength)
 {
     int i=0;
-    uart_SendByte((1<<7) | messageType);
+
+    uint8_t *p = comm_packetTxBuffer;
+
+    *p = ((1<<7) | type);
+    p++;
     
     for(i=0; i<dataLength; i++)
     {
         // Every data byte is splitted into two transmitted bytes.
-        uart_SendByte((data[i]&0xf0) >> 4); // Most significant bits.
-        uart_SendByte((data[i]&0x0f)); // Least significant bits.
+        *p = ((data[i]&0xf0) >> 4); // Most significant bits.
+        p++;
+        *p = ((data[i]&0x0f)); // Least significant bits.
+        p++;
     }
+
+    uart_SendBytesAsync(comm_packetTxBuffer, dataLength*2+1);
 }
 
 /**
   * @brief  Process the received byte to interpret the messages.
   * @param  rxData: the byte to be processed and interpreted.
   */
 void comm_HandleByte(uint8_t rxData)
 {
     if(rxData & (1<<7)) // The start byte has the most significant bit high.
     {
         rxCurrentMessageType = (rxData & ~(1<<7)); // Remove the start bit.
         rxBytesCount = 0;
     }
     else
         rxBytesCount++;
         
     if(rxBytesCount % 2 == 1) // First half of the data byte has been received.
         firstHalfByte = rxData; // Store it until the second half arrives.
     else // Second half of the data byte has been received (or no data bytes yet).
     {
         int dataBytesReady = rxBytesCount/2;
         rxDataBytesBuffer[dataBytesReady-1] = (firstHalfByte<<4) + (rxData & 0xf);
         
         switch(rxCurrentMessageType)
         {
         case PC_MESSAGE_DO_NOTHING:
             if(dataBytesReady == 0)
                 ; // Do nothing.
             break;
         
         case PC_MESSAGE_SEND_STATUS:
             if(dataBytesReady == 0)
                 comm_SendPacket(STM_MESSAGE_STATUS, (uint8_t*)(&statusReg), 4);
             break;
         
         case PC_MESSAGE_START_STREAMING:
             if(dataBytesReady == 0)
             {
                 statusReg |=    DATA_UPSTREAM_ENB;
                 TIM_SetCounter(TIM7, 0);
                 TIM_Cmd(TIM7, ENABLE);
             }
             break;
         
         case PC_MESSAGE_STOP_STREAMING:
             if(dataBytesReady == 0)
             {
                 TIM_Cmd(TIM7, DISABLE);
                 statusReg   &= ~DATA_UPSTREAM_ENB;
             }
             break;
         
             case PC_MESSAGE_GET_VAR:
                 if(dataBytesReady == 1)
                 {
                     uint32_t variableValue;
                     
                     // Extract the index, that indicates which variable will be sent.
                     uint8_t variableIndex = rxDataBytesBuffer[0];
                     
                     // If the variable index is out of range, ignore the request.
                     if(variableIndex >= N_VARIABLES)
                         return;
                 
                     // Prepare the message to be sent to the PC.
                     // First byte: variable index.
                     txBuffer[0] = variableIndex;
                     
                     // Four following bytes: actual value of the variable.
                     variableValue = comm_GetVar(variableIndex);
 
                     memcpy(&txBuffer[1], &variableValue, 4);
                 
                     // Send the message to the PC.
                     comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 5);
                 }
                 break;
             
             case PC_MESSAGE_SET_VAR:
                 if(dataBytesReady == 5)
                 {
                     // Extract the index (that indicates which variable will change),
                     // and the new value of the variable.
                     uint32_t newValue;
                     uint8_t variableIndex = rxDataBytesBuffer[0];
                     memcpy(&newValue, &rxDataBytesBuffer[1], 4);
                     
                     // Set the selected variable with the new value.
                     comm_SetVar(variableIndex, newValue);
                 }
                 break;
             
             case PC_MESSAGE_SET_STREAMED_VAR:
                 if(dataBytesReady == 4)
                 {
                     uint8_t *newSelectedVars = &rxDataBytesBuffer[0];
                     memcpy(&selectedVariablesToStream, newSelectedVars, 4);
                 }
                 break;
             
             default: // No data bytes for the other message types.
                 break;
         }
     }
 }
 
-void comm_SendDebugMessage(char format[], ...)
+/**
+ * @brief Sends a debug message to the computer.
+ * @param format format string.
+ * @param ... variables to be printed in the format string.
+ */
+void comm_SendDebugMessage(const char* format, ...)
 {
+    uint16_t length;
     va_list args;
-	va_start(args, format);
-	snprintf(comm_debugMessageBuffer, DEBUG_MESSAGE_BUFFER_SIZE,
-             format, args);
-	va_end(args);
+    
+    va_start(args, format);
+    
+	length = vsnprintf(comm_debugMessageBuffer, DEBUG_MESSAGE_BUFFER_SIZE,
+                       format, args);
     
     comm_SendPacket(STM_MESSAGE_DEBUG_TEXT, (uint8_t*)comm_debugMessageBuffer,
-                    strlen(comm_debugMessageBuffer));
-}
+                    length+1);
 
-#define comm_SendDebugMessageDecimatedII(decimation, COUNTER, message, ...) \
-	static int comm_decim##COUNTER = 0; \
-	if(comm_decim##COUNTER++ % decimation == 0) \
-		comm_SendDebugMessage(message, __VA_ARGS__);
-        
-#define comm_SendDebugMessageDecimatedI(decimation, format, ...) \
-    do { \
-        comm_SendDebugMessageDecimatedII(decimation, __COUNTER__, format, __VA_ARGS__) \
-        } while(0)
-        
-void comm_SendDebugMessageDecimated(int decimation, char format[], ...)
-{
-    va_list args;
-	va_start(args, format);
-	comm_SendDebugMessageDecimatedI(decimation, format, args);
-	va_end(args);
+    va_end(args);
 }
+
diff --git a/Firmware/src/communication.h b/Firmware/src/communication.h
index fa8228e..bf1c710 100644
--- a/Firmware/src/communication.h
+++ b/Firmware/src/communication.h
@@ -1,79 +1,57 @@
 #ifndef __COMMUNICATION_H
 #define __COMMUNICATION_H
 
 #include "main.h"
+#include "definitions.h"
 
-// Message IDs sent by host (PC) to the device (STM).
-#define PC_MESSAGE_DO_NOTHING 0 // Do nothing.
-#define PC_MESSAGE_TOGGLE_LED 1 // Toggle the LED (debug).
-#define PC_MESSAGE_SEND_STATUS 2 // Request the device to send the main status register.
-#define PC_MESSAGE_START_STREAMING 3 // Request the device to start streaming continuously.
-#define PC_MESSAGE_STOP_STREAMING 4 // Request the device to stop streaming continuously.
-#define PC_MESSAGE_GET_VAR 5 // Request the device to send the selected value.
-#define PC_MESSAGE_SET_VAR 6 // Set the selected variable.
-#define PC_MESSAGE_SET_STREAMED_VAR 7 // Set the variables to be streamed.
-
-// Message IDs sent by device (STM) to the device (PC).
-#define STM_MESSAGE_STATUS 0 // Main status register.
-#define STM_MESSAGE_VAR 1 // Variable state.
-#define STM_MESSAGE_STREAMING_PACKET 2 // Streaming packet.
-#define STM_MESSAGE_DEBUG_TEXT 3 // Debug text message.
-
-// Variables IDs.
-#define VAR_TIMESTAMP 0 // Timestamp. [us]
-#define VAR_POS_REG_PID_P 1 // Position regulator - proportionnal coefficient. [1]
-#define VAR_POS_REG_PID_I 2 // Position regulator - integral coefficient. [1]
-#define VAR_POS_REG_PID_D 3 // Position regulator - derivative coefficient. [1]
-#define VAR_POS_REG_PID_ARW 4 // Position regulator - ARW (integrator max value). [1]
-#define VAR_POS_REG_PID_INTEGRATOR 5 // Position regulator - integrator state. [1]
-#define VAR_POS_REG_PID_OUTPUT 6 // Position regulator - output. [mN.m]
-#define VAR_POS_REG_SAMPLING_PERIOD 7 // Position regulator - sampling period [us]
-#define VAR_WALL_POSITION 8 // Position regulator - target (wall position). [pulses]
-#define VAR_MEASURED_ANGLE_RAW 9 // Measured output angle, unfiltered. [deg]
-#define VAR_MEASURED_ANGLE_FILTERED 10 // Measured output angle, filtered. [deg]
-#define VAR_COMM_TX_PERIOD 11 // Communication loop - sending period. [us]
-#define VAR_ACTIVE_FILTER 12 // Active filter type (0: none, 1: first-order, 2: running mean). [None]
-#define VAR_1ST_FILT_TAU 13 // First order filter - time constant. [1]
-#define VAR_DAC1_VOLTAGE 14 // DAC 1 channel voltage. [V]
-#define VAR_DAC2_VOLTAGE 15 // DAC 2 channel voltage. [V]
-#define VAR_ADC1_VOLTAGE 16 // ACDC 1 channel voltage. [V]
-#define VAR_ADC2_VOLTAGE 17 // ACDC 2 channel voltage. [V]
-#define VAR_HALL_ANGLE 18 // Angle measured by the Hall sensor. [deg]
-#define VAR_TACHO_SPEED 19 // Angular speed measured by the tachometer. [deg/s]
-#define VAR_STRAIN_GAUGE_FORCE 20 // Force measured by the strain gauge. [?]
-#define VAR_USER_VAR_1 21 // User Variable 1 []
-#define VAR_USER_VAR_2 22 // User Variable 2 []
-#define VAR_USER_VAR_3 23 // User Variable 3 []
-#define VAR_USER_VAR_4 24 // User Variable 4 []
-#define VAR_USER_VAR_5 25 // User Variable 5 []
-
-#define N_VARIABLES 26 // Number of variables, used for enumeration of all variables.
+#include <stdarg.h>
 
 /** @defgroup Communication Main / Communication
   * @brief Control the communication with the computer.
   *
   * This module controls all the communication logic between the board and the
   * computer. It uses a specific communication protocol between the
   * computer and the board, with a system of messages. Thanks to this, the
   * the MATLAB application is able to get the value of selected variables on the
   * STM, and is even capable of modifiying them remotely.
   *
   * Make sure that the files communication.h/.c are up-to-date with the Excel
   * spreadsheet "Protocol description.xlsx".
   *
   * Call comm_Init() to setup this module. Its interrupt function will be called
   * automatically when a message arrives, or periodically when the data
   * streaming is enabled.
   *
   * @addtogroup Communication
   * @{
   */
 
 void comm_Init(void);
-void comm_SendDebugMessage(char format[], ...);
-void comm_SendDebugMessageDecimated(int decimation, char format[], ...);
+void comm_Step(void);
+void comm_SendDebugMessage(const char *format, ...);
+
+/**
+ * @brief Sends a debug message to the computer, with decimation.
+ * This macro is useful to print human-readable text, in a fast loop, to avoid
+ * overloading the communication bus, or the computer.
+ * @param decimation this macro will actually print once out of decimation, and
+ * do nothing otherwise.
+ * @param format format string. See the printf() documentation for format
+ * specification.
+ * @param ... variables to be printed in the format string.
+ */ 
+#define comm_SendDebugMessageDecimated(decimation, format, ...) \
+do \
+{ \
+    static int comm_decim##__COUNTER__ = 0; \
+    if(comm_decim##__COUNTER__++ % decimation == 0) \
+    { \
+        comm_SendDebugMessage(format, ##__VA_ARGS__); \
+    } \
+} while(0)
+
 /**
   * @}
   */
 
 #endif
diff --git a/Firmware/src/communication.h b/Firmware/src/definitions.h
similarity index 72%
copy from Firmware/src/communication.h
copy to Firmware/src/definitions.h
index fa8228e..9f263b2 100644
--- a/Firmware/src/communication.h
+++ b/Firmware/src/definitions.h
@@ -1,79 +1,50 @@
-#ifndef __COMMUNICATION_H
-#define __COMMUNICATION_H
-
-#include "main.h"
+#ifndef DEF_DEFINITIONS_H
+#define DEF_DEFINITIONS_H
 
 // Message IDs sent by host (PC) to the device (STM).
 #define PC_MESSAGE_DO_NOTHING 0 // Do nothing.
 #define PC_MESSAGE_TOGGLE_LED 1 // Toggle the LED (debug).
 #define PC_MESSAGE_SEND_STATUS 2 // Request the device to send the main status register.
 #define PC_MESSAGE_START_STREAMING 3 // Request the device to start streaming continuously.
 #define PC_MESSAGE_STOP_STREAMING 4 // Request the device to stop streaming continuously.
 #define PC_MESSAGE_GET_VAR 5 // Request the device to send the selected value.
 #define PC_MESSAGE_SET_VAR 6 // Set the selected variable.
 #define PC_MESSAGE_SET_STREAMED_VAR 7 // Set the variables to be streamed.
 
 // Message IDs sent by device (STM) to the device (PC).
 #define STM_MESSAGE_STATUS 0 // Main status register.
 #define STM_MESSAGE_VAR 1 // Variable state.
 #define STM_MESSAGE_STREAMING_PACKET 2 // Streaming packet.
 #define STM_MESSAGE_DEBUG_TEXT 3 // Debug text message.
 
 // Variables IDs.
 #define VAR_TIMESTAMP 0 // Timestamp. [us]
 #define VAR_POS_REG_PID_P 1 // Position regulator - proportionnal coefficient. [1]
 #define VAR_POS_REG_PID_I 2 // Position regulator - integral coefficient. [1]
 #define VAR_POS_REG_PID_D 3 // Position regulator - derivative coefficient. [1]
 #define VAR_POS_REG_PID_ARW 4 // Position regulator - ARW (integrator max value). [1]
 #define VAR_POS_REG_PID_INTEGRATOR 5 // Position regulator - integrator state. [1]
 #define VAR_POS_REG_PID_OUTPUT 6 // Position regulator - output. [mN.m]
 #define VAR_POS_REG_SAMPLING_PERIOD 7 // Position regulator - sampling period [us]
 #define VAR_WALL_POSITION 8 // Position regulator - target (wall position). [pulses]
 #define VAR_MEASURED_ANGLE_RAW 9 // Measured output angle, unfiltered. [deg]
 #define VAR_MEASURED_ANGLE_FILTERED 10 // Measured output angle, filtered. [deg]
 #define VAR_COMM_TX_PERIOD 11 // Communication loop - sending period. [us]
 #define VAR_ACTIVE_FILTER 12 // Active filter type (0: none, 1: first-order, 2: running mean). [None]
 #define VAR_1ST_FILT_TAU 13 // First order filter - time constant. [1]
 #define VAR_DAC1_VOLTAGE 14 // DAC 1 channel voltage. [V]
 #define VAR_DAC2_VOLTAGE 15 // DAC 2 channel voltage. [V]
 #define VAR_ADC1_VOLTAGE 16 // ACDC 1 channel voltage. [V]
 #define VAR_ADC2_VOLTAGE 17 // ACDC 2 channel voltage. [V]
 #define VAR_HALL_ANGLE 18 // Angle measured by the Hall sensor. [deg]
 #define VAR_TACHO_SPEED 19 // Angular speed measured by the tachometer. [deg/s]
 #define VAR_STRAIN_GAUGE_FORCE 20 // Force measured by the strain gauge. [?]
 #define VAR_USER_VAR_1 21 // User Variable 1 []
 #define VAR_USER_VAR_2 22 // User Variable 2 []
 #define VAR_USER_VAR_3 23 // User Variable 3 []
 #define VAR_USER_VAR_4 24 // User Variable 4 []
 #define VAR_USER_VAR_5 25 // User Variable 5 []
 
 #define N_VARIABLES 26 // Number of variables, used for enumeration of all variables.
 
-/** @defgroup Communication Main / Communication
-  * @brief Control the communication with the computer.
-  *
-  * This module controls all the communication logic between the board and the
-  * computer. It uses a specific communication protocol between the
-  * computer and the board, with a system of messages. Thanks to this, the
-  * the MATLAB application is able to get the value of selected variables on the
-  * STM, and is even capable of modifiying them remotely.
-  *
-  * Make sure that the files communication.h/.c are up-to-date with the Excel
-  * spreadsheet "Protocol description.xlsx".
-  *
-  * Call comm_Init() to setup this module. Its interrupt function will be called
-  * automatically when a message arrives, or periodically when the data
-  * streaming is enabled.
-  *
-  * @addtogroup Communication
-  * @{
-  */
-
-void comm_Init(void);
-void comm_SendDebugMessage(char format[], ...);
-void comm_SendDebugMessageDecimated(int decimation, char format[], ...);
-/**
-  * @}
-  */
-
 #endif
diff --git a/Firmware/src/drivers/callback_timers.c b/Firmware/src/drivers/callback_timers.c
index d1e98ea..1e4d267 100644
--- a/Firmware/src/drivers/callback_timers.c
+++ b/Firmware/src/drivers/callback_timers.c
@@ -1,230 +1,227 @@
 #include "callback_timers.h"
 #include "../controller.h"
 #include "../lib/utils.h"
 
 cbt_PeriodicTaskFunc cbt_tim1Task, cbt_tim6Task, cbt_tim7Task;
 volatile float32_t cbt_ucLoad; // Processor load (%).
 
 void tim1InitFunc(void);
 void tim67InitFunc(void);
 
 /**
   * @brief  Initialize the timers to call an interrupt routine periodically.
   */
 void cbt_Init(void)
-{
-    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC|RCC_AHB1Periph_GPIOD|RCC_AHB1Periph_GPIOE, ENABLE);
-    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4); // ???
-    
+{    
     // Initialize the timers.
     tim1InitFunc();
     tim67InitFunc();
     
     // Initialize the function pointers to NULL, in order to be able to test
     // if they are already affected or not.
     cbt_tim1Task = NULL;
     cbt_tim6Task = NULL;
     cbt_tim7Task = NULL;
 }
 
 /**
   * @brief  Set the function to call periodically by the timer 1.
   * @param  f: the function to call periodically.
   * @param  period: the period between each call of f [us].
   */
 void cbt_SetCurrentLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
 {
     cbt_tim1Task = f;
     
     utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
     TIM1->ARR = (uint16_t) period;
 }
 
 /**
   * @brief  Set the function to call periodically by the timer 6.
   * @param  f: the function to call periodically.
   * @param  period: the period between each call of f [us].
   */
 void cbt_SetPositionLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
 {
     cbt_tim6Task = f;
     
     utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
     TIM6->ARR = (uint16_t) period;
 }
 
 /**
   * @brief  Set the function to call periodically by the timer 7.
   * @param  f: the function to call periodically.
   * @param  period: the period between each call of f [us].
   */
 void cbt_SetCommLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
 {
     cbt_tim7Task = f;
     
     utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
     TIM7->ARR = (uint16_t) period;
 }
 
 /**
   * @brief  Initialize TIM1 used for timing the current loop (resolution 1us).
   */
 void tim1InitFunc(void)
 {
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
     //TIM_OCInitTypeDef   TIM_OCInitStruct;
 
     NVIC_InitTypeDef NVIC_InitStruct;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,  ENABLE);
     
     NVIC_InitStruct.NVIC_IRQChannel                   = TIM1_UP_TIM10_IRQn;
     NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = CURRENT_LOOP_IRQ_PRIORITY;
     NVIC_InitStruct.NVIC_IRQChannelSubPriority        = 0;
     NVIC_InitStruct.NVIC_IRQChannelCmd                = ENABLE;
     NVIC_Init(&NVIC_InitStruct);
 
     TIM_TimeBaseStruct.TIM_Period        = (uint16_t)(TE_CURRENT_LOOP_DEFAULT_VAL-1);    
     TIM_TimeBaseStruct.TIM_Prescaler     = TIM1_PRESCALER;
     TIM_TimeBaseStruct.TIM_ClockDivision = 0; // TIM_CKD_DIV2;
     TIM_TimeBaseStruct.TIM_CounterMode   = TIM_CounterMode_Up;
     TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStruct);
 
     TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);      
     TIM_Cmd(TIM1, ENABLE);
 }
 
 /**
   * @brief  Initialize TIM6, for timing the main control loop (resolution 1us)
   *         Initialize TIM7, for timing the data transmission loop (resolution 1us)
   */
 void tim67InitFunc(void)
 {
     TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
     NVIC_InitTypeDef        NVIC_InitStruct;
 
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6|RCC_APB1Periph_TIM7, ENABLE);
 
     NVIC_InitStruct.NVIC_IRQChannel                   = TIM6_DAC_IRQn;
     NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = CONTROL_LOOP_IRQ_PRIORITY;
     NVIC_InitStruct.NVIC_IRQChannelSubPriority        = 0;
     NVIC_InitStruct.NVIC_IRQChannelCmd                = ENABLE;
     NVIC_Init(&NVIC_InitStruct);
     
     NVIC_InitStruct.NVIC_IRQChannel                   = TIM7_IRQn;
     NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = DATA_LOOP_IRQ_PRIORITY;
     NVIC_InitStruct.NVIC_IRQChannelSubPriority        = 0;
     NVIC_Init(&NVIC_InitStruct);    
 
     TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
     TIM_TimeBaseStruct.TIM_Prescaler     = TIM6_PRESCALER;
     TIM_TimeBaseStruct.TIM_Period        = (uint16_t)(TE_CONTROL_LOOP_DEFAULT_VAL-1);
     TIM_TimeBaseStruct.TIM_ClockDivision = 0;
     TIM_TimeBaseStruct.TIM_CounterMode   = TIM_CounterMode_Up;
     TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStruct);
 
     TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
     TIM_TimeBaseStruct.TIM_Prescaler     = TIM7_PRESCALER;
     TIM_TimeBaseStruct.TIM_Period        = (uint16_t)(TE_DATA_LOOP_DEFAULT_VAL-1);
     TIM_TimeBaseStruct.TIM_ClockDivision = 0;
     TIM_TimeBaseStruct.TIM_CounterMode   = TIM_CounterMode_Up;
     TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStruct);
 
     TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE);
     TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
     TIM_Cmd(TIM6, ENABLE);
     TIM_Cmd(TIM7, DISABLE);
 }
 
 /**
   * @brief  Interrupt from current control loop timer (TIM1)
   */
 void TIM1_UP_TIM10_IRQHandler(void)
 {
 	if(TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
     {
         if(cbt_tim1Task != NULL)
             cbt_tim1Task();
 		
 		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
 	}
 }
 
 /**
   * @brief  Interrupt from main control loop timer (TIM6)
   */
 void TIM6_DAC_IRQHandler(void)
 {
 	if(TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
     {
         if(cbt_tim6Task != NULL)
             cbt_tim6Task();
         
         // Percentage of time consumed by the control task  0..100%.
 		cbt_ucLoad = (((float32_t)(TIM6->CNT))*100)/((float32_t)(TIM6->ARR));
 
 		TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
 	}
 }
 
 
 /**
   * @brief  Interrupt from data transmission loop timer (TIM7) (data loop)
   */
 void TIM7_IRQHandler(void)
 {
 	if(TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
     {
 		if(cbt_tim7Task != NULL)
             cbt_tim7Task();
         
 		TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
 	}
 }
 
 /**
   * @brief  Set the period of the position loop.
   * @param  period: the new period of the position loop [us].
   */
 void cbt_SetPositionLoopPeriod(uint32_t period)
 {
     utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
     TIM6->ARR = period;
 }
 
 /**
   * @brief  Set the period of the communication loop.
   * @param  period: the new period of the communication loop [us].
   */
 void cbt_SetCommLoopPeriod(uint32_t period)
 {
     utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
     TIM7->ARR = period;
 }
 
 /**
   * @brief  Get the period of the current loop.
   * @return the period of the current loop [us].
   */
 uint32_t cbt_GetCurrentLoopPeriod(void)
 {
     return TIM1->ARR;
 }
 
 /**
   * @brief  Get the period of the position loop.
   * @return the period of the position loop [us].
   */
 uint32_t cbt_GetPositionLoopPeriod(void)
 {
     return TIM6->ARR;
 }
 
 /**
   * @brief  Get the period of the communication loop.
   * @return the period of the communication loop [us].
   */
 uint32_t cbt_GetCommLoopPeriod(void)
 {
     return TIM7->ARR;
 }
diff --git a/Firmware/src/drivers/uart.c b/Firmware/src/drivers/uart.c
index 1460998..8030485 100644
--- a/Firmware/src/drivers/uart.c
+++ b/Firmware/src/drivers/uart.c
@@ -1,95 +1,281 @@
 #include "uart.h"
+#include "../lib/utils.h"
+#include "../communication.h" // TODO: REMOVE. DEBUG ONLY.
 
-uart_rxByteHandlerFunc uart_rxHandler;
+#define UART_RX_DMA DMA1_Stream5
+#define UART_RX_DMA_CHANNEL DMA_Channel_4
+#define UART_TX_DMA DMA1_Stream6
+#define UART_TX_DMA_CHANNEL DMA_Channel_4
+
+#define UART_TX_BUFFER_SIZE 512
+#define UART_RX_BUFFER_SIZE 512
+#define UART_USER_RX_QUEUE_SIZE 512
+
+uint8_t uart_txBuffer[2][UART_TX_BUFFER_SIZE];
+uint8_t uart_currentTxBufferToWriteTo;
+uint16_t uart_txBufferIndex;
+uint16_t uart_nBytesToTransferDma;
+
+uint8_t uart_rxBuffer[UART_RX_BUFFER_SIZE];
+uint8_t const * uart_rxBuffTail;
+
+uint8_t uart_userRxQueue[UART_USER_RX_QUEUE_SIZE];
+cb_CircularBuffer uart_rxQueue;
+
+#define UART_DMA_TX_IS_BUSY (DMA_GetCmdStatus(UART_TX_DMA) == ENABLE && DMA_GetCurrDataCounter(UART_TX_DMA) > 0)
 
 /**
-  * @brief  Initialize the UART module.
-  * @param  f: a pointer to the function to be called each time a byte arrives.
+  * @brief Initializes the UART module.
   */
-void uart_Init(uart_rxByteHandlerFunc f)
+void uart_Init(void)
 {
-    GPIO_InitTypeDef  GPIO_InitStruct;
+    GPIO_InitTypeDef GPIO_InitStruct;
     USART_InitTypeDef USART_InitStruct;   
-    NVIC_InitTypeDef  NVIC_InitStruct;
-
-    // Periph clock enable
+    DMA_InitTypeDef DMA_InitStruct;
+    
+    // Enable UART and DMA peripherals clocks.
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
 
-    // GPIO (PC10 & PC11)
+    // Setup GPIOs as UART pins.
     GPIO_InitStruct.GPIO_Pin   = USART_TX_Pin;
     GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AF;
     GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
     GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
     GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_UP;
     GPIO_Init(USART_TX_Port, &GPIO_InitStruct);
     GPIO_PinAFConfig(USART_TX_Port, USART_TX_PinSource, GPIO_AF_USART2);
     
     GPIO_InitStruct.GPIO_Pin = USART_RX_Pin;
     GPIO_Init(USART_RX_Port, &GPIO_InitStruct);     
     GPIO_PinAFConfig(USART_RX_Port, USART_RX_PinSource, GPIO_AF_USART2);
 
-    // UART config
+    // Setup the UART peripheral.
     USART_InitStruct.USART_BaudRate            = USART_BAUDRATE;
     USART_InitStruct.USART_WordLength          = USART_WordLength_8b;
-    USART_InitStruct.USART_StopBits            = USART_StopBits_1; // 1 stop bit (standard)
+    USART_InitStruct.USART_StopBits            = USART_StopBits_1;
     USART_InitStruct.USART_Parity              = USART_Parity_No;
     USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
-    USART_InitStruct.USART_Mode                = USART_Mode_Tx|USART_Mode_Rx;  //Two way datas
-    USART_Init(USART2, &USART_InitStruct);
+    USART_InitStruct.USART_Mode                = USART_Mode_Tx | USART_Mode_Rx;
+    USART_Init(USART_PC_COMM, &USART_InitStruct);
+    
+    USART_Cmd(USART_PC_COMM, ENABLE);
+    
+    // Setup the DMA for RX.
+    DMA_DeInit(UART_RX_DMA);
     
-    // USART Rx interrupt
-    USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); 
-    NVIC_InitStruct.NVIC_IRQChannel                   = USART2_IRQn;
-    NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = UART_RX_IRQ_PRIORIY;
-    NVIC_InitStruct.NVIC_IRQChannelSubPriority        = 0;         
-    NVIC_InitStruct.NVIC_IRQChannelCmd                = ENABLE;            
-    NVIC_Init(&NVIC_InitStruct);
+    DMA_InitStruct.DMA_Channel = UART_RX_DMA_CHANNEL;
+    DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
+    DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_rxBuffer[0];
+    DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
+    DMA_InitStruct.DMA_BufferSize = UART_RX_BUFFER_SIZE;
+    DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
+    DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+    DMA_InitStruct.DMA_Mode = DMA_Mode_Circular;
+    DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
+    DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
+    DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+    DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+    
+    DMA_Init(UART_RX_DMA, &DMA_InitStruct);
 
-    USART_Cmd(USART2, ENABLE);
+    USART_DMACmd(USART_PC_COMM, USART_DMAReq_Rx | USART_DMAReq_Tx, ENABLE);
+    DMA_ITConfig(UART_RX_DMA, DMA_IT_TC, ENABLE);
+    DMA_Cmd(UART_RX_DMA, ENABLE);
+    
+    uart_rxBuffTail = &uart_rxBuffer[0];
+    
+    // Setup the DMA for TX.
+    DMA_DeInit(UART_TX_DMA);
+    
+    DMA_InitStruct.DMA_Channel = UART_TX_DMA_CHANNEL;
+    DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
+    DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_txBuffer[0][0];
+    DMA_InitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+    DMA_InitStruct.DMA_BufferSize = UART_TX_BUFFER_SIZE;
+    DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
+    DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+    DMA_InitStruct.DMA_Mode = DMA_Mode_Normal;
+    DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
+    DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
+    DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+    DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+    
+    DMA_Init(UART_TX_DMA, &DMA_InitStruct);
 
-    // Function called when a byte arrives, to process it.
-    uart_rxHandler = f;
+    USART_DMACmd(USART_PC_COMM, USART_DMAReq_Rx | USART_DMAReq_Tx, ENABLE);
+    DMA_ITConfig(UART_TX_DMA, DMA_IT_TC, ENABLE);
+    //DMA_Cmd(UART_TX_DMA, ENABLE);
+    
+    // Initialize the RX circular buffer.
+    cb_Init(&uart_rxQueue, uart_userRxQueue, UART_USER_RX_QUEUE_SIZE);
+    
+    // Initialize the variables for the UART TX.
+    uart_currentTxBufferToWriteTo = 0;
+    uart_txBufferIndex = 0;
+    uart_nBytesToTransferDma = 0;
 }
 
 /**
-  * @brief  Get the received byte, and send it to the specified RX handler.
-  * @note   Called by the received byte interrupt.
-  */
-void USART2_IRQHandler(void)
+ * @brief Copies the received bytes into the user-accessible queue.
+ * Reads all the available bytes in the DMA RX buffer, and copies them to the
+ * user-accessible queue.
+ * @remark This function must called often, otherwise the DMA RX buffer may be
+ * full.
+ */
+void uart_Step(void)
 {
-    if(USART_GetITStatus(USART2, USART_IT_RXNE))
+    // Get the location of the location currently pointed by the DMA.
+    uint8_t const * head = uart_rxBuffer + UART_RX_BUFFER_SIZE
+                           - DMA_GetCurrDataCounter(UART_RX_DMA);
+                           
+    // Even if the STM32F4 reference manual (RM0090) states that the NDTR
+    // register is decremented after the transfer, this is not the case.
+    // So we wait a few cycles to be sure that the DMA actually performed the
+    // transfer.
+    utils_DelayUs(1);
+
+    // RX: add the received bytes into the user queue.
+    while(uart_rxBuffTail != head)
     {
-        uint8_t rxData = USART2->DR; // Read the DataRegister (also clears the RX interrupt pending bit).
-        
-        // Call the external RX byte handler.
-        uart_rxHandler(rxData);
+        uint8_t b =  *uart_rxBuffTail;
+        cb_Push(&uart_rxQueue, b);
+
+        uart_rxBuffTail++;
+
+        if(uart_rxBuffTail >= uart_rxBuffer + UART_RX_BUFFER_SIZE)
+            uart_rxBuffTail -= UART_RX_BUFFER_SIZE;
     }
 }
 
+/**
+ * @brief Gets the user-accessible queue of the received bytes.
+ * @return the address of the queue.
+ */
+cb_CircularBuffer* uart_GetRxQueue(void)
+{
+    return &uart_rxQueue;
+}
 
 /**
-  * @brief  Sent a byte through UART.
-  * @param  data: byte to send through UART.
-  * @note This function blocks until the byte is sent.
-  */
-void uart_SendByte(uint8_t data)
+ * @brief Starts the DMA transfer to send bytes to UART peripheral.
+ */
+void uart_StartDma(void)
 {
-    while( !(USART_PC_COMM->SR & 0x00000040) ) // Wait until data register is empty.
-        ;
-    USART_SendData(USART_PC_COMM, data);
+    DMA_InitTypeDef DMA_InitStruct;
+    
+    //
+    uart_nBytesToTransferDma = uart_txBufferIndex;
+    uart_currentTxBufferToWriteTo = !uart_currentTxBufferToWriteTo;
+    uart_txBufferIndex = 0;
+
+    // Start the DMA transfer.
+    DMA_DeInit(UART_TX_DMA);
+    
+    DMA_InitStruct.DMA_Channel = UART_TX_DMA_CHANNEL;
+    DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
+    DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_txBuffer[!uart_currentTxBufferToWriteTo][0];
+    DMA_InitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+    DMA_InitStruct.DMA_BufferSize = uart_nBytesToTransferDma;
+    DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
+    DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+    DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+    DMA_InitStruct.DMA_Mode = DMA_Mode_Normal;
+    DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
+    DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
+    DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+    DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+    
+    DMA_Init(UART_TX_DMA, &DMA_InitStruct);
+                          
+    DMA_Cmd(UART_TX_DMA, ENABLE);
 }
 
 
 /**
-  * @brief  Sent an array of bytes through UART.
-  * @note This function blocks until all the bytes are sent.
-  * @param  data: bytes array to send through UART.
-  * @param  length: number of bytes in the array.
-  */
-void uart_SendBytes(uint8_t *data, int length)
+ * @brief Asynchronously sends the given byte through the UART bus.
+ * @param data the data byte to send.
+ */
+void uart_SendByteAsync(uint8_t data)
+{
+    // Check that it is possible to write in the current buffer.
+    if(uart_txBufferIndex >= UART_TX_BUFFER_SIZE)
+    {
+        // If the DMA is idle, switch to the other buffer.
+        if(UART_DMA_TX_IS_BUSY)
+        {
+            utils_TrapCpu(); // Error, can't write anywhere!
+            return;
+        }
+        else
+            uart_StartDma();
+    }
+
+    // Write the byte in the buffer.
+    uart_txBuffer[uart_currentTxBufferToWriteTo][uart_txBufferIndex] = data;
+    uart_txBufferIndex++;
+
+    // If the buffer is full, start the DMA transfer.
+    if(uart_txBufferIndex == UART_TX_BUFFER_SIZE)
+        uart_StartDma();
+}
+
+/**
+ * @brief Asynchronously sends the given bytes through the UART bus.
+ * @param data pointer to the data bytes array to send.
+ * @param length number of bytes to send (array size).
+ * @remark The bytes may not be send immediately, they are stored temporarily in
+ * an intermediate buffer that the DMA will copy to UART peripheral, when it is
+ * full. Call uart_FlushTx() to start the DMA transfer immediately.
+ */
+void uart_SendBytesAsync(uint8_t *data, int length)
 {
-    int i;
+    while(length > 0)
+    {
+        uint16_t nBytesToWriteInBuf;
     
-    for(i=0; i<length; i++)
-        uart_SendByte(data[i]);
+        // Check that it is possible to write in the current buffer.
+        if(uart_txBufferIndex >= UART_TX_BUFFER_SIZE)
+        {
+            // If the DMA is idle, switch to the other buffer.
+            if(UART_DMA_TX_IS_BUSY)
+            {
+                utils_TrapCpu(); // Error, can't write anywhere!
+                return;
+            }
+            else
+                uart_StartDma();
+        }
+
+        // Write as many bytes as possible in the current buffer.
+        nBytesToWriteInBuf = UART_TX_BUFFER_SIZE - uart_txBufferIndex;
+
+        if(nBytesToWriteInBuf > length)
+            nBytesToWriteInBuf = length;
+
+        memcpy(&uart_txBuffer[uart_currentTxBufferToWriteTo][uart_txBufferIndex],
+               data, nBytesToWriteInBuf);
+        uart_txBufferIndex += nBytesToWriteInBuf;
+
+        data += nBytesToWriteInBuf;
+        length -= nBytesToWriteInBuf;
+
+        // If the buffer is full, start the DMA transfer.
+        if(uart_txBufferIndex == UART_TX_BUFFER_SIZE)
+            uart_StartDma();
+    }
+}
+
+/**
+ * @brief Start the DMA to send the bytes waiting in the intermediate buffer.
+ */
+void uart_FlushTx(void)
+{
+    if(uart_txBufferIndex > 0 && !UART_DMA_TX_IS_BUSY)
+        uart_StartDma();
 }
diff --git a/Firmware/src/drivers/uart.h b/Firmware/src/drivers/uart.h
index 9999532..8a83d89 100644
--- a/Firmware/src/drivers/uart.h
+++ b/Firmware/src/drivers/uart.h
@@ -1,48 +1,52 @@
 #ifndef __UART_H
 #define __UART_H
 
 #include "../main.h"
+#include "../lib/circular_buffer.h"
 
 #define USART_RX_Pin								GPIO_Pin_5
 #define USART_RX_PinSource					GPIO_PinSource5
 #define USART_RX_Port								GPIOD
 #define USART_TX_Pin								GPIO_Pin_6
 #define USART_TX_PinSource					GPIO_PinSource6
 #define USART_TX_Port								GPIOD
 
 #define USART_PC_COMM  USART2 // UART peripheral used for the comm with the PC.
 #define USART_BAUDRATE 1843200 //921600 // USART baudrate [b/s].
 
 /** @defgroup UART Driver / UART
   * @brief Driver for the UART serial communication peripheral.
   *
   * This driver controls the UART peripheral of the STM32, connected to the
   * USB-to-serial chip.
   *
   * Call uart_Init() first in the initialization code, specifiying the function
   * to call when a byte arrives (sent from the computer). To send data, call
   * uart_SendByte() or uart_SendBytes().
   *
   * Note that this module should not be used directly. It is a better option to
   * use it through the Communication module, to benefit from the already
   * implemented communication protocol between the board and MATLAB.
   *
   * @addtogroup UART
   * @{
   */
 
 /**
   * Typedef for a pointer to a function to call automatically when a byte
   * arrives from the computer.
   */
 typedef void (*uart_rxByteHandlerFunc)(uint8_t rxByte);
 
-void uart_Init(uart_rxByteHandlerFunc f);
-void uart_SendByte(uint8_t data);
-void uart_SendBytes(uint8_t *data, int length);
+void uart_Init(void);
+void uart_Step(void);
+cb_CircularBuffer* uart_GetRxQueue(void);
+void uart_SendByteAsync(uint8_t data);
+void uart_SendBytesAsync(uint8_t *data, int length);
+void uart_FlushTx(void);
 
 /**
   * @}
   */
 
 #endif
diff --git a/Firmware/src/lib/circular_buffer.c b/Firmware/src/lib/circular_buffer.c
new file mode 100644
index 0000000..3f847b6
--- /dev/null
+++ b/Firmware/src/lib/circular_buffer.c
@@ -0,0 +1,109 @@
+#include "circular_buffer.h"
+#include "utils.h"
+
+/**
+ * @brief Initializes a cb_CircularBuffer structure.
+ * Initializes a cb_CircularBuffer structure with the given buffer. The buffer
+ * has to be provided by the user, to avoid dynamic memory allocation.
+ * @param cb the cb_CircularBuffer structure to initialize.
+ * @param buffer pointer to an existing array.
+ * @param bufferSize length of the circular buffer. The given buffer should have
+ * a size greater or equal to this value.
+ */
+void cb_Init(cb_CircularBuffer* cb, uint8_t *buffer, uint16_t bufferSize)
+{
+    cb->buffer = buffer;
+    cb->bufferSize = bufferSize;
+    cb->readIndex = 0;
+    cb->writeIndex = 0;
+}
+
+/**
+ * @brief Gets the number of bytes stored in the queue.
+ * @param cb the cb_CircularBuffer to check.
+ * @return the number of bytes stored in the queue.
+ */
+uint16_t cb_ItemsCount(cb_CircularBuffer* cb)
+{
+    if (cb->writeIndex >= cb->readIndex)
+        return cb->writeIndex - cb->readIndex;
+    else
+        return cb->bufferSize - cb->readIndex + cb->writeIndex;
+}
+
+/**
+ * @brief Check if the queue is empty.
+ * @param cb the cb_CircularBuffer to check.
+ * @return 1 if there are no bytes stored in the queue, 0 otherwise.
+ */
+bool cb_IsEmpty(cb_CircularBuffer* cb)
+{
+    return cb->writeIndex == cb->readIndex;
+}
+
+/**
+ * @brief Check if the queue is full.
+ * @param cb the cb_CircularBuffer to check.
+ * @return 1 if the queue is full, 0 otherwise.
+ */
+bool cb_IsFull(cb_CircularBuffer* cb)
+{
+    if (cb->writeIndex == cb->readIndex)
+        return false;
+    else if (cb->readIndex < cb->writeIndex)
+        return (cb->readIndex == 0 && cb->writeIndex == cb->bufferSize - 1);
+    else
+        return (cb->writeIndex == cb->readIndex-1);
+}
+
+/**
+ * @brief Add a item at the back of the queue.
+ * @param cb the cb_CircularBuffer to affect.
+ * @param newElem the byte to add to the back of the queue.
+ * @warning If the queue is already full, this function does nothing if
+ * CPU_TRAPS_ENABLED is 0. If CPU_TRAPS_ENABLED is 1, this function will block
+ * forever the program execution, so the problem can be found with the
+ * debugger.
+ */
+void cb_Push(cb_CircularBuffer* cb, uint8_t newElem)
+{
+    if (!cb_IsFull(cb)) // Not full.
+    {
+        cb->buffer[cb->writeIndex] = newElem;
+        cb->writeIndex++;
+
+        if (cb->writeIndex >= cb->bufferSize)
+            cb->writeIndex = 0;
+    }
+    else // Error, can't push an item, because the queue is full.
+        utils_TrapCpu();
+}
+
+/**
+ * @brief Extract the item at the front of the queue.
+ * Returns the value of the item at the front of the queue, and remove this item
+ * from the queue.
+ * @param cb the cb_CircularBuffer to affect.
+ * @return the value of the byte that has been extracted from the queue.
+ * @warning If the queue is empty and CPU_TRAPS_ENABLED is 0, this function
+ * returns 0. If CPU_TRAPS_ENABLED is 1, this function will block forever the
+ * program execution, so the problem can be found with the debugger.
+ */
+uint8_t cb_Pull(cb_CircularBuffer* cb)
+{
+    if (cb->writeIndex != cb->readIndex) // Not empty.
+    {
+        uint8_t pulled = cb->buffer[cb->readIndex];
+        cb->readIndex++;
+
+        if (cb->readIndex >= cb->bufferSize)
+            cb->readIndex = 0;
+
+        return pulled;
+    }
+    else // Error, can't pull an item, because the queue is empty.
+    {
+        utils_TrapCpu();
+        return 0;
+    }
+}
diff --git a/Firmware/src/lib/circular_buffer.h b/Firmware/src/lib/circular_buffer.h
new file mode 100644
index 0000000..5d628ba
--- /dev/null
+++ b/Firmware/src/lib/circular_buffer.h
@@ -0,0 +1,40 @@
+#ifndef __CIRCULAR_BUFFER_H
+#define __CIRCULAR_BUFFER_H
+
+#include "../main.h"
+
+/** @defgroup CircularBuffer Circular buffer
+  * @brief Bytes queue (FIFO) implemented with a circular buffer.
+  *
+  * This bytes container works as a queue, which means "first in, first out".
+  * Create a cb_CircularBuffer structure, and initialize it with cb_Init().
+  * Then, add bytes using cb_Push(), and extract them with cb_Pull().
+  *
+  * @ingroup Lib
+  * @addtogroup CircularBuffer
+  * @{
+  */
+
+/**
+ * @brief Circular buffer structure.
+ */
+typedef struct
+{
+    uint8_t *buffer; ///< Pointer to the byte buffer.
+    uint16_t bufferSize; ///< Size of buffer.
+    volatile uint16_t readIndex, ///< Index of the element at the front of the queue.
+                      writeIndex; ///< Index of the next free location at the end of the queue.
+} cb_CircularBuffer;
+
+void cb_Init(cb_CircularBuffer* cb, uint8_t *buffer, uint16_t bufferSize);
+uint16_t cb_ItemsCount(cb_CircularBuffer* cb);
+bool cb_IsEmpty(cb_CircularBuffer* cb);
+bool cb_IsFull(cb_CircularBuffer* cb);
+void cb_Push(cb_CircularBuffer* cb, uint8_t newElem);
+uint8_t cb_Pull(cb_CircularBuffer* cb);
+
+/**
+  * @}
+  */
+
+#endif
diff --git a/Firmware/src/lib/utils.c b/Firmware/src/lib/utils.c
index 3a1a5f5..2b8ac8b 100644
--- a/Firmware/src/lib/utils.c
+++ b/Firmware/src/lib/utils.c
@@ -1,87 +1,99 @@
 #include "utils.h"
 
+/**
+ * @brief Endless loop function to stop the execution of the program here.
+ * @note This function does nothing if CPU_TRAPS_ENABLED is set to zero.
+ */
+void utils_TrapCpu(void)
+{
+#if CPU_TRAPS_ENABLED
+    while(1)
+        ;
+#endif
+}
+
 /**
   * @brief  "Busy wait" delay function 
   * @param  duration: Delay time in [us] (approximative value based on a 168MHz core clock)
   */
 void utils_DelayUs(uint32_t duration)
 {
 	uint32_t i 	= 0;
 	uint32_t j 	= 0;
     
 	for(i=0; i<=duration; i++)
     {
 		for(j=0; j<=26; j++)
         {
 			__asm{NOP};
 			__asm{NOP};
 		}
 	}
 }
 
 
 /**
   * @brief  "Busy wait"  delay function 
   * @param  duration: Delay time in [ms] (approximative value based on a 168MHz core clock).
   * @note This delay is approximative, and may last longer if there are many interrupts.
   */
 void utils_DelayMs(uint32_t duration)
 {
 	uint32_t i 	= 0;
 	uint32_t j 	= 0;
 	for(i=0; i<=duration; i++)
     {
 		for(j=0; j<=33600; j++)
         {
 			__asm{NOP};
 		}
 	}
 }
 
 
 /**
   * @brief  Saturate a float number between two bounds.
   * @param  val: value to constrain between two limits.
   * @param	min: minimum
   * @param	max: maximum
   * @retval None.
   */
 void utils_SaturateF(float32_t *val, float32_t min, float32_t max)
 {
 	if(*val < min)
 		*val = min;
 	else if(*val > max)
 		*val = max;
 }
 
 /**
   * @brief  Saturate an integer number between two bounds.
   * @param  val: value to constrain between the two limits.
   * @param	min: lower limit.
   * @param	max: upper limit.
   * @retval None.
   */
 void utils_SaturateU(uint32_t *val, uint32_t min, uint32_t max)
 {
 	if(*val < min)
 		*val = min;
 	else if(*val > max)
 		*val = max;
 }
 
 /**
   * @brief  Compute the mean of the array values.
   * @param  array: array of float number to get the mean from.
   * @param	size: size of the array.
   * @retval the mean of the array values.
   */
 float32_t utils_Mean(float32_t *array, int size)
 {
     int i;
     float32_t sum = 0.0f;
     
     for(i=0; i<size; i++)
         sum += array[i];
     
     return sum / (float32_t) size;
 }
diff --git a/Firmware/src/lib/utils.h b/Firmware/src/lib/utils.h
index cea14dc..71f5c4e 100644
--- a/Firmware/src/lib/utils.h
+++ b/Firmware/src/lib/utils.h
@@ -1,29 +1,36 @@
 #ifndef __UTILS_H
 #define __UTILS_H
 
 #include "main.h"
 
 /** @defgroup Utils Lib / Utils
   * @brief Utility functions and constants.
   *
   * This module provides functions and constants that may be used at many
   * places in the code.
   *
   * @addtogroup Utils
   * @{
   */
 
+#define CPU_TRAPS_ENABLED 1 ///< utils_TrapCpu() will be block forever if 1, or return immediately if 0.
+
 #define SECOND_TO_MICROSECOND 1000000.0f
 #define MICROSECOND_TO_SECOND (1.0f/SECOND_TO_MICROSECOND)
 
+void utils_TrapCpu(void);
+
+void utils_DelayUs(uint32_t duration);
+void utils_DelayMs(uint32_t duration);
+
 void utils_DelayUs(uint32_t duration);
 void utils_DelayMs(uint32_t duration);
 void utils_SaturateF(float32_t *val, float32_t min, float32_t max);
 void utils_SaturateU(uint32_t *val, uint32_t min, uint32_t max);
 float32_t utils_Mean(float32_t *array, int size);
 
 /**
   * @}
   */
 
 #endif
diff --git a/Firmware/src/main.c b/Firmware/src/main.c
index e61e1fe..3472032 100644
--- a/Firmware/src/main.c
+++ b/Firmware/src/main.c
@@ -1,53 +1,62 @@
 #include "main.h"
 #include "communication.h"
 #include "controller.h"
 #include "drivers/adc.h"
 #include "drivers/callback_timers.h"
 #include "drivers/dac.h"
 #include "drivers/h_bridge.h"
 #include "drivers/hall.h"
 #include "drivers/incr_encoder.h"
 #include "drivers/strain_gauge.h"
 #include "drivers/tachometer.h"
 #include "lib/utils.h"
 
 volatile uint32_t statusReg = 0x00000000; // Main state register.
 
 /**
   * @brief  Main function, setups all the drivers and controllers.
   */
 int main(void)
 {
+    // Setup the GPIOs and the interrupts.
+    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB |
+                           RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD |
+                           RCC_AHB1Periph_GPIOE, ENABLE);
+    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
+
+    //
     cbt_Init(); // Setup the timers that will call the loops functions.
     
 	adc_Init(); // Setup the ADC.
 	
 	comm_Init(); // Setup the communication module.
     
     ctrl_Init(); // Setup the controllers.
 	
     enc_Init(); // Incremental encoders.		
     hb_Init(); // H-bridge.
     hb_Enable();
     
     // Delay to let the power electronics stabilize before calibrating the
     // current sensor.
     utils_DelayMs(200);
     
     adc_CalibrateCurrentSens();
     statusReg |= RUN_CURRENT_LOOP;
     
     dac_Init();
     
     // Init external sensors drivers, connected to the ADC.
     hall_Init(ANIN1);
     //sg_Init(ANIN2);
     //tac_Init(ANIN2);
-	
-	// Endless loop. It is empty, because all the control is performed in the
-	// functions called periodically by the interrupts.
+
+    comm_SendDebugMessage("Initialization finished.");
+
+	// Endless loop. The low priority functions are called here.
 	while(1)
 	{
-		
+		// Update the communication.
+        comm_Step();
 	}
 }
diff --git a/Firmware/src/main.h b/Firmware/src/main.h
index e994352..5b48018 100644
--- a/Firmware/src/main.h
+++ b/Firmware/src/main.h
@@ -1,97 +1,98 @@
 /** \mainpage HRI board firmware documentation
  * \section intro_sec Introduction
  * This is the documentation of the HRI board firmware. You will find a
  * description of the provided library and drivers. As all the hardware and
  * software is very new, please do not hesitate to suggest improvements to the
  * course assistants (Romain Baud, Philipp Hörler and Laurent Jenni).
  *
  * \section code_struct_sec Code structure
  * The code is divided into 3 parts:
  *  - main: this is where the controllers and the communication protocol are
  * implemented. The call of all the initialization functions is also made here.
  *  - library: all the algorithms, data structures that could be used at many
  * places, are put into the library.
  *  - drivers: there is one driver per peripheral. All the hardware-specific
  * code is wrapped into these files, so that the user does not have to use a 
  *
  * One module (controller, communication, library or driver) is always splitted
  * in two files, .h and .c, like in C++.
  * @image html soft_architecture.png
  *
  * \section lib_how_to How to use the provided library
  * For most of the driver/library modules, call x_Init() only once, when the
  * program starts (typically in the main()). Then, you can call
  * x_Step()/x_Set()/x_Get() everytime as needed, typically in the control
  * functions called periodically (ctrl_RegulatePosition()...).
  * 
  * See the "Modules" section of this documentation to see how they work.
  */
 
 #ifndef __MAIN_H
 #define __MAIN_H
 
 #include "stm32f4_discovery.h"
 #include <math.h>
+#include <stdbool.h>
 #include "arm_math.h"
 
 // Clocks configuration.
 #define APB1_PRESCALER 4
 #define APB2_PRESCALER 2
 #define TIM_MULTIPLIER 2
 
 // Interupt priority.
 #define CURRENT_LOOP_IRQ_PRIORITY 1 // High freq loop, should not be interrupted.
 #define CONTROL_LOOP_IRQ_PRIORITY 2
 #define CODER_INDEX_IRQ_PRIORITY  2 // Useless, remove?
 #define UART_RX_IRQ_PRIORIY       3
 #define DATA_LOOP_IRQ_PRIORITY    4 // Streaming packets, lowest priority.
 
 // Electrical parameters.
 #define STM_SUPPLY_VOLTAGE 3.3f // [V].
 #define H_BRIDGE_SUPPLY_VOLTAGE 24.0f // [V].
 #define CURRENT_SHUNT_RESISTANCE 0.01f // [ohm].
 #define CURRENT_SHUNT_AMPLIFIER_GAIN 50 // [].
 #define MOTOR_RESISTANCE 10.6f + 5.0f // 10.6 ohm according to the datasheet, actually more, depends on the motor [ohm].
 
 // Mechanical parameters.
 #define REDUCTION_RATIO 16.5f
 #define MOTOR_TORQUE_CONST 0.0538f // [N.m/A].
 
 // Status register bit list.
 #define STREG_0              ((uint32_t)0x00000001)
 #define STREG_1              ((uint32_t)0x00000002)
 #define REF_POS_INIT         ((uint32_t)0x00000004) // 1 -> Ref position succesfully found
 #define CALIBRE              ((uint32_t)0x00000008) // 1 -> Calibration done
 #define RUN_CURRENT_LOOP     ((uint32_t)0x00000010) // 1 -> Current loop active (if motorDrv eneable but RUN_CURRENT_LOOP=false -> fixed 50% PWM on the motor)
 #define UN_CONTROL_LOOP      ((uint32_t)0x00000020) // 1 -> Control loop active
 #define STREG_6              ((uint32_t)0x00000040)
 #define STREG_7              ((uint32_t)0x00000080)
 
 #define STREG_8              ((uint32_t)0x00000100)
 #define DATA_UPSTREAM_ENB    ((uint32_t)0x00000200) // 1 -> data upstream active
 #define STREG_10             ((uint32_t)0x00000400)
 #define RX_COMPLETED         ((uint32_t)0x00000800) // 1 -> packet recieved
 #define RX_ERROR             ((uint32_t)0x00001000) // 1 -> last received packet incorrect
 #define PROCESSING_REQUEST   ((uint32_t)0x00002000) // 1 -> processing last received packet in progress (can not receive new data)
 #define LAST_REQUEST_IGNORED ((uint32_t)0x00004000)
 #define STREG_15             ((uint32_t)0x00008000)
 
 #define STREG_16             ((uint32_t)0x00010000)
 #define STREG_17             ((uint32_t)0x00020000)
 #define STREG_18             ((uint32_t)0x00040000)
 #define STREG_19             ((uint32_t)0x00080000)
 #define STREG_20             ((uint32_t)0x00100000)
 #define STREG_21             ((uint32_t)0x00200000)
 #define STREG_22             ((uint32_t)0x00400000)
 #define STREG_23             ((uint32_t)0x00800000)
 
 #define INDEX_ERROR          ((uint32_t)0x01000000) // 1 -> wrong step count over 1 rotation
 #define STREG_25             ((uint32_t)0x02000000)
 #define STREG_26             ((uint32_t)0x04000000)
 #define STREG_27             ((uint32_t)0x08000000)
 #define STREG_28             ((uint32_t)0x10000000)
 #define STREG_29             ((uint32_t)0x20000000)
 #define STREG_30             ((uint32_t)0x40000000)
 #define STREG_31             ((uint32_t)0x80000000)
 
 #endif