diff --git a/control/c/main.c b/control/c/main.c
new file mode 100644
index 0000000..5361097
--- /dev/null
+++ b/control/c/main.c
@@ -0,0 +1,22 @@
+#include <stdio.h>
+#include "sim.c"
+
+void main(){
+  float rk[] = {0.75, 0.5, 1};
+  float vk = 0.0;
+  float uk = 0.0;
+
+  int x, z;
+  int y = 0;
+
+  for ( z=0 ; z<3 ; z++ ) {
+    for ( x=0 ; x<250 ; x++ ) {
+      if ( y == 0 ) {
+        uk = Controller(rk[z] - vk);
+      }
+      vk = Motor(uk);
+      printf("%f %f %f\n", rk[z], uk, vk);
+      y = y>8 ? 0 : ++y;
+    }
+  }
+}
diff --git a/control/c/sim.c b/control/c/sim.c
new file mode 100644
index 0000000..2d6a32a
--- /dev/null
+++ b/control/c/sim.c
@@ -0,0 +1,46 @@
+/*
+ * File:   motorsim.c
+ * Author: Koldo Basterretxea
+ *
+ * Created on 15 de abril de 2020, 12:51
+ */
+
+// Motor
+
+const float c[] = {0.35003, 0.062554};
+const float d[] = {-0.84659, 0.00183};
+
+float v[] = {0, 0};
+float th[] = {0, 0};
+
+volatile float Motor(float vk) {
+    float thk = (c[0]*v[0]) + (c[1]*v[1]) - (d[0]*th[0]) - (d[1]*th[1]); // abiadura ardatzean rad/sec
+
+    v[1]=v[0];
+    v[0]=vk;
+    th[1]=th[0];
+    th[0]=thk;
+
+    return thk*9.549297; // rad/sec to rpm
+}
+
+// Controller
+
+const float b[] = {0.005455, 0.003455};
+const float a = -1;
+
+volatile float uk = 0;
+float uk1 = 0;
+float ek1 = 0;;
+
+volatile float Controller(float ek) {
+  uk = (b[0]*ek) + (b[1]*ek1) - (a*uk);
+  if (uk > 6) {
+    uk=6;
+  }
+
+  uk1=uk;
+  ek1=ek;
+
+  return uk;
+}
diff --git a/control/src/plant.vhd b/control/src/plant.vhd
index a003455..7222db2 100644
--- a/control/src/plant.vhd
+++ b/control/src/plant.vhd
@@ -14,17 +14,36 @@ end entity;
 ---
 
 architecture arch of plant is
+
   signal s: std_logic_vector(7 downto 0);
+
+  constant c : real_vector(0 to 1) := (0.35003, 0.062554);
+  constant d : real_vector(0 to 1) := (-0.84659, 0.00183);
+
 begin
 
   process(CLK)
+    variable v : real_vector(0 to 1) := (others=>0.0);
+    variable th : real_vector(0 to 1) := (others=>0.0);
+    variable vk, thk : real;
   begin
     if rising_edge(CLK) then
       if RST then
         s <= (others=>'0');
       elsif EN then
-        s <= std_logic_vector(signed(s)+1) when signed(I)>0 else
-             std_logic_vector(signed(s)-1) when signed(I)<0 else s;
+
+        --s <= std_logic_vector(signed(s)+1) when signed(I)>0 else
+        --     std_logic_vector(signed(s)-1) when signed(I)<0 else s;
+
+--        vk := to_real(to_sfixed(I, 1, -6));
+--
+--        thk := (c(0)*v(0)) + (c(1)*v(1)) - (d(0)*th(0)) - (d(1)*th(1)); -- abiadura ardatzean rad/sec
+--
+--        v(1)  := v(0);
+--        v(0)  := vk;
+--        th(1) := th(0);
+--        th(0) := thk;
+--        O <= to_stdv(to_sfixed(thk*9.549297, 5, -2))
       end if;
     end if;
   end process;
diff --git a/control/test/tb_soft_twoproc_bin.vhd b/control/test/tb_soft_twoproc_bin.vhd
index 68a58c3..d1a2d8e 100644
--- a/control/test/tb_soft_twoproc_bin.vhd
+++ b/control/test/tb_soft_twoproc_bin.vhd
@@ -74,11 +74,7 @@ begin
     constant Dd : real := -0.9990;
   begin
     if rising_edge(clk) then
-      if rst = '1' then
-        y <= 0.0;
-      else
-        y <= Nd * u - Dd * y;
-      end if;
+      y <= 0.0 when rst else Nd * u - Dd * y;
     end if;
   end process;