From 1a25dcb977cfd8dbcca2e5cffc9e7fbd1f9a08e4 Mon Sep 17 00:00:00 2001
From: pimpest <82343504+pimpest@users.noreply.github.com>
Date: Mon, 9 Feb 2026 15:09:24 +0100
Subject: [PATCH] Now vl and vr return v_x and omega respectively

---
 firmware/base/src/main.c | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

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;