diff --git a/src/timer.rs b/src/timer.rs
line changes: +15/-9
index a3a341a..d682d0b
--- a/src/timer.rs
+++ b/src/timer.rs

@@ -68,22 +68,28 @@ impl Timer {
             }
         });
         if let Some(tc) = self.callbacks[0] {
-            let dt = tc.tt - self.t;
-            self.systick.start(dt as Ms);
+            self.systick.start(1 as Ms);
         } else {
             self.systick.stop();
             self.t = 0;
         }
     }
 
-    fn execute_next(&mut self) {
+    fn tick(&mut self) {
+        self.t += 1;
+
         if !self.callbacks[0].is_none() {
-            let tc = self.callbacks[0].take().unwrap();
-            unsafe {
-                (tc.f)();
+            let timer_expired = {
+                let tc = self.callbacks[0].as_ref().unwrap();
+                tc.tt <= self.t
+            };
+            if timer_expired {
+                let tc = self.callbacks[0].take().unwrap();
+                unsafe {
+                    (tc.f)();
+                }
+                self.recalculate();
             }
-            self.t = tc.tt;
-            self.recalculate();
         }
     }
 }

@@ -107,6 +113,6 @@ pub fn schedule(d: TimeValue, f: fn()) {
 fn SysTick() {
     interrupt::free(|_| unsafe {
         let timer = TIMER.as_mut().expect("Timer not yet initialized");
-        timer.execute_next();
+        timer.tick();
     });
 }