From e2938857fb8cc9288d23a82203fa124b557ae85d Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Sat, 7 Mar 2026 14:08:15 +0100
Subject: [PATCH] dt

---
 Core/Inc/modelec.h   |  2 +-
 Core/Inc/setup.h     |  2 +-
 Core/Src/main.c      |  2 +-
 Core/Src/modelec.cpp | 16 ++++++++++++----
 Core/Src/setup.cpp   | 15 ++++++++++++---
 5 files changed, 27 insertions(+), 10 deletions(-)

diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index a05ae6c..df0cade 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -75,7 +75,7 @@ public:
 
 	float readEncoderLeft();
 
-	DiffBot(Point pos, float dt);
+	DiffBot(Point pos);
 
 	void stop(bool stop);
 
diff --git a/Core/Inc/setup.h b/Core/Inc/setup.h
index 47236a9..56683e5 100644
--- a/Core/Inc/setup.h
+++ b/Core/Inc/setup.h
@@ -13,7 +13,7 @@ extern "C" {
 #endif
 
 void ModelecOdometrySetup();
-void ModelecOdometryLoop(float dt);
+void ModelecOdometryLoop();
 
 #ifdef __cplusplus
 }
diff --git a/Core/Src/main.c b/Core/Src/main.c
index dee1308..68ace55 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -111,7 +111,7 @@ int main(void)
   while (1)
   {
 
-	  ModelecOdometryLoop(0.01f);
+	  ModelecOdometryLoop();
 
     /* USER CODE END WHILE */
 
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index a7f582c..231bf48 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -48,7 +48,7 @@ float DiffBot::readEncoderRight()
     return distance / dt;
 }
 
-DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt)
+DiffBot::DiffBot(Point pos) : pos(pos)
 {
 };
 
@@ -73,7 +73,11 @@ void DiffBot::update(float dt_actual)
 {
     this->dt = dt_actual;
 
-    if (!isDelayPassed(dt * 1000)) return;
+    if (!isDelayPassed(dt * 1000))
+    {
+        motor.update();
+        return;
+    }
 
     float rightVel = readEncoderRight();
     float leftVel = readEncoderLeft();
@@ -88,8 +92,8 @@ void DiffBot::update(float dt_actual)
     pos.y += dDistance * sinf(avgTheta);
     pos.theta = normalizeAngle(pos.theta + dTheta);
 
-    if (std::abs(rightVel) < 0.001f && std::abs(leftVel) < 0.001f &&
-        (std::abs(motor.rightTarget_PWM) > 50 || std::abs(motor.leftTarget_PWM) > 50))
+    if (std::abs(rightVel) < 0.0005f && std::abs(leftVel) < 0.0005f &&
+        (std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
     {
         if (!no_move)
         {
@@ -154,6 +158,10 @@ void DiffBot::update(float dt_actual)
             wRef = pidTheta.compute(0, -finalAngleErr, dt);
 
         } else {
+            char log[64];
+            int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index);
+            CDC_Transmit_FS((uint8_t*)log, len);
+
             targets[index].active = false;
             index = (index + 1) % MAX_WAYPOINTS;
             resetPID();
diff --git a/Core/Src/setup.cpp b/Core/Src/setup.cpp
index 1fce036..e03ef7f 100644
--- a/Core/Src/setup.cpp
+++ b/Core/Src/setup.cpp
@@ -9,13 +9,22 @@
 #include <modelec.h>
 #include "commSTM.h"
 
-DiffBot bot(Point(), 0.01f);
+DiffBot bot = DiffBot(Point());
 
 void ModelecOdometrySetup() {
 	bot.setup();
 }
 
-void ModelecOdometryLoop(float dt) {
+uint32_t lastTick = 0;
+
+void ModelecOdometryLoop() {
+	uint32_t currentTick = HAL_GetTick();
+	float actualDt = (currentTick - lastTick) / 1000.0f;
+
+	if (actualDt <= 0.0f) return;
+
 	USB_Comm_Process();
-	bot.update(dt);
+	bot.update(actualDt);
+
+	lastTick = currentTick;
 }