diff --git a/encoder.c b/encoder.c
index 584969d..9ba66f6 100644
--- a/encoder.c
+++ b/encoder.c
@@ -5,6 +5,7 @@
 #include <stddef.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <string.h>
 #include "encoder.h"
 #include "systick.h"
 
@@ -77,8 +78,8 @@ int encoder_speed() {
 int encoder_current_speed() {
   int dt = ticks_ms(tick - speed_ticks[SPEED_AVG_NUM - 1].tick);
   if (dt > 200) {
-    printf("dt: %d", dt);
-    return 0;
+    memmove(&speed_ticks[0], &speed_ticks[1], SPEED_AVG_NUM * sizeof speed_ticks[0]);
+    speed_ticks[SPEED_AVG_NUM].tick = tick;
   }
   return 1000
     * (speed_ticks[SPEED_AVG_NUM-1].pos - speed_ticks[0].pos)