diff --git a/firmware/base/src/main.c b/firmware/base/src/main.c
index 0258175..cdf5b9b 100644
--- a/firmware/base/src/main.c
+++ b/firmware/base/src/main.c
@@ -66,7 +66,7 @@ static calib_diff_t calib_enc = {
     .right_gain = 1.0000
 };
 
-bool update_pos_cb() {
+void update_pos_cb() {
 
     substep_update(&state_l);
     substep_update(&state_r);
@@ -89,12 +89,18 @@ bool update_pos_cb() {
     vel_l /=64 * ENCODER_CPR;
     vel_r /=64 * ENCODER_CPR;
 
-    base_vl = vel_l;
-    base_vr = vel_r;
-
     vel_l *=  WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
     vel_r *= -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
 
+    const double vl_enc = state_l.speed;
+    const double vr_enc = state_r.speed;
+
+    const double vl = (vl_enc/(64 * ENCODER_CPR)) * WHEEL_RADIUS * 2 * M_PI * calib_enc.left_gain;
+    const double vr = (vr_enc/(64 * ENCODER_CPR)) * -WHEEL_RADIUS * 2 * M_PI * calib_enc.right_gain;
+
+    base_vl = (vl + vr)/2;
+    base_vr = (vr - vl)/wheel_separation;
+
     const double linear = (vel_l + vel_r) / 2;
     const double angular = (vel_r - vel_l) / wheel_separation;
     const double r = linear / angular;