diff --git a/fpga/Principal.vhd b/fpga/Principal.vhd
index 59f7fd8..2731d66 100644
--- a/fpga/Principal.vhd
+++ b/fpga/Principal.vhd
@@ -4,24 +4,27 @@ use IEEE.NUMERIC_STD.ALL;
 
 
 entity Principal is
-    Port ( CLK : in  STD_LOGIC; -- Clock
-           BTN : in  STD_LOGIC; -- Reset
-
-           -- FA & Encoder
-           IO : inout  STD_LOGIC_VECTOR (21 downto 16);
-           -- Debug
-           LED : out  STD_LOGIC_VECTOR (3 downto 0);
-           AN : out  STD_LOGIC_VECTOR (3 downto 0);
-           A_TO_G : out  STD_LOGIC_VECTOR (6 downto 0);
-           DOT : out  STD_LOGIC
-       );
+    Generic(
+               fFpga : INTEGER := 50_000_000;
+               fBaud : INTEGER := 9600
+           );
+    Port (
+             CLK : in  std_logic;
+             BTN: in std_logic;
+             RX: in std_logic;
+             TX: out std_logic;
+             LEFTCHA: in std_logic;
+             LEFTCHB: in std_logic;
+             RIGHTCHA: in std_logic;
+             RIGHTCHB: in std_logic;
+             FRONTTRIGGER: out std_logic;
+             FRONTECHO: in std_logic;
+             BACKTRIGGER: out std_logic;
+             BACKECHO: in std_logic
+         );
 end Principal;
 
 architecture Behavioral of Principal is
-    -- Blink led
-    signal count : integer := 0;
-    signal theled: std_logic_vector(3 downto 0) := "0000";
-
     -- General
     signal reset : std_logic := '0';
 
@@ -44,12 +47,26 @@ architecture Behavioral of Principal is
     -- Sensors
     signal front : integer := 0;
     signal back : integer := 0;
+    component hcsr04 IS
+        generic(
+                   fFpga : INTEGER := fFpga
+               );
+        port(
+                clk : IN STD_LOGIC;
+                reset : IN STD_LOGIC;
+                echo : IN STD_LOGIC;
+                distance : OUT INTEGER;
+                trigger : OUT STD_LOGIC;
+                start : IN STD_LOGIC;
+                finished : OUT STD_LOGIC
+            );
+    end component;
 
     -- AF
     component uart is
         generic (
-                    baud                : positive := 9600;
-                    clock_frequency     : positive := 50_000_000
+                    baud                : positive := fBaud;
+                    clock_frequency     : positive := fFpga
                 );
         port (
                  clock               :   in  std_logic;
@@ -64,8 +81,6 @@ architecture Behavioral of Principal is
              );
     end component;
 
-    constant BAUD_COUNT: std_logic_vector := x"1458"; -- 96000 Baud at 50 MHz
-
     signal txData : std_logic_vector(7 downto 0);
     signal txStb : std_logic := '0';
     signal txAck : std_logic := '0';
@@ -91,39 +106,46 @@ architecture Behavioral of Principal is
              );
     end component;
 
-    -- Debug
-    component sevenseg is
-        Port (
-                 data : in  STD_LOGIC_VECTOR (15 downto 0);
-                 clock : in STD_LOGIC;
-                 anode : out  STD_LOGIC_VECTOR (3 downto 0);
-                 segment : out  STD_LOGIC_VECTOR (6 downto 0);
-                 dot : out STD_LOGIC
-             );
-    end component;
-    signal sevensegdata: std_logic_vector(15 downto 0);
-    signal fullseg: std_logic_vector(7 downto 0);
 begin
 
     reset <= BTN;
 
     leftCoder: hedm port map (
-                 clk => CLK,
-                 chA => IO(19),
-                 chB => IO(18),
-                 reset => reset,
-                 zero => zerocoder,
-                 counts => left
-             );
+                                 clk => CLK,
+                                 chA => LEFTCHA,
+                                 chB => LEFTCHB,
+                                 reset => reset,
+                                 zero => zerocoder,
+                                 counts => left
+                             );
 
     rightCoder: hedm port map (
-                 clk => CLK,
-                 chA => IO(17),
-                 chB => IO(16),
-                 reset => reset,
-                 zero => zerocoder,
-                 counts => right
-             );
+                                  clk => CLK,
+                                  chA => RIGHTCHA,
+                                  chB => RIGHTCHB,
+                                  reset => reset,
+                                  zero => zerocoder,
+                                  counts => right
+                              );
+    frontCapt: hcsr04 port map (
+                                   clk => CLK,
+                                   reset => reset,
+                                   echo => FRONTECHO,
+                                   distance => front,
+                                   trigger => FRONTTRIGGER,
+                                   start => '1'
+                               -- finished =>
+                               );
+
+    backCapt: hcsr04 port map (
+                                  clk => CLK,
+                                  reset => reset,
+                                  echo => BACKECHO,
+                                  distance => back,
+                                  trigger => BACKTRIGGER,
+                                  start => '1'
+                              -- finished =>
+                              );
 
     FA: uart port map(
                          clock               => CLK,
@@ -133,8 +155,8 @@ begin
                          data_stream_in_ack  => txAck,
                          data_stream_out     => rxData,
                          data_stream_out_stb => rxStb,
-                         tx                  => IO(21),
-                         rx                  => IO(20)
+                         tx                  => TX,
+                         rx                  => RX
                      );
 
     com: communication port map(
@@ -151,40 +173,5 @@ begin
                                    rxData  => rxData,
                                    rxStb  => rxStb
                                );
-
-
-    -- Debug
-    blinkled : process(CLK, reset)
-    begin
-        if reset = '1' then
-            count <= 0;
-            theled <= "0000";
-            front <= 0;
-            back <= 0;
-        elsif CLK'event and CLK = '1' then
-            if count = 9999999 then
-                count <= 0;
-                theled(3) <= not theled(3);
-                theled(2 downto 0) <= "000";
-            else
-                count <= count + 1;
-                theled(2 downto 0)  <= theled(2 downto 0) or (txStb & rxStb & txAck);
-            end if;
-
-        end if;
-    end process;
-
-    LED <= theled;
-
-    debugSeg: sevenseg port map(
-                                   data => sevensegdata,
-                                   clock => CLK,
-                                   anode => AN,
-                                   segment => A_TO_G,
-                                   dot => DOT
-                               );
-    sevensegdata(15 downto 8) <= rxData;
-    sevensegdata(7 downto 0) <= txData;
-
 end Behavioral;
 
diff --git a/fpga/Principal_tb.gtkw b/fpga/Principal_tb.gtkw
index 68fc097..6ff2c01 100644
--- a/fpga/Principal_tb.gtkw
+++ b/fpga/Principal_tb.gtkw
@@ -1,19 +1,18 @@
 [*]
 [*] GTKWave Analyzer v3.3.86 (w)1999-2017 BSI
-[*] Tue Feb 27 09:39:06 2018
+[*] Tue Feb 27 18:30:48 2018
 [*]
 [dumpfile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/build/Principal_tb.ghw"
-[dumpfile_mtime] "Tue Feb 27 09:38:56 2018"
-[dumpfile_size] 32911228
+[dumpfile_mtime] "Tue Feb 27 18:29:45 2018"
+[dumpfile_size] 4891772
 [savefile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/Principal_tb.gtkw"
 [timestart] 0
 [size] 1600 862
 [pos] -1 -1
-*-41.636795 7980000000000 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+*-43.418156 1760000000000 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
 [treeopen] top.
 [treeopen] top.principal_tb.
 [treeopen] top.principal_tb.dut.
-[treeopen] top.principal_tb.dut.fa.
 [sst_width] 213
 [signals_width] 198
 [sst_expanded] 1
@@ -73,8 +72,34 @@ top.principal_tb.dut.left
 top.principal_tb.dut.rightcoder.cha
 [color] 6
 top.principal_tb.dut.rightcoder.chb
-@8421
+@8420
 [color] 6
 top.principal_tb.dut.right
+@28
+[color] 3
+top.principal_tb.dut.frontcapt.start
+[color] 3
+top.principal_tb.dut.frontcapt.trigger
+[color] 3
+top.principal_tb.dut.frontcapt.echo
+@8420
+[color] 3
+top.principal_tb.dut.frontcapt.distancecounter
+@420
+[color] 3
+top.principal_tb.dut.front
+@28
+[color] 3
+top.principal_tb.dut.backcapt.start
+[color] 3
+top.principal_tb.dut.backcapt.trigger
+[color] 3
+top.principal_tb.dut.backcapt.echo
+@8421
+[color] 3
+top.principal_tb.dut.backcapt.distancecounter
+@420
+[color] 3
+top.principal_tb.dut.back
 [pattern_trace] 1
 [pattern_trace] 0
diff --git a/fpga/Principal_tb.vhd b/fpga/Principal_tb.vhd
index d35f18e..bbe9cad 100644
--- a/fpga/Principal_tb.vhd
+++ b/fpga/Principal_tb.vhd
@@ -10,63 +10,84 @@ entity Principal_tb is
 
 architecture tb of Principal_tb is
 
-    component Principal
-        port (CLK    : in std_logic;
-              BTN    : in std_logic;
-              IO     : inout std_logic_vector (21 downto 16);
-              LED    : out std_logic_vector (3 downto 0);
-              AN     : out std_logic_vector (3 downto 0);
-              A_TO_G : out std_logic_vector (6 downto 0);
-              DOT    : out std_logic);
+    constant fFpga : INTEGER := 2_000_000;
+    constant fBaud : INTEGER := 9600;
+
+    component Principal is
+        Generic(
+                   fFpga : INTEGER := fFpga;
+                   fBaud : INTEGER := fBaud
+               );
+        Port (
+                 CLK : in  std_logic;
+                 BTN: in std_logic;
+                 RX: in std_logic;
+                 TX: out std_logic;
+                 LEFTCHA: in std_logic;
+                 LEFTCHB: in std_logic;
+                 RIGHTCHA: in std_logic;
+                 RIGHTCHB: in std_logic;
+                 FRONTTRIGGER: out std_logic;
+                 FRONTECHO: in std_logic;
+                 BACKTRIGGER: out std_logic;
+                 BACKECHO: in std_logic
+             );
     end component;
 
-    signal CLK    : std_logic;
-    signal BTN    : std_logic;
-    signal IO     : std_logic_vector (21 downto 16);
-    signal LED    : std_logic_vector (3 downto 0);
-    signal AN     : std_logic_vector (3 downto 0);
-    signal A_TO_G : std_logic_vector (6 downto 0);
-    signal DOT    : std_logic;
+    signal CLK          : std_logic;
+    signal BTN          : std_logic;
+    signal RX           : std_logic;
+    signal TX           : std_logic;
+    signal LEFTCHA      : std_logic;
+    signal LEFTCHB      : std_logic;
+    signal RIGHTCHA     : std_logic;
+    signal RIGHTCHB     : std_logic;
+    signal FRONTTRIGGER : std_logic;
+    signal FRONTECHO    : std_logic;
+    signal BACKTRIGGER  : std_logic;
+    signal BACKECHO     : std_logic;
 
-    constant TbPeriod : time := 20 ns;
+    constant TbPeriod : time := 500 ns;
     signal TbClock : std_logic := '0';
     signal TbSimEnded : std_logic := '0';
 
-    constant BaudPeriod : time := 104167 ns; -- 9600 baud
-    constant CharacterPeriod : time := 10 * BaudPeriod;
-    signal rx : std_logic;
-    signal tx : std_logic;
+    signal TbDoneWithCapt : std_logic := '0';
 
-    constant CoderPeriod : time := 27611 ns;
+    constant BaudPeriod : time := 1E9 ns / fBaud;
+    constant CharacterPeriod : time := 10 * BaudPeriod;
+
+    constant CoderPeriod : time := 27611 ns; -- 10 km/h
 begin
 
     dut : Principal
-    port map (CLK    => CLK,
-              BTN    => BTN,
-              IO     => IO,
-              LED    => LED,
-              AN     => AN,
-              A_TO_G => A_TO_G,
-              DOT    => DOT);
+    port map (CLK          => CLK,
+              BTN          => BTN,
+              RX           => RX,
+              TX           => TX,
+              LEFTCHA      => LEFTCHA,
+              LEFTCHB      => LEFTCHB,
+              RIGHTCHA     => RIGHTCHA,
+              RIGHTCHB     => RIGHTCHB,
+              FRONTTRIGGER => FRONTTRIGGER,
+              FRONTECHO    => FRONTECHO,
+              BACKTRIGGER  => BACKTRIGGER,
+              BACKECHO     => BACKECHO);
 
     -- Clock generation
     TbClock <= not TbClock after TbPeriod/2 when TbSimEnded /= '1' else '0';
 
     CLK <= TbClock;
 
-    IO(20) <= rx;
-    tx <= IO(21);
-
     leftCoder : process
     begin
         while TbSimEnded = '0' loop
-            IO(19) <= '1';
+            LEFTCHA <= '1';
             wait for CoderPeriod;
-            IO(18) <= '1';
+            LEFTCHB <= '1';
             wait for CoderPeriod;
-            IO(19) <= '0';
+            LEFTCHA <= '0';
             wait for CoderPeriod;
-            IO(18) <= '0';
+            LEFTCHB <= '0';
             wait for CoderPeriod;
         end loop;
         wait;
@@ -75,18 +96,36 @@ begin
     rightCoder : process
     begin
         while TbSimEnded = '0' loop
-            IO(16) <= '0';
+            RIGHTCHA <= '0';
             wait for CoderPeriod;
-            IO(17) <= '0';
+            RIGHTCHB <= '0';
             wait for CoderPeriod;
-            IO(16) <= '1';
+            RIGHTCHA <= '1';
             wait for CoderPeriod;
-            IO(17) <= '1';
+            RIGHTCHB <= '1';
             wait for CoderPeriod;
         end loop;
         wait;
     end process;
 
+    frontCapt: process
+    begin
+        FRONTECHO <= '0';
+
+        wait on FRONTTRIGGER until FRONTTRIGGER = '1';
+        wait on FRONTTRIGGER until FRONTTRIGGER = '0';
+
+        wait for 10 ms;
+        FRONTECHO <= '1';
+        wait for 15 ms;
+        FRONTECHO <= '0';
+        wait for 35 ms;
+
+        TbDoneWithCapt <= '1';
+
+        wait;
+    end process;
+
     stimuli : process
         procedure send
         (char : std_logic_vector(7 downto 0)) is
@@ -101,7 +140,9 @@ begin
             wait for BaudPeriod;
         end procedure;
     begin
-        rx <= '1';
+        BTN <= '0';
+        RX <= '1';
+        BACKECHO <= '0';
 
         -- Reset generation
         BTN <= '1';
@@ -128,22 +169,19 @@ begin
         wait for 2 * BaudPeriod;
 
 
-        -- Send 'C'
-        send(x"43"); -- '?'
-        wait for 5 * CharacterPeriod;
-
-        -- Wait margin
-        wait for 5 * BaudPeriod;
-
-
         -- Send 'D'
-        send(x"44"); -- '?'
+        send(x"44");
+        wait for 5 * CharacterPeriod;
+
+        wait on TbDoneWithCapt until TbDoneWithCapt = '1';
+
+        -- Send 'C'
+        send(x"43");
         wait for 5 * CharacterPeriod;
 
         -- Wait margin
         wait for 5 * BaudPeriod;
 
-
         -- Stop the clock and hence terminate the simulation
         TbSimEnded <= '1';
         wait;
diff --git a/fpga/debug.ucf b/fpga/debug.ucf
deleted file mode 100644
index b707d17..0000000
--- a/fpga/debug.ucf
+++ /dev/null
@@ -1,179 +0,0 @@
-#      __  ____                 _   __                
-#    /  |/  (_)_____________  / | / /___ _   ______ _
-#   / /|_/ / / ___/ ___/ __ \/  |/ / __ \ | / / __ `/
-#  / /  / / / /__/ /  / /_/ / /|  / /_/ / |/ / /_/ / 
-# /_/  /_/_/\___/_/   \____/_/ |_/\____/|___/\__,_/  
-#                                                  
-# Mercury BASEBOARD User Constraints File
-# Revision 1.0.0 (03/25/2015)
-# Copyright (c) 2015 MicroNova, LLC
-# www.micro-nova.com
-
-# system oscillators
-NET "EXT_CLK"   LOC = "P44" | IOSTANDARD = LVTTL ;
-NET "CLK"       LOC = "P43" | IOSTANDARD = LVTTL ;
-NET "CLK"       TNM_NET = "CLK"; 
-TIMESPEC        "TS_CLK" = PERIOD "CLK" 20 ns HIGH 50 %;
-
-# PS/2
-NET "PS2_DATA"  LOC = "P13" | IOSTANDARD = LVTTL ;
-NET "PS2_CLK"   LOC = "P15" | IOSTANDARD = LVTTL ;
-
-# Buttons
-NET "USR_BTN"	LOC = "P41" | IOSTANDARD = LVTTL ;
-NET "BTN<0>"	LOC = "P68" | IOSTANDARD = LVTTL ;
-NET "BTN<1>"	LOC = "P97" | IOSTANDARD = LVTTL ;
-NET "BTN<2>"	LOC = "P7"  | IOSTANDARD = LVTTL ;
-NET "BTN<3>"	LOC = "P82" | IOSTANDARD = LVTTL ;
-
-# VGA
-NET "RED<0>" 	LOC = "P20" | IOSTANDARD = LVTTL ;
-NET "RED<1>" 	LOC = "P32" | IOSTANDARD = LVTTL ;
-NET "RED<2>" 	LOC = "P33" | IOSTANDARD = LVTTL ;
-NET "GRN<0>" 	LOC = "P34" | IOSTANDARD = LVTTL ;
-NET "GRN<1>" 	LOC = "P35" | IOSTANDARD = LVTTL ;
-NET "GRN<2>" 	LOC = "P36" | IOSTANDARD = LVTTL ;
-NET "BLU<0>" 	LOC = "P37" | IOSTANDARD = LVTTL ;
-NET "BLU<1>"	LOC = "P40" | IOSTANDARD = LVTTL ;
-NET "HSYNC"	LOC = "P16" | IOSTANDARD = LVTTL ;
-NET "VSYNC"     LOC = "P19" | IOSTANDARD = LVTTL ;
-
-# SWITCHES
-NET "SW<0>"  	LOC = "P59" | IOSTANDARD = LVTTL ;
-NET "SW<1>"     LOC = "P60" | IOSTANDARD = LVTTL ;
-NET "SW<2>"     LOC = "P61" | IOSTANDARD = LVTTL ;
-NET "SW<3>"     LOC = "P62" | IOSTANDARD = LVTTL ;
-NET "SW<4>"     LOC = "P64" | IOSTANDARD = LVTTL ;
-NET "SW<5>"     LOC = "P57" | IOSTANDARD = LVTTL ;
-NET "SW<6>"     LOC = "P56" | IOSTANDARD = LVTTL ;
-NET "SW<7>"     LOC = "P52" | IOSTANDARD = LVTTL ;
-
-# 7 SEG
-NET "AN<0>"      LOC = "P50" | IOSTANDARD = LVTTL ;
-NET "AN<1>"      LOC = "P49" | IOSTANDARD = LVTTL ;
-NET "AN<2>"      LOC = "P85" | IOSTANDARD = LVTTL ;
-NET "AN<3>"      LOC = "P84" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<0>"  LOC = "P72" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<1>"  LOC = "P71" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<2>"  LOC = "P70" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<3>"  LOC = "P65" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<4>"  LOC = "P77" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<5>"  LOC = "P78" | IOSTANDARD = LVTTL ;
-NET "A_TO_G<6>"  LOC = "P83" | IOSTANDARD = LVTTL ;
-NET "DOT"        LOC = "P73" | IOSTANDARD = LVTTL ;
-
-# PMOD
-NET "PMOD<0>"   LOC = "P5"  | IOSTANDARD = LVTTL ;
-NET "PMOD<1>"   LOC = "P4"  | IOSTANDARD = LVTTL ;
-NET "PMOD<2>"   LOC = "P6"  | IOSTANDARD = LVTTL ;
-NET "PMOD<3>"   LOC = "P98" | IOSTANDARD = LVTTL ;
-NET "PMOD<4>"   LOC = "P94" | IOSTANDARD = LVTTL ;
-NET "PMOD<5>"   LOC = "P93" | IOSTANDARD = LVTTL ;
-NET "PMOD<6>"   LOC = "P90" | IOSTANDARD = LVTTL ;
-NET "PMOD<7>"   LOC = "P89" | IOSTANDARD = LVTTL ;
-
-# AUDIO OUT
-NET "AUDIO_OUT_R" LOC = "P88" | IOSTANDARD = LVTTL ;
-NET "AUDIO_OUT_L" LOC = "P86" | IOSTANDARD = LVTTL ;
-
-# memory & bus-switch
-NET "SWITCH_OEN" LOC = "P3"  | IOSTANDARD = LVTTL ;
-NET "MEMORY_OEN" LOC = "P30" | IOSTANDARD = LVTTL ;
-
-# flash/usb interface
-NET "FPGA_CSN"	 LOC = "P39" | IOSTANDARD = LVTTL ;
-NET "FLASH_CSN"  LOC = "P27" | IOSTANDARD = LVTTL ;
-NET "SPI_MOSI" LOC = "P46" | IOSTANDARD = LVTTL ;
-NET "SPI_MISO" LOC = "P51" | IOSTANDARD = LVTTL ;
-NET "SPI_SCK"  LOC = "P53" | IOSTANDARD = LVTTL ;
-
-# ADC interface
-NET "ADC_MISO"	LOC = "P21" | IOSTANDARD = LVTTL ;
-NET "ADC_MOSI"	LOC = "P10" | IOSTANDARD = LVTTL ;
-NET "ADC_SCK" 	LOC = "P9"  | IOSTANDARD = LVTTL ;
-NET "ADC_CSN"	LOC = "P12" | IOSTANDARD = LVTTL ;
-#      __  ____                 _   __
-#    /  |/  (_)_____________  / | / /___ _   ______ _
-#   / /|_/ / / ___/ ___/ __ \/  |/ / __ \ | / / __ `/
-#  / /  / / / /__/ /  / /_/ / /|  / /_/ / |/ / /_/ /
-# /_/  /_/_/\___/_/   \____/_/ |_/\____/|___/\__,_/
-#
-# Mercury User Constraints File
-# Revision 1.0.142 (10/24/2012)
-# Copyright (c) 2012 MicroNova, LLC
-# www.micro-nova.com
-
-# user LEDs and button
-NET "LED<0>"    LOC = "P13" | IOSTANDARD = LVTTL ;
-NET "LED<1>"    LOC = "P15" | IOSTANDARD = LVTTL ;
-NET "LED<2>"    LOC = "P16" | IOSTANDARD = LVTTL ;
-NET "LED<3>"    LOC = "P19" | IOSTANDARD = LVTTL ;
-NET "BTN"       LOC = "P41" | IOSTANDARD = LVTTL ;
-
-# direct and global-clock I/O
-NET "DIO<0>"    LOC = "P20" | IOSTANDARD = LVTTL ;
-NET "DIO<1>"    LOC = "P32" | IOSTANDARD = LVTTL ;
-NET "DIO<2>"    LOC = "P33" | IOSTANDARD = LVTTL ;
-NET "DIO<3>"    LOC = "P34" | IOSTANDARD = LVTTL ;
-NET "DIO<4>"    LOC = "P35" | IOSTANDARD = LVTTL ;
-NET "DIO<5>"    LOC = "P36" | IOSTANDARD = LVTTL ;
-NET "DIO<6>"    LOC = "P37" | IOSTANDARD = LVTTL ;
-NET "CIO<0>"    LOC = "P40" | IOSTANDARD = LVTTL ;
-NET "CIO<1>"    LOC = "P44" | IOSTANDARD = LVTTL ;
-
-# in-only pins
-NET "INPIN<0>"  LOC = "P68" | IOSTANDARD = LVTTL ;
-NET "INPIN<1>"  LOC = "P97" | IOSTANDARD = LVTTL ;
-NET "INPIN<2>"  LOC = "P7"  | IOSTANDARD = LVTTL ;
-NET "INPIN<3>"  LOC = "P82" | IOSTANDARD = LVTTL ;
-
-# level-shifted I/O
-NET "IO<0>"     LOC = "P59" | IOSTANDARD = LVTTL ;
-NET "IO<1>"     LOC = "P60" | IOSTANDARD = LVTTL ;
-NET "IO<2>"     LOC = "P61" | IOSTANDARD = LVTTL ;
-NET "IO<3>"     LOC = "P62" | IOSTANDARD = LVTTL ;
-NET "IO<4>"     LOC = "P64" | IOSTANDARD = LVTTL ;
-NET "IO<5>"     LOC = "P57" | IOSTANDARD = LVTTL ;
-NET "IO<6>"     LOC = "P56" | IOSTANDARD = LVTTL ;
-NET "IO<7>"     LOC = "P52" | IOSTANDARD = LVTTL ;
-NET "IO<8>"     LOC = "P50" | IOSTANDARD = LVTTL ;
-NET "IO<9>"     LOC = "P49" | IOSTANDARD = LVTTL ;
-NET "IO<10>"    LOC = "P85" | IOSTANDARD = LVTTL ;
-NET "IO<11>"    LOC = "P84" | IOSTANDARD = LVTTL ;
-NET "IO<12>"    LOC = "P83" | IOSTANDARD = LVTTL ;
-NET "IO<13>"    LOC = "P78" | IOSTANDARD = LVTTL ;
-NET "IO<14>"    LOC = "P77" | IOSTANDARD = LVTTL ;
-NET "IO<15>"    LOC = "P65" | IOSTANDARD = LVTTL ;
-NET "IO<16>"    LOC = "P70" | IOSTANDARD = LVTTL ;
-NET "IO<17>"    LOC = "P71" | IOSTANDARD = LVTTL ;
-NET "IO<18>"    LOC = "P72" | IOSTANDARD = LVTTL ;
-NET "IO<19>"    LOC = "P73" | IOSTANDARD = LVTTL ;
-NET "IO<20>"    LOC = "P5"  | IOSTANDARD = LVTTL ;
-NET "IO<21>"    LOC = "P4"  | IOSTANDARD = LVTTL ;
-NET "IO<22>"    LOC = "P6"  | IOSTANDARD = LVTTL ;
-NET "IO<23>"    LOC = "P98" | IOSTANDARD = LVTTL ;
-NET "IO<24>"    LOC = "P94" | IOSTANDARD = LVTTL ;
-NET "IO<25>"    LOC = "P93" | IOSTANDARD = LVTTL ;
-NET "IO<26>"    LOC = "P90" | IOSTANDARD = LVTTL ;
-NET "IO<27>"    LOC = "P89" | IOSTANDARD = LVTTL ;
-NET "IO<28>"    LOC = "P88" | IOSTANDARD = LVTTL ;
-NET "IO<29>"    LOC = "P86" | IOSTANDARD = LVTTL ;
-
-# memory & bus-switch
-NET "switch_oen" LOC = "P3"  | IOSTANDARD = LVTTL ;
-NET "memory_oen" LOC = "P30" | IOSTANDARD = LVTTL ;
-
-# flash/usb interface
-NET "fpga_csn"   LOC = "P39" | IOSTANDARD = LVTTL ;
-NET "flash_csn"  LOC = "P27" | IOSTANDARD = LVTTL ;
-NET "spi_mosi" LOC = "P46" | IOSTANDARD = LVTTL ;
-NET "spi_miso" LOC = "P51" | IOSTANDARD = LVTTL ;
-NET "spi_sck"  LOC = "P53" | IOSTANDARD = LVTTL ;
-
-# ADC interface
-NET "adc_miso"  LOC = "P21" | IOSTANDARD = LVTTL ;
-NET "adc_mosi"  LOC = "P10" | IOSTANDARD = LVTTL ;
-NET "adc_sck"   LOC = "P9"  | IOSTANDARD = LVTTL ;
-NET "adc_csn"   LOC = "P12" | IOSTANDARD = LVTTL ;
-
-# CLOCK timing
diff --git a/fpga/hcSr04.vhd b/fpga/hcSr04.vhd
deleted file mode 100644
index e09724b..0000000
--- a/fpga/hcSr04.vhd
+++ /dev/null
@@ -1,88 +0,0 @@
-LIBRARY ieee;
-USE ieee.std_logic_1164.all;
-
-ENTITY hcSr04 IS
-    GENERIC(
-               fFpga : INTEGER := 50_000_000 -- frequency of the FPGA clock (Hz)
-           );
-PORT(
-        clk : IN STD_LOGIC; -- clock of the FPGA
-        echo : IN STD_LOGIC; -- echo pin of the hcSr04
-        distance : OUT INTEGER RANGE 0 TO 65535; -- Divide by 58 to get the value in cm
-        trigger : OUT STD_LOGIC; -- trigger pin of the hcSr04
-        start : IN STD_LOGIC; -- Set to '1' everytime a measurement is needed (or keep at '1' for continuous measurement)
-        finished : OUT STD_LOGIC -- Driven to '1' everytime a measurement has finished
-        );
-END hcSr04;
-
-ARCHITECTURE Behavioral OF hcSr04 IS
-    -- Generate us clock
-    CONSTANT fUs : INTEGER := 1_000_000; -- Frequency of the microsecond clock
-    CONSTANT usTicks : INTEGER := fFPGA / fUs; -- Number of FPGA tick that makes a microsecond
-    SIGNAL fpgaCounter : INTEGER RANGE 0 TO usTicks - 1; -- Count the microsecond
-    SIGNAL usClk : STD_LOGIC; -- Clock that ticks every us
-
-    -- Trigger
-    CONSTANT triggerDuration : INTEGER := 10; -- Number of us that makes up a trigger sequence
-    SIGNAL triggerCounter : INTEGER RANGE 0 TO triggerDuration - 1 := 0; -- Progress in the trigger sequence
-    SIGNAL theTrigger : STD_LOGIC := '0'; -- Trigger pin but with default value
-
-    -- Measurement
-    CONSTANT measurementDuration : INTEGER := 60_000; -- Number of us that makes up a measurement cycle
-    SIGNAL measurementCounter : INTEGER RANGE 0 TO measurementDuration - 1 := 0; -- Progress in the measurement cycle
-
-    -- Distance
-    SIGNAL distanceCounter : INTEGER RANGE 0 TO 65535 := 0; -- Distance measured (in us)
-
-    -- State machine
-    TYPE stateType IS (waiting, triggering, measuring);
-    SIGNAL state : stateType;
-
-BEGIN
-    us : process(clk)
-    begin
-        if rising_edge(clk) then
-            if fpgaCounter = 0 then
-                usClk <= '0';
-            elsif fpgaCounter = 1 then
-                usClk <= '1';
-            end if;
-            fpgaCounter <= fpgaCounter + 1;
-        end if;
-    end process;
-
-    trigger <= theTrigger;
-
-    measure : process(usClk)
-    begin
-        if rising_edge(usClk) then
-            CASE state IS
-                WHEN waiting =>
-                    finished <= '0';
-                    IF start = '1' THEN
-                        theTrigger <= '1';
-                        triggerCounter <= 0;
-                        state <= triggering;
-                    END IF;
-                WHEN triggering =>
-                    triggerCounter <= triggerCounter + 1;
-                    IF triggerCounter = triggerDuration - 1 THEN
-                        theTrigger <= '0';
-                        measurementCounter <= 0;
-                        distanceCounter <= 0;
-                        state <= measuring;
-                    END IF;
-                WHEN measuring =>
-                    IF echo = '1' and distanceCounter < 65535 THEN
-                        distanceCounter <= distanceCounter + 1;
-                    END IF;
-                    measurementCounter <= measurementCounter + 1;
-                    IF measurementCounter = measurementDuration - 1 THEN
-                        distance <= distanceCounter;
-                        finished <= '1';
-                        state <= waiting;
-                    END IF;
-            END CASE;
-        end if;
-    end process;
-END Behavioral;
diff --git a/fpga/hcsr04.vhd b/fpga/hcsr04.vhd
new file mode 100644
index 0000000..26738cf
--- /dev/null
+++ b/fpga/hcsr04.vhd
@@ -0,0 +1,106 @@
+LIBRARY ieee;
+USE ieee.std_logic_1164.all;
+
+ENTITY hcsr04 IS
+    GENERIC(
+               fFpga : INTEGER := 50_000_000 -- frequency of the FPGA clock (Hz)
+           );
+    PORT(
+            clk : IN STD_LOGIC; -- clock of the FPGA
+            reset : IN STD_LOGIC; -- hard reset
+            echo : IN STD_LOGIC; -- echo pin of the hcsr04
+            distance : OUT INTEGER; -- Divide by 58 to get the value in cm
+            trigger : OUT STD_LOGIC; -- trigger pin of the hcsr04
+            start : IN STD_LOGIC; -- Set to '1' everytime a measurement is needed (or keep at '1' for continuous measurement)
+            finished : OUT STD_LOGIC -- Driven to '1' everytime a measurement has finished
+        );
+END hcsr04;
+
+ARCHITECTURE Behavioral OF hcsr04 IS
+    -- Generate us clock
+    CONSTANT fUs : INTEGER := 1_000_000; -- Frequency of the microsecond clock
+    CONSTANT usTicks : INTEGER := fFPGA / fUs / 2; -- Number of FPGA tick that makes a demi-microsecond
+    SIGNAL fpgaCounter : INTEGER; -- Count the microsecond
+    SIGNAL usClk : STD_LOGIC := '0'; -- Clock that ticks every us
+
+    -- Trigger
+    CONSTANT triggerDuration : INTEGER := 10; -- Number of us that makes up a trigger sequence
+    SIGNAL triggerCounter : INTEGER; -- Progress in the trigger sequence
+    SIGNAL theTrigger : STD_LOGIC := '0'; -- Trigger pin but with default value
+
+    -- Measurement
+    CONSTANT measurementDuration : INTEGER := 60_000; -- Number of us that makes up a measurement cycle
+    SIGNAL measurementCounter : INTEGER := 0; -- Progress in the measurement cycle
+
+    -- Distance
+    CONSTANT maxDistance : INTEGER := 29000; -- 5m
+    SIGNAL distanceCounter : INTEGER := 0; -- Distance measured (in us)
+
+    -- State machine
+    TYPE stateType IS (waiting, triggering, measuring);
+    SIGNAL state : stateType;
+    SIGNAL startUs : STD_LOGIC := '0'; -- A propagation of the start signal
+
+BEGIN
+    us : process(clk, reset)
+    begin
+        if reset = '1' then
+            usClk <= '0';
+            fpgaCounter <= 0;
+            startUs <= '0';
+        elsif rising_edge(clk) then
+
+            if start = '1' then
+                startUs <= '1';
+            elsif state /= waiting then
+                startUs <= '0';
+            end if;
+
+            if fpgaCounter >= usTicks - 1 then
+                fpgaCounter <= 0;
+                usClk <= not usClk;
+            else
+                fpgaCounter <= fpgaCounter + 1;
+            end if;
+        end if;
+    end process;
+
+    trigger <= theTrigger;
+
+    measure : process(usClk, reset)
+    begin
+        if reset = '1' then
+            state <= waiting;
+            finished <= '0';
+            distance <= 0;
+        elsif rising_edge(usClk) then
+            CASE state IS
+                WHEN waiting =>
+                    finished <= '0';
+                    IF startUs = '1' THEN
+                        theTrigger <= '1';
+                        triggerCounter <= 0;
+                        state <= triggering;
+                    END IF;
+                WHEN triggering =>
+                    triggerCounter <= triggerCounter + 1;
+                    IF triggerCounter >= triggerDuration - 1 THEN
+                        theTrigger <= '0';
+                        measurementCounter <= 0;
+                        distanceCounter <= 0;
+                        state <= measuring;
+                    END IF;
+                WHEN measuring =>
+                    IF echo = '1' and distanceCounter < maxDistance THEN
+                        distanceCounter <= distanceCounter + 1;
+                    END IF;
+                    measurementCounter <= measurementCounter + 1;
+                    IF measurementCounter >= measurementDuration - 1 THEN
+                        distance <= distanceCounter;
+                        finished <= '1';
+                        state <= waiting;
+                    END IF;
+            END CASE;
+        end if;
+    end process;
+END Behavioral;
diff --git a/fpga/hcsr04_tb.gtkw b/fpga/hcsr04_tb.gtkw
new file mode 100644
index 0000000..80804ea
--- /dev/null
+++ b/fpga/hcsr04_tb.gtkw
@@ -0,0 +1,48 @@
+[*]
+[*] GTKWave Analyzer v3.3.86 (w)1999-2017 BSI
+[*] Tue Feb 27 14:49:24 2018
+[*]
+[dumpfile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/build/hcsr04_tb.ghw"
+[dumpfile_mtime] "Tue Feb 27 14:48:30 2018"
+[dumpfile_size] 5884923
+[savefile] "/home/geoffrey/Documents/Polytech/Robotech/2017-2018/CdF/cdf2018-principal/fpga/hcsr04_tb.gtkw"
+[timestart] 0
+[size] 1600 862
+[pos] -1 -1
+*-44.256817 34900000000000 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1 -1
+[treeopen] top.
+[treeopen] top.hcsr04_tb.
+[sst_width] 213
+[signals_width] 174
+[sst_expanded] 1
+[sst_vpaned_height] 243
+@28
+top.hcsr04_tb.dut.clk
+top.hcsr04_tb.dut.reset
+@8420
+top.hcsr04_tb.dut.fpgacounter
+@28
+top.hcsr04_tb.dut.usclk
+[color] 5
+top.hcsr04_tb.dut.start
+[color] 2
+top.hcsr04_tb.dut.startus
+[color] 1
+top.hcsr04_tb.dut.trigger
+@8420
+[color] 2
+top.hcsr04_tb.dut.measurementcounter
+@28
+[color] 1
+top.hcsr04_tb.dut.echo
+@8420
+[color] 2
+top.hcsr04_tb.dut.distancecounter
+@28
+[color] 5
+top.hcsr04_tb.dut.finished
+@420
+[color] 5
+top.hcsr04_tb.distance
+[pattern_trace] 1
+[pattern_trace] 0
diff --git a/fpga/hcsr04_tb.vhd b/fpga/hcsr04_tb.vhd
new file mode 100644
index 0000000..414a671
--- /dev/null
+++ b/fpga/hcsr04_tb.vhd
@@ -0,0 +1,115 @@
+-- Testbench automatically generated online
+-- at http://vhdl.lapinoo.net
+-- Generation date : 27.2.2018 12:52:54 GMT
+
+library ieee;
+use ieee.std_logic_1164.all;
+
+entity hcsr04_tb is
+    end hcsr04_tb;
+
+architecture tb of hcsr04_tb is
+
+    component hcSr04
+        generic (
+                    fFpga : INTEGER := 2_000_000 -- Reduce speed to increase simulation speed
+                );
+        port (
+                 clk      : in std_logic;
+                 reset    : in std_logic;
+                 echo     : in std_logic;
+                 distance : out integer;
+                 trigger  : out std_logic;
+                 start    : in std_logic;
+                 finished : out std_logic
+             );
+    end component;
+
+    signal clk      : std_logic;
+    signal reset    : std_logic;
+    signal echo     : std_logic;
+    signal distance : integer;
+    signal trigger  : std_logic;
+    signal start    : std_logic;
+    signal finished : std_logic;
+
+    constant TbPeriod : time := 500 ns;
+    signal TbClock : std_logic := '0';
+    signal TbSimEnded : std_logic := '0';
+
+begin
+
+    dut : hcSr04 port map (clk      => clk,
+                           reset    => reset,
+                           echo     => echo,
+                           distance => distance,
+                           trigger  => trigger,
+                           start    => start,
+                           finished => finished);
+
+    -- Clock generation
+    TbClock <= not TbClock after TbPeriod/2 when TbSimEnded /= '1' else '0';
+
+    clk <= TbClock;
+
+    stimuli : process
+        variable startT : time;
+        variable stopT : time;
+        variable delta : time;
+    begin
+        echo <= '0';
+        start <= '0';
+
+        -- Reset generation
+        reset <= '1';
+        wait for 100 ns;
+        reset <= '0';
+        wait for 100 ns;
+
+
+        report "TEST First measurement" severity note;
+        startT := now;
+        start <= '1';
+        wait for TbPeriod;
+        start <= '0';
+
+        wait for 19 ms;
+        echo <= '1';
+        wait for 15 ms;
+        echo <= '0';
+
+        wait on finished until finished = '1';
+        stopT := now;
+
+        delta := stopT - startT;
+        report "Measurement took " & time'image(now);
+
+        assert distance = 15000 report "Wrong distance reported" severity error;
+
+        wait for 5 ms; -- Margin
+
+
+        report "TEST Second measurement" severity note;
+        startT := now;
+        start <= '1';
+        wait for TbPeriod;
+        start <= '0';
+
+
+        wait on finished until finished = '1';
+        stopT := now;
+
+        delta := stopT - startT;
+        report "Measurement took " & time'image(now);
+
+        assert distance = 0 report "Wrong distance reported" severity error;
+
+        wait for 5 ms; -- Margin
+
+
+        -- Stop the clock and hence terminate the simulation
+        TbSimEnded <= '1';
+        wait;
+    end process;
+
+end tb;
diff --git a/fpga/principal.ucf b/fpga/principal.ucf
new file mode 100644
index 0000000..38b8989
--- /dev/null
+++ b/fpga/principal.ucf
@@ -0,0 +1,38 @@
+# CLK
+NET "CLK" LOC = "P43" | IOSTANDARD = LVTTL ;
+NET "CLK" TNM_NET = "CLK";
+TIMESPEC "TS_CLK" = PERIOD "CLK" 20 ns HIGH 50 %;
+
+
+# BTN
+NET "BTN" LOC = "P41" | IOSTANDARD = LVTTL ;
+
+# IO<0>
+NET "RX" LOC = "P59" | IOSTANDARD = LVTTL ;
+
+# IO<1>
+NET "TX" LOC = "P60" | IOSTANDARD = LVTTL ;
+
+# IO<2>
+NET "LEFTCHA" LOC = "P61" | IOSTANDARD = LVTTL ;
+
+# IO<3>
+NET "LEFTCHB" LOC = "P62" | IOSTANDARD = LVTTL ;
+
+# IO<4>
+NET "RIGHTCHA" LOC = "P64" | IOSTANDARD = LVTTL ;
+
+# IO<5>
+NET "RIGHTCHB" LOC = "P57" | IOSTANDARD = LVTTL ;
+
+# IO<6>
+NET "FRONTTRIGGER" LOC = "P56" | IOSTANDARD = LVTTL ;
+
+# IO<7>
+NET "FRONTECHO" LOC = "P52" | IOSTANDARD = LVTTL ;
+
+# IO<8>
+NET "BACKTRIGGER" LOC = "P50" | IOSTANDARD = LVTTL ;
+
+# IO<9>
+NET "BACKECHO" LOC = "P49" | IOSTANDARD = LVTTL ;
diff --git a/fpga/project.cfg b/fpga/project.cfg
index b10ce91..6622ec0 100644
--- a/fpga/project.cfg
+++ b/fpga/project.cfg
@@ -5,15 +5,5 @@ PROGRAMMER = mercpcl
 
 TOPLEVEL = Principal
 # Prod
-# VHDSOURCE = $(TOPLEVEL).vhd uart.vhd
-# CONSTRAINTS = mercury.ucf
-# Debug
-VHDSOURCE = $(TOPLEVEL).vhd $(filter-out %_tb.vhd,$(wildcard *.vhd))
-CONSTRAINTS = debug.ucf
-
-
-# Implement design
-# Allow unmatched LOC Constraints
-NGDBUILD_OPTS += -aul
-# Allow unmatched Timing Group Constraints
-NGDBUILD_OPTS += -aut
+VHDSOURCE = $(TOPLEVEL).vhd communication.vhd uart.vhd hedm.vhd hcsr04.vhd
+CONSTRAINTS = principal.ucf