diff --git a/Core/Inc/tim.h b/Core/Inc/tim.h
index 5b0c7db..629e237 100644
--- a/Core/Inc/tim.h
+++ b/Core/Inc/tim.h
@@ -38,6 +38,8 @@ extern TIM_HandleTypeDef htim2;
 
 extern TIM_HandleTypeDef htim3;
 
+extern TIM_HandleTypeDef htim6;
+
 extern TIM_HandleTypeDef htim8;
 
 /* USER CODE BEGIN Private defines */
@@ -47,6 +49,7 @@ extern TIM_HandleTypeDef htim8;
 void MX_TIM1_Init(void);
 void MX_TIM2_Init(void);
 void MX_TIM3_Init(void);
+void MX_TIM6_Init(void);
 void MX_TIM8_Init(void);
 
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 68ace55..40da0e3 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -92,6 +92,7 @@ int main(void)
   MX_TIM3_Init();
   MX_USB_Device_Init();
   MX_TIM8_Init();
+  MX_TIM6_Init();
   /* USER CODE BEGIN 2 */
 
   HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1); // IN1A
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index b65f5c3..aea7b0b 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -74,8 +74,7 @@ void DiffBot::handleStallCondition()
     motor.stop(true);
 
     char log[64];
-    int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d;POS;%.2f;%.2f\n",
-                       index, pos.x, pos.y);
+    int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index);
     CDC_Transmit_FS((uint8_t*)log, len);
 
     targets[index].active = false;
@@ -116,17 +115,17 @@ void DiffBot::update(float dt_actual)
     float vRightAct = readEncoderRight();
 
     float dDistance = ((vLeftAct + vRightAct) * 0.5f) * dt;
-    float dTheta    = ((vRightAct - vLeftAct) / WHEEL_BASE) * dt;
+    float dTheta = ((vRightAct - vLeftAct) / WHEEL_BASE) * dt;
 
     float midTheta = pos.theta + (dTheta * 0.5f);
     pos.x += dDistance * cosf(midTheta);
     pos.y += dDistance * sinf(midTheta);
     pos.theta = normalizeAngle(pos.theta + dTheta);
 
-    bool commandingMove = (std::abs(motor.leftTarget_PWM) > 50 || std::abs(motor.rightTarget_PWM) > 50);
-    bool isStationary   = (std::abs(vLeftAct) < 0.001f && std::abs(vRightAct) < 0.001f);
+    // bool commandingMove = (std::abs(motor.leftTarget_PWM) > 50 || std::abs(motor.rightTarget_PWM) > 50);
+    // bool isStationary = (std::abs(vLeftAct) == 0.0f && std::abs(vRightAct) == 0.0f);
 
-    if (commandingMove && isStationary) {
+    /*if (commandingMove && isStationary) {
         if (!no_move)
         {
             no_move = true; publishNotMoved = HAL_GetTick();
@@ -137,7 +136,7 @@ void DiffBot::update(float dt_actual)
         }
     } else {
         no_move = false;
-    }
+    }*/
 
     if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) {
         publishStatus();
diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp
index 538e110..bf764ca 100644
--- a/Core/Src/motors.cpp
+++ b/Core/Src/motors.cpp
@@ -14,7 +14,7 @@ float approach(float current, float target, float step) {
 
 void Motor::update() {
 
-    int16_t max_step = 20;
+    int16_t max_step = 10;
 
     leftTarget_PWM  = std::max(-PWM_MAX, std::min((float)leftTarget_PWM,  PWM_MAX));
     rightTarget_PWM = std::max(-PWM_MAX, std::min((float)rightTarget_PWM, PWM_MAX));
diff --git a/Core/Src/setup.cpp b/Core/Src/setup.cpp
index e03ef7f..719dc47 100644
--- a/Core/Src/setup.cpp
+++ b/Core/Src/setup.cpp
@@ -8,23 +8,39 @@
 #include <setup.h>
 #include <modelec.h>
 #include "commSTM.h"
+#include "tim.h"
 
 DiffBot bot = DiffBot(Point());
 
-void ModelecOdometrySetup() {
-	bot.setup();
-}
+uint32_t lastUs = 0;
 
-uint32_t lastTick = 0;
+void ModelecOdometrySetup() {
+    HAL_TIM_Base_Start(&htim6);
+
+    lastUs = __HAL_TIM_GET_COUNTER(&htim6);
+
+    bot.setup();
+}
 
 void ModelecOdometryLoop() {
-	uint32_t currentTick = HAL_GetTick();
-	float actualDt = (currentTick - lastTick) / 1000.0f;
+    uint32_t currentUs = __HAL_TIM_GET_COUNTER(&htim6);
+    uint32_t diffUs;
 
-	if (actualDt <= 0.0f) return;
+    if (currentUs >= lastUs) {
+        diffUs = currentUs - lastUs;
+    } else {
+        diffUs = (65535 - lastUs) + currentUs + 1;
+    }
 
-	USB_Comm_Process();
-	bot.update(actualDt);
+    float actualDt = diffUs / 1000000.0f;
 
-	lastTick = currentTick;
+    if (actualDt <= 0.0001f || actualDt > 0.1f) {
+        if (actualDt > 0.1f) lastUs = currentUs;
+        return;
+    }
+
+    USB_Comm_Process();
+    bot.update(actualDt);
+
+    lastUs = currentUs;
 }
diff --git a/Core/Src/tim.c b/Core/Src/tim.c
index 41ed689..495273a 100644
--- a/Core/Src/tim.c
+++ b/Core/Src/tim.c
@@ -27,6 +27,7 @@
 TIM_HandleTypeDef htim1;
 TIM_HandleTypeDef htim2;
 TIM_HandleTypeDef htim3;
+TIM_HandleTypeDef htim6;
 TIM_HandleTypeDef htim8;
 
 /* TIM1 init function */
@@ -197,6 +198,39 @@ void MX_TIM3_Init(void)
 
   /* USER CODE END TIM3_Init 2 */
 
+}
+/* TIM6 init function */
+void MX_TIM6_Init(void)
+{
+
+  /* USER CODE BEGIN TIM6_Init 0 */
+
+  /* USER CODE END TIM6_Init 0 */
+
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM6_Init 1 */
+
+  /* USER CODE END TIM6_Init 1 */
+  htim6.Instance = TIM6;
+  htim6.Init.Prescaler = 15;
+  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim6.Init.Period = 65535;
+  htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
+  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM6_Init 2 */
+
+  /* USER CODE END TIM6_Init 2 */
+
 }
 /* TIM8 init function */
 void MX_TIM8_Init(void)
@@ -294,6 +328,17 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM1_MspInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM6)
+  {
+  /* USER CODE BEGIN TIM6_MspInit 0 */
+
+  /* USER CODE END TIM6_MspInit 0 */
+    /* TIM6 clock enable */
+    __HAL_RCC_TIM6_CLK_ENABLE();
+  /* USER CODE BEGIN TIM6_MspInit 1 */
+
+  /* USER CODE END TIM6_MspInit 1 */
+  }
   else if(tim_baseHandle->Instance==TIM8)
   {
   /* USER CODE BEGIN TIM8_MspInit 0 */
@@ -439,6 +484,17 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM1_MspDeInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM6)
+  {
+  /* USER CODE BEGIN TIM6_MspDeInit 0 */
+
+  /* USER CODE END TIM6_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM6_CLK_DISABLE();
+  /* USER CODE BEGIN TIM6_MspDeInit 1 */
+
+  /* USER CODE END TIM6_MspDeInit 1 */
+  }
   else if(tim_baseHandle->Instance==TIM8)
   {
   /* USER CODE BEGIN TIM8_MspDeInit 0 */
diff --git a/pcb_odo.ioc b/pcb_odo.ioc
index c1371f2..d38d6a4 100644
--- a/pcb_odo.ioc
+++ b/pcb_odo.ioc
@@ -13,10 +13,11 @@ Mcu.IP2=SYS
 Mcu.IP3=TIM1
 Mcu.IP4=TIM2
 Mcu.IP5=TIM3
-Mcu.IP6=TIM8
-Mcu.IP7=USB
-Mcu.IP8=USB_DEVICE
-Mcu.IPNb=9
+Mcu.IP6=TIM6
+Mcu.IP7=TIM8
+Mcu.IP8=USB
+Mcu.IP9=USB_DEVICE
+Mcu.IPNb=10
 Mcu.Name=STM32G491K(C-E)Ux
 Mcu.Package=UFQFPN32
 Mcu.Pin0=PF0-OSC_IN
@@ -25,8 +26,9 @@ Mcu.Pin10=PB8-BOOT0
 Mcu.Pin11=VP_SYS_VS_Systick
 Mcu.Pin12=VP_SYS_VS_DBSignals
 Mcu.Pin13=VP_TIM1_VS_ClockSourceINT
-Mcu.Pin14=VP_TIM8_VS_ClockSourceINT
-Mcu.Pin15=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
+Mcu.Pin14=VP_TIM6_VS_ClockSourceINT
+Mcu.Pin15=VP_TIM8_VS_ClockSourceINT
+Mcu.Pin16=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS
 Mcu.Pin2=PA1
 Mcu.Pin3=PA4
 Mcu.Pin4=PA8
@@ -35,7 +37,7 @@ Mcu.Pin6=PA11
 Mcu.Pin7=PA12
 Mcu.Pin8=PB4
 Mcu.Pin9=PB6
-Mcu.PinsNb=16
+Mcu.PinsNb=17
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32G491KEUx
@@ -186,6 +188,9 @@ TIM2.IPParameters=PeriodNoDither,EncoderMode
 TIM2.PeriodNoDither=65535
 TIM3.EncoderMode=TIM_ENCODERMODE_TI12
 TIM3.IPParameters=EncoderMode
+TIM6.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
+TIM6.IPParameters=Prescaler,AutoReloadPreload
+TIM6.Prescaler=15
 TIM8.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_ENABLE
 TIM8.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM8.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
@@ -201,6 +206,8 @@ VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
 VP_TIM1_VS_ClockSourceINT.Mode=Internal
 VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
+VP_TIM6_VS_ClockSourceINT.Mode=Enable_Timer
+VP_TIM6_VS_ClockSourceINT.Signal=TIM6_VS_ClockSourceINT
 VP_TIM8_VS_ClockSourceINT.Mode=Internal
 VP_TIM8_VS_ClockSourceINT.Signal=TIM8_VS_ClockSourceINT
 VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Mode=CDC_FS