diff --git a/spinnyboy_rust/src/bin/dc_driver/dshot.rs b/spinnyboy_rust/src/bin/dc_driver/dshot.rs
index 5f27fbf..82f842e 100644
--- a/spinnyboy_rust/src/bin/dc_driver/dshot.rs
+++ b/spinnyboy_rust/src/bin/dc_driver/dshot.rs
@@ -322,9 +322,9 @@ impl<'a> DShot<'a> {
                                              // eRPM = (60*1000 000)/T_us next RPM can be achived -> RPM = eRPM/(poles/2):
 
                                             let rpm = (((erpm))/((XING_EPRO_22_COILS)));                                            
-                                            rprintln!("RPM: {}", rpm);
-                                            rprintln!("ERPM: {}", erpm);
-                                            rprintln!("period_ms: {}", period_ms);
+                                            // rprintln!("RPM: {}", rpm);
+                                            // rprintln!("ERPM: {}", erpm);
+                                            // rprintln!("period_ms: {}", period_ms);
                                             self.rpm.set(Some(rpm))
                                         }
                                     }
diff --git a/spinnyboy_rust/src/bin/main.rs b/spinnyboy_rust/src/bin/main.rs
index 6b5bb36..821b874 100644
--- a/spinnyboy_rust/src/bin/main.rs
+++ b/spinnyboy_rust/src/bin/main.rs
@@ -17,12 +17,13 @@ use esp_hal::gpio::{Input, InputConfig};
 use esp_hal::handler;
 use esp_hal::rmt::{Rmt, TxChannelConfig, TxChannelCreator};
 use esp_hal::time::{ Instant, Rate};
+use core::fmt::Debug;
 //pid-controller
 use core::time::Duration;
-use discrete_pid::pid::FuncPidController;
 use discrete_pid::pid::PidContext;
 use discrete_pid::time::Millis;
 use discrete_pid::{pid, time};
+use discrete_pid::time::InstantLike;
 
 use core::cell::RefCell;
 use core::mem::forget;
@@ -60,6 +61,22 @@ const DEFAULT_SPIN_TIME: u32 = 10;
 esp_bootloader_esp_idf::esp_app_desc!();
 use crate::peripherals::Command;
 
+#[derive(Copy, Clone)]
+struct Time {
+    sec: i32,
+    nsec: i32,
+}
+
+impl InstantLike for Time {
+    fn duration_since(&self, other: Self) -> Duration {
+        let sec = self.sec - other.sec;
+        let nsec = self.nsec - other.nsec;
+        Duration::new(sec as u64, nsec as u32)
+    }
+}
+
+
+
 #[main]
 fn main() -> ! {
     // generator version: 1.0.1
@@ -104,29 +121,26 @@ fn main() -> ! {
 
     //dshot_esc.arm(&delay).unwrap();
     //PID-Controller
-    let loop_time = core::time::Duration::from_millis(500);
-
+    let loop_time = Duration::from_micros(125);
     let cfg = pid::PidConfigBuilder::default()
-        .kp(1.0)
-        .ki(1.5)
-        .sample_time(loop_time)
-        // .filter_tc(0.1)
-        .output_limits(50.00, 1000.00)
-        .build()
-        .expect("Failed to build a PID configuration");
-    let pid = FuncPidController::new(cfg);
-
+    .kp(2.0)
+    .ki(1.5)
+    .sample_time(loop_time)
+    .filter_tc(0.1)
+    .build()
+    .expect("Failed to build a PID configuration");
+    let controller = pid::FuncPidController::new(cfg);  
     let mut ctx = PidContext::new(Millis(0), 0.0, 0.0);
     let set_point = DEFAULT_TARGET_RPM;
     dshot_esc.arm();
     let mut rpm = 0;
     let mut control: f32 = 0.0;
+    // let timestamp = Instant::now();
     loop {
-        let mut begin = Instant::now();
         dshot_esc.process();
         //let control_val = dc_driver::
         //this bad boy needs floats, this will be fun :)
-        dshot_esc.set_throttle(200);
+        dshot_esc.set_throttle(400);
         rpm = match dshot_esc.get_rpm() {
             Some(x) => {
                 // rprintln!("GOT RPM {}", x);
@@ -138,11 +152,12 @@ fn main() -> ! {
             }
         };
         // rprintln!("RPM:{}",rpm);
-        let last_time = ctx.last_time().unwrap_or(Millis(0));
-        // let timestamp = last_time + Duration::from_millis(begin.elapsed().as_millis() as _); 
-        // (control, ctx) = pid.compute(ctx, rpm as _, set_point as _, timestamp, None);
-        
-        // rprintln!("control:{},rpm:{}", control,rpm);
+        let last_time = esp_hal::time::Instant::now();
+     
+        let timestamp = last_time + Duration::from_millis(Instant::now().duration_since_epoch().as_millis() as _); 
+        (control, ctx) = controller.compute(ctx, rpm as _, set_point as _, timestamp, None);
+        let dumped_context = ctx.last_time().expect("LAST TIME!"); 
+        rprintln!("control:{},rpm:{},timestamp:{:?}", control,rpm,dumped_context);
     }
 
     // rprintln!("RMT SENT!");