diff --git a/Firmware/carteHRI.uvoptx b/Firmware/carteHRI.uvoptx
index ccc8985..8f2521d 100644
--- a/Firmware/carteHRI.uvoptx
+++ b/Firmware/carteHRI.uvoptx
@@ -1,775 +1,814 @@
 <?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 -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>uart_txBuffer[0]</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
           <ItemText>uart_txBuffer[1]</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
           <ItemText>demoSineOutput</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
           <ItemText>ctrl_motorPosHall</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
           <ItemText>ctrl_timestamp</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>
+    <File>
+      <GroupNumber>3</GroupNumber>
+      <FileNumber>17</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_i2c.c</PathWithFileName>
+      <FilenameWithoutPath>stm32f4xx_i2c.c</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>
+      <FileNumber>18</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>
+      <FileNumber>19</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>
+      <FileNumber>20</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>
+      <FileNumber>21</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>
+      <FileNumber>22</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>
+      <FileNumber>23</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>
+      <FileNumber>24</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>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\led.c</PathWithFileName>
       <FilenameWithoutPath>led.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\debug_gpio.c</PathWithFileName>
       <FilenameWithoutPath>debug_gpio.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <Focus>0</Focus>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>.\src\drivers\button.c</PathWithFileName>
       <FilenameWithoutPath>button.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>28</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <Focus>0</Focus>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>.\src\drivers\i2c.c</PathWithFileName>
+      <FilenameWithoutPath>i2c.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>4</GroupNumber>
+      <FileNumber>29</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <Focus>0</Focus>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>.\src\drivers\mpu_6050.c</PathWithFileName>
+      <FilenameWithoutPath>mpu_6050.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>27</FileNumber>
+      <FileNumber>30</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>28</FileNumber>
+      <FileNumber>31</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>29</FileNumber>
+      <FileNumber>32</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>30</FileNumber>
+      <FileNumber>33</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 2a6b74b..43e992c 100644
--- a/Firmware/carteHRI.uvprojx
+++ b/Firmware/carteHRI.uvprojx
@@ -1,591 +1,606 @@
 <?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>
+            <Optim>1</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>
+            <File>
+              <FileName>stm32f4xx_i2c.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_i2c.c</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>led.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\led.c</FilePath>
             </File>
             <File>
               <FileName>debug_gpio.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\debug_gpio.c</FilePath>
             </File>
             <File>
               <FileName>button.c</FileName>
               <FileType>1</FileType>
               <FilePath>.\src\drivers\button.c</FilePath>
             </File>
+            <File>
+              <FileName>i2c.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\drivers\i2c.c</FilePath>
+            </File>
+            <File>
+              <FileName>mpu_6050.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\drivers\mpu_6050.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/drivers/i2c.c b/Firmware/src/drivers/i2c.c
new file mode 100644
index 0000000..4c4df61
--- /dev/null
+++ b/Firmware/src/drivers/i2c.c
@@ -0,0 +1,246 @@
+#include "i2c.h"
+
+#include "stm32f4xx_i2c.h"
+
+#include "../lib/utils.h"
+
+const uint32_t TIMEOUT = 3200; ///< Approximate maximum time for a I2C operation to complete (approx. 100us).
+
+/**
+ * @brief Repeats until the expression returns false, or the timeout is reached.
+ */
+#define WAIT_WITH_TIMEOUT(x) \
+do \
+{ \
+    uint32_t duration = 0;\
+    while((x)) \
+    { \
+        if(duration++ > TIMEOUT) \
+        { \
+            i2c_Reset(); \
+            if(ok != NULL) \
+                *ok = false; \
+            return; \
+        } \
+    } \
+} while(0)
+
+/**
+ * @brief Initializes the I2C bus.
+ */
+void i2c_Init(void)
+{
+    GPIO_InitTypeDef GPIO_InitStruct;
+    I2C_InitTypeDef I2C_InitStruct;
+    
+    // Enable the peripheral clock.
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+
+    // Setup the pins as I2C SDA/SCL.
+    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AF;
+    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
+    GPIO_InitStruct.GPIO_OType = GPIO_OType_OD;
+    GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+    
+    GPIO_InitStruct.GPIO_Pin = I2C_SDA_Pin;
+    GPIO_Init(I2C_SDA_Port, &GPIO_InitStruct);
+    
+    GPIO_InitStruct.GPIO_Pin = I2C_SCL_Pin;
+    GPIO_Init(I2C_SCL_Port, &GPIO_InitStruct);
+    
+    GPIO_PinAFConfig(I2C_SDA_Port, I2C_SDA_PinSource, I2C_GPIO_AF);  
+    GPIO_PinAFConfig(I2C_SCL_Port, I2C_SCL_PinSource, I2C_GPIO_AF);
+    
+    // Setup the I2C peripheral.
+    I2C_InitStruct.I2C_ClockSpeed = 100000; // [Hz].
+    I2C_InitStruct.I2C_Mode = I2C_Mode_I2C;
+    I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2;
+    I2C_InitStruct.I2C_OwnAddress1 = 0x00;
+    I2C_InitStruct.I2C_Ack = I2C_Ack_Enable;
+    I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
+    
+    I2C_DeInit(I2C_PERIPH);
+    I2C_Init(I2C_PERIPH, &I2C_InitStruct);
+    I2C_Cmd(I2C_PERIPH, ENABLE);
+    
+    //
+    utils_DelayUs(50);
+    I2C_GenerateSTART(I2C_PERIPH, ENABLE);
+    utils_DelayUs(50);
+    I2C_GenerateSTART(I2C_PERIPH, DISABLE);
+    utils_DelayUs(50);
+    
+    I2C_GenerateSTOP(I2C_PERIPH, ENABLE);
+    utils_DelayUs(50);
+    I2C_GenerateSTOP(I2C_PERIPH, DISABLE);
+    utils_DelayUs(50);
+}
+
+/**
+ * @brief Performs a stop condition, to abort the current transaction.
+ */
+void i2c_Reset(void)
+{
+    I2C_GenerateSTOP(I2C_PERIPH, ENABLE);
+    utils_DelayUs(50);
+    I2C_GenerateSTOP(I2C_PERIPH, DISABLE);
+    utils_DelayUs(50);
+}
+
+/**
+ * @brief Writes a 8-bit value to a register.
+ * @param slaveAddress 7-bit I2C slave address.
+ * @param registerAddress 8-bit register address.
+ * @param registerValue value to write to the 8-bit register.
+ * @param ok the value pointed will be set to true if the operation completed
+ * successfully, or to false if an error occured. If a NULL pointer is given,
+ * then it is ignored.
+ */
+void i2c_WriteRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                       uint8_t registerValue, bool *ok)
+{
+    i2c_WriteMultiBytesRegister(slaveAddress, registerAddress,
+                                &registerValue, 1, ok);
+}
+
+/**
+ * @brief Writes several bytes to a register.
+ * @param slaveAddress 7-bit I2C slave address.
+ * @param registerAddress 8-bit register address.
+ * @param registerValue values array to write to the 8-bit register.
+ * @param registerSize number of bytes to write.
+ * @param ok the value pointed will be set to true if the operation completed
+ * successfully, or to false if an error occured. If a NULL pointer is given,
+ * then it is ignored.
+ */
+void i2c_WriteMultiBytesRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                                 uint8_t const *registerValue,
+                                 uint8_t registerSize, bool *ok)
+{
+    int i;
+
+    // Wait until the I2C peripheral is ready.
+    WAIT_WITH_TIMEOUT(I2C_GetFlagStatus(I2C_PERIPH, I2C_FLAG_BUSY));
+  
+	// Send a start condition, and wait until the slave has acknowledged.
+    I2C_GenerateSTART(I2C_PERIPH, ENABLE);
+    WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_MODE_SELECT));
+
+	// Send slave address and write mode, and wait until the slave has
+    // acknowledged.
+    I2C_Send7bitAddress(I2C_PERIPH, slaveAddress<<1, I2C_Direction_Transmitter);
+    WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
+    
+    // Write the register address.
+    I2C_SendData(I2C_PERIPH, registerAddress);
+    WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+    
+    // Write the register content.
+    for(i=0; i<registerSize; i++)
+    {
+        I2C_SendData(I2C_PERIPH, registerValue[i]);
+        WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+    }
+    
+    // Release the bus.
+    I2C_GenerateSTOP(I2C_PERIPH, ENABLE);
+    
+    // Operation successful.
+    if(ok != NULL)
+        *ok = true;
+}
+
+/**
+ * @brief Reads a 8-bit value from a register.
+ * @param slaveAddress 7-bit I2C slave address.
+ * @param registerAddress 8-bit register address.
+ * @param ok the value pointed will be set to true if the operation completed
+ * successfully, or to false if an error occured. If a NULL pointer is given,
+ * then it is ignored.
+ * @return the register value. If the operation failed (*ok set to false), then
+ * 0 is returned.
+ */
+uint8_t i2c_ReadRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                         bool *ok)
+{
+    uint8_t registerValue = 0;
+
+    i2c_ReadMultiBytesRegister(slaveAddress, registerAddress,
+                               &registerValue, 1, ok);
+                                       
+    return registerValue;
+}
+
+/**
+ * @brief Reads a multi bytes value from a register.
+ * @param slaveAddress 7-bit I2C slave address.
+ * @param registerAddress 8-bit register address.
+ * @param readData array that will hold the read data. Its size should be at
+ * least registerSize.
+ * @param registerSize number of bytes to read.
+ * @param ok the value pointed will be set to true if the operation completed
+ * successfully, or to false if an error occured. If a NULL pointer is given,
+ * then it is ignored.
+ */
+void i2c_ReadMultiBytesRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                                uint8_t *readData, uint8_t registerSize,
+                                bool *ok)
+{
+    int i;
+    
+    // Wait until the I2C peripheral is ready.
+    WAIT_WITH_TIMEOUT(I2C_GetFlagStatus(I2C_PERIPH, I2C_FLAG_BUSY));
+  
+	// Send a start condition, and wait until the slave has acknowledged.
+	I2C_GenerateSTART(I2C_PERIPH, ENABLE);
+	WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_MODE_SELECT));
+		
+	// Send slave address and write mode, and wait until the slave has
+    // acknowledged.
+	I2C_Send7bitAddress(I2C_PERIPH, slaveAddress<<1, I2C_Direction_Transmitter);
+	WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
+    
+    // Write the register address.
+    I2C_SendData(I2C_PERIPH, registerAddress);
+    WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
+        
+    // Restart condition.
+    I2C_GenerateSTOP(I2C_PERIPH, ENABLE);
+    utils_DelayUs(5);
+    I2C_GenerateSTOP(I2C_PERIPH, DISABLE);
+    utils_DelayUs(5);
+    
+    I2C_GenerateSTART(I2C_PERIPH, ENABLE);
+    utils_DelayUs(5);
+    I2C_GenerateSTART(I2C_PERIPH, DISABLE);
+    utils_DelayUs(5);
+    
+    // Send slave address and write mode, and wait until the slave has
+    // acknowledged.
+	I2C_Send7bitAddress(I2C_PERIPH, slaveAddress<<1, I2C_Direction_Receiver);
+	WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
+    
+    // Read the register content.
+    for(i=0; i<registerSize; i++)
+    {
+        if(i < registerSize - 1) // Not the last byte, need to acknowledge.
+        {
+            I2C_AcknowledgeConfig(I2C_PERIPH, ENABLE);
+            WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_RECEIVED));
+            readData[i] = I2C_ReceiveData(I2C_PERIPH);
+        }
+        else // Last byte to read, no acknowledge.
+        {
+            I2C_AcknowledgeConfig(I2C_PERIPH, DISABLE);
+            WAIT_WITH_TIMEOUT(!I2C_CheckEvent(I2C_PERIPH, I2C_EVENT_MASTER_BYTE_RECEIVED));
+            readData[i] = I2C_ReceiveData(I2C_PERIPH);
+        }
+    }
+    
+    // Release the bus.
+    I2C_GenerateSTOP(I2C_PERIPH, ENABLE);
+
+    // Operation successful.
+    if(ok != NULL)
+        *ok = true;
+}
diff --git a/Firmware/src/drivers/i2c.h b/Firmware/src/drivers/i2c.h
new file mode 100644
index 0000000..f6f835a
--- /dev/null
+++ b/Firmware/src/drivers/i2c.h
@@ -0,0 +1,47 @@
+#ifndef __I2C_H
+#define __I2C_H
+
+#include "../main.h"
+
+#define I2C_SDA_Pin        GPIO_Pin_9
+#define I2C_SDA_PinSource  GPIO_PinSource9
+#define I2C_SDA_Port       GPIOB
+#define I2C_SCL_Pin        GPIO_Pin_8
+#define I2C_SCL_PinSource  GPIO_PinSource8
+#define I2C_SCL_Port       GPIOB
+
+#define I2C_PERIPH I2C1
+#define I2C_GPIO_AF GPIO_AF_I2C1
+
+/** @defgroup I2C Driver / I2C
+  * @brief Driver for the I2C bus communication peripheral.
+  *
+  * This driver controls the I2C peripheral of the STM32, that is accessible
+  * from the digital extension connector of the HRI board.
+  *
+  * Call i2c_Init() first in the initialization code. Then, the communication
+  * function i2c_readRegister() or i2c_writeRegister() can be used.
+  * Note that both functions are synchronous, hence blocking the execution until
+  * the operation completes.
+  *
+  * @addtogroup I2C
+  * @{
+  */
+
+void i2c_Init(void);
+void i2c_WriteRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                       uint8_t registerValue, bool *ok);
+void i2c_WriteMultiBytesRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                                 uint8_t const *registerValue,
+                                 uint8_t registerSize, bool *ok);
+uint8_t i2c_ReadRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                         bool *ok);
+void i2c_ReadMultiBytesRegister(uint8_t slaveAddress, uint8_t registerAddress,
+                                uint8_t *readData, uint8_t registerSize,
+                                bool *ok);
+
+/**
+  * @}
+  */
+
+#endif
diff --git a/Firmware/src/drivers/mpu_6050.c b/Firmware/src/drivers/mpu_6050.c
new file mode 100644
index 0000000..3ce84a5
--- /dev/null
+++ b/Firmware/src/drivers/mpu_6050.c
@@ -0,0 +1,212 @@
+#include "mpu_6050.h"
+
+#include <limits.h>
+
+#include "i2c.h"
+#include "../lib/utils.h"
+
+#define GRAVITY_INTENSITY 9.81f
+#define SINT16_MAX_F ((float)USHRT_MAX)
+
+#define SLAVE_ADDRESS 0x68 ///< 0x68 if the AD0 pin is low, 0x69 otherwise.
+
+#define REG_CONFIG       0x1A ///< Configuration register.
+#define REG_GYRO_CONFIG  0x1B ///< Gyroscope configuration register.
+#define REG_ACCEL_CONFIG 0x1C ///< Accelerometer configuration register.
+#define REG_SMPLRT_DIV   0x19 ///< Sample rate divider register.
+#define REG_ACCEL_VALUES 0x3b ///< Accelerometer measurements register.
+#define REG_TEMP_VALUES  0x41 ///< Temperature measurements register.
+#define REG_GYRO_VALUES  0x43 ///< Gyroscope measurements register.
+#define REG_SIG_PATH_RST 0x68 ///< Signal path reset register.
+#define REG_USER_CTRL    0x6A ///< User control register.
+#define REG_POWER_MGMT_1 0x6B ///< Power management 1 register.
+#define REG_WHO_AM_I     0x75 ///< Who am I register.
+
+#define WHO_AM_I_VAL 0x68 ///< Expected value of the REG_WHO_AM_I register.
+
+/// Gets the actual range value from a mpu_AccelRange enum value.
+const float MPU_ACCEL_RANGE_REG_TO_CONV_FACTOR[] =
+{
+     2.0f / SINT16_MAX_F * GRAVITY_INTENSITY,
+     4.0f / SINT16_MAX_F * GRAVITY_INTENSITY,
+     8.0f / SINT16_MAX_F * GRAVITY_INTENSITY,
+    16.0f / SINT16_MAX_F * GRAVITY_INTENSITY
+};
+
+/// Gets the actual range value from a mpu_GyroRange enum value.
+const float MPU_GYRO_RANGE_REG_TO_CONV_FACTOR[] =
+{
+     250.0f / SINT16_MAX_F,
+     500.0f / SINT16_MAX_F,
+    1000.0f / SINT16_MAX_F,
+    2000.0f / SINT16_MAX_F
+};
+
+bool mpu_initialized = false; ///< True if the MPU-6050 is initialized, false otherwise.
+
+float accelFactor, ///< Converts a raw 16-bit integer value to an acceleration [m/s^2].
+      gyroFactor; ///< Converts a raw 16-bit integer value to an angular speed [deg/s].
+
+/**
+ * @brief Initializes the MPU-6050 device.
+ * @param accelRange accelerometer range.
+ * @param gyroRange gyroscope range.
+ * @param bandwidth bandwith of the accelerometer and gyroscope.
+ * @return true if the initialization was successful, false otherwise.
+ */
+bool mpu_Init(mpu_AccelRange accelRange, mpu_GyroRange gyroRange,
+              mpu_Bandwidth bandwidth)
+{
+    uint16_t id;
+    
+    //
+    accelFactor = MPU_ACCEL_RANGE_REG_TO_CONV_FACTOR[accelRange];
+    gyroFactor = MPU_GYRO_RANGE_REG_TO_CONV_FACTOR[gyroRange];
+
+    // Reset the MPU-6050.
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1, (1<<7)|(1<<6), NULL); // Device reset.
+    utils_DelayMs(100);
+
+    // Test the communication with the chip.
+    id = i2c_ReadRegister(SLAVE_ADDRESS, REG_WHO_AM_I, NULL);
+
+    if(id != WHO_AM_I_VAL) // Error.
+        return false;
+
+    // Reset the MPU-6050 again.
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1,
+                      (1<<7)|(1<<6), NULL); // Device reset.
+    utils_DelayMs(100);
+
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_SIG_PATH_RST,
+                      (1<<2)|(1<<1)|(1<<0), NULL); // Signal path reset.
+    utils_DelayMs(100);
+
+    // Setup the MPU-60X0 registers.
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_USER_CTRL,
+                      (0<<6) | // Disable FIFO.
+                      (0<<5) | // Disable master mode for the external I2C sensor.
+                      (0<<4) | // Do not disable the I2C slave interface (mandatory for the MPU-6050).
+                      (0<<2) | // Do not reset the FIFO.
+                      (0<<1) | // Do not reset the I2C.
+                      (0<<0),  // Do not reset the signal paths for the sensors.
+                      NULL);
+
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_POWER_MGMT_1,
+                      (0<<7) | // Do not reset the device.
+                      (0<<6) | // Disable sleep.
+                      (0<<5) | // Disable the "cycle" mode.
+                      (0<<3) | // Do not disable the temperature sensor.
+                      (3<<0),  // PLL with Z axis gyro ref.
+                      NULL);
+
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_GYRO_CONFIG,
+                      (0<<7) | // Do not perform self-test for axis X.
+                      (0<<6) | // Do not perform self-test for axis Y.
+                      (0<<5) | // Do not perform self-test for axis Z.
+                      (gyroRange<<3), // Gyroscope range.
+                      NULL);
+
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_ACCEL_CONFIG,
+                      (0<<7) | // Do not perform self-test for axis X.
+                      (0<<6) | // Do not perform self-test for axis Y.
+                      (0<<5) | // Do not perform self-test for axis Z.
+                      (accelRange<<3), // Accelerometer range.
+                      NULL);
+
+    i2c_WriteRegister(SLAVE_ADDRESS, REG_CONFIG,
+                      (0<<3) | // Disable synchronisation with FSYNC pin.
+                      (bandwidth<<0), // Accelerometer and gyroscope bandwidth.
+                      NULL);
+
+    if(bandwidth == MPU_DLPF_BW_256HZ)
+        i2c_WriteRegister(SLAVE_ADDRESS, REG_SMPLRT_DIV, 7, NULL);
+    else
+        i2c_WriteRegister(SLAVE_ADDRESS, REG_SMPLRT_DIV, 0, NULL);
+        
+    //
+    mpu_initialized = true;
+        
+    return true;
+}
+
+/**
+ * @brief Acquires the acceleration from the IMU.
+ * @param ax variable to write the X-axis acceleration on [m/s^2].
+ * @param ay variable to write the Y-axis acceleration on [m/s^2].
+ * @param az variable to write the Z-axis acceleration on [m/s^2].
+ * @remark ax, ay, and az pointer must be valid (they can't be NULL).
+ * @remark if the MPU-6050 is not initialized (mpu_Init() was not called
+ * successfully), then this function does nothing.
+ * @remark If the operation failed, then *ax, *ay and *az are not modified.
+ */
+void mpu_GetAcceleration(float *ax, float *ay, float *az)
+{   
+    bool ok;
+    uint8_t accelData[6];
+    
+    if(!mpu_initialized)
+        return;
+
+    i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_ACCEL_VALUES,
+                               &accelData[0], 6, &ok);
+                               
+    if(!ok)
+        return;
+
+    *ax = ((float)(int16_t)((accelData[0]<<8) | accelData[1])) * accelFactor;
+    *ay = ((float)(int16_t)((accelData[2]<<8) | accelData[3])) * accelFactor;
+    *az = ((float)(int16_t)((accelData[4]<<8) | accelData[5])) * accelFactor;
+}
+
+/**
+ * @brief Acquires the angular speed from the IMU.
+ * @param gx variable to write the X-axis angular speed on [deg/s].
+ * @param gy variable to write the Y-axis angular speed on [deg/s].
+ * @param gz variable to write the Z-axis angular speed on [deg/s].
+ * @remark gx, gy, and gz pointer must be valid (they can't be NULL).
+ * @remark if the MPU-6050 is not initialized (mpu_Init() was not called
+ * successfully), then this function does nothing.
+ * @remark If the operation failed, then *ax, *ay and *az are not modified.
+ */
+void mpu_GetAngularSpeed(float *gx, float *gy, float *gz)
+{
+    bool ok;
+    uint8_t gyroData[6];
+    
+    if(!mpu_initialized)
+        return;
+    
+    i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_GYRO_VALUES,
+                               &gyroData[0], 6, &ok);
+                               
+    if(!ok)
+        return;
+
+    *gx = ((float)(int16_t)((gyroData[0]<<8) | gyroData[1])) * gyroFactor;
+    *gy = ((float)(int16_t)((gyroData[2]<<8) | gyroData[3])) * gyroFactor;
+    *gz = ((float)(int16_t)((gyroData[4]<<8) | gyroData[5])) * gyroFactor;
+}
+
+/**
+ * @brief Acquires the temperature from the IMU.
+ * @return the measured temperature [°C], or 0 if the operation failed.
+ * @remark if the MPU-6050 is not initialized (mpu_Init() was not called
+ * successfully), then this function does nothing.
+ */
+float mpu_GetTemperature(void)
+{
+    bool ok;
+    uint8_t tempData[2];
+    
+    if(!mpu_initialized)
+        return 0.0f;
+
+    i2c_ReadMultiBytesRegister(SLAVE_ADDRESS, REG_TEMP_VALUES,
+                               &tempData[0], 2, &ok);
+                               
+    if(!ok)
+        return 0.0f;
+
+    return ((float)(int16_t)((tempData[0]<<8) | tempData[1])) / 340.0f + 36.53f; // Coefs from the registers map spec.
+}
diff --git a/Firmware/src/drivers/mpu_6050.h b/Firmware/src/drivers/mpu_6050.h
new file mode 100644
index 0000000..b2f2c91
--- /dev/null
+++ b/Firmware/src/drivers/mpu_6050.h
@@ -0,0 +1,67 @@
+#ifndef __MPU_6050_H
+#define __MPU_6050_H
+
+#include "../main.h"
+
+/** @defgroup MPU-6050 Driver / MPU-6050
+  * @brief Driver for the MPU-6050, a 6-axis IMU from Invensense.
+  *
+  * This driver uses the I2C bus of the digital extension connector.
+  *
+  * Call mpu_Init() first in the initialization code. Then, call mpu_Get() to
+  * get the accelerations and angular speeds.
+  *
+  * @note This driver setup the MPU-6050 sampling rate to 1 kHz. Calling
+  * mpu_Get() more often will return the same values.
+  *
+  * @addtogroup MPU-6050
+  * @{
+  */
+
+/**
+ * @brief Enumeration of the possible accelerometer ranges.
+ */
+typedef enum
+{
+    MPU_ACCEL_RANGE_2G  = 0, ///< +-2 G (+-19.62 m/s^2).
+    MPU_ACCEL_RANGE_4G  = 1, ///< +-4 G (+-39.24 m/s^2).
+    MPU_ACCEL_RANGE_8G  = 2, ///< +-8 G (+-78.48 m/s^2).
+    MPU_ACCEL_RANGE_16G = 3  ///< +-16 G (+-156.96 m/s^2).
+} mpu_AccelRange;
+
+/**
+  * @brief Enumeration of the possible gyrometer ranges.
+  */
+typedef enum
+{
+    MPU_GYRO_RANGE_250DPS  = 0, ///< +- 250 deg/s.
+    MPU_GYRO_RANGE_500DPS  = 1, ///< +- 500 deg/s.
+    MPU_GYRO_RANGE_1000DPS = 2, ///< +- 1000 deg/s.
+    MPU_GYRO_RANGE_2000DPS = 3 ///< +- 2000 deg/s.
+} mpu_GyroRange;
+
+/**
+ * @brief Enumeration of the possible bandwiths.
+ */
+typedef enum
+{
+    MPU_DLPF_BW_256HZ = 0, ///< Cut-off freq: 256 Hz. IMU sampling freq: 8kHz.
+    MPU_DLPF_BW_188HZ = 1, ///< Cut-off freq: 188 Hz. IMU sampling freq: 1kHz.
+    MPU_DLPF_BW_98HZ  = 2, ///< Cut-off freq: 98 Hz. IMU sampling freq: 1kHz.
+    MPU_DLPF_BW_42HZ  = 3, ///< Cut-off freq: 42 Hz. IMU sampling freq: 1kHz.
+    MPU_DLPF_BW_20HZ  = 4, ///< Cut-off freq: 20 Hz. IMU sampling freq: 1kHz.
+    MPU_DLPF_BW_10HZ  = 5, ///< Cut-off freq: 10 Hz. IMU sampling freq: 1kHz.
+    MPU_DLPF_BW_5HZ   = 6  ///< Cut-off freq: 5 Hz. IMU sampling freq: 1kHz.
+} mpu_Bandwidth;
+
+bool mpu_Init(mpu_AccelRange accelRange, mpu_GyroRange gyroRange,
+              mpu_Bandwidth bandwidth);
+void mpu_GetAcceleration(float *ax, float *ay, float *az);
+void mpu_GetAngularSpeed(float *gx, float *gy, float *gz);
+float mpu_GetTemperature(void);
+
+/**
+  * @}
+  */
+
+#endif