From 74dde8d2283f58d9f7942b8fad18860588438595 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Mon, 9 Mar 2026 18:33:58 +0100
Subject: [PATCH] omg i pray for time like this

---
 Core/Inc/modelec.h   |  2 +-
 Core/Src/modelec.cpp | 38 ++++++++++++++++++++++++--------------
 Core/Src/motors.cpp  |  2 +-
 3 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index df0cade..9097995 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -63,7 +63,7 @@ public:
 
 	float preciseAngleFinal = 0.017f;
 	float preciseAngle = 0.39f;
-	float precisePosFinal = 0.001f;
+	float precisePosFinal = 0.0005f;
 	float precisePos2 = 0.02f;
 	float precisePos = 0.1f;
 
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 231bf48..a39a9e3 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) : pos(pos)
+DiffBot::DiffBot(Point pos) : pos(pos), dt(0)
 {
 };
 
@@ -60,10 +60,10 @@ void DiffBot::stop(bool stop)
 
 void DiffBot::setup()
 {
-    pidLeft = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX);
-    pidRight = PID(2.5f, 0.1f, 0.0f, -PWM_MAX, PWM_MAX);
-    pidPos = PID(3.0f, 0.0f, 0.01f, -V_MAX, V_MAX);
-    pidTheta = PID(5.0f, 0.0f, 0.01f, -M_PI, M_PI);
+    pidLeft = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
+    pidRight = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
+    pidPos = PID(6.0f, 0.0f, 0.1f, -V_MAX, V_MAX);
+    pidTheta = PID(10.0f, 0.0f, 0.1f, -M_PI, M_PI);
 
     prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
     prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -92,7 +92,7 @@ void DiffBot::update(float dt_actual)
     pos.y += dDistance * sinf(avgTheta);
     pos.theta = normalizeAngle(pos.theta + dTheta);
 
-    if (std::abs(rightVel) < 0.0005f && std::abs(leftVel) < 0.0005f &&
+    if (std::abs(rightVel) == 0.0f && std::abs(leftVel) == 0.0f &&
         (std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
     {
         if (!no_move)
@@ -102,17 +102,20 @@ void DiffBot::update(float dt_actual)
         }
         else if (HAL_GetTick() - publishNotMoved > notMovedMaxTime)
         {
-            char log[64];
-            int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index);
-            CDC_Transmit_FS((uint8_t*)log, len);
+            /*if (targets[index].active)
+            {
+                char log[64];
+                sprintf(log, "SET;WAYPOINT;REACH;%d\n", index);
+                CDC_Transmit_FS((uint8_t*)log, strlen(log));
 
-            targets[index].active = false;
+                targets[index].active = false;
+            }
 
             index = (index + 1) % MAX_WAYPOINTS;
 
             motor.stop(true);
             resetPID();
-            return;
+            return;*/
         }
     }
     else
@@ -148,7 +151,6 @@ void DiffBot::update(float dt_actual)
                 CDC_Transmit_FS((uint8_t*)log, len);
 
                 targets[index].active = false;
-                index = (index + 1) % MAX_WAYPOINTS;
                 motor.stop(true);
                 resetPID();
                 return;
@@ -197,8 +199,16 @@ void DiffBot::update(float dt_actual)
         return limited;
     };
 
-    motor.leftTarget_PWM = static_cast<int16_t>(applyLimits(pwmLeft));
-    motor.rightTarget_PWM = static_cast<int16_t>(applyLimits(pwmRight));
+    auto applyDeadzone = [&](float pwm) {
+        const float min_pwm = 35.0f;
+        if (std::abs(pwm) < 1.0f) return 0.0f;
+
+        if (pwm > 0) return pwm + min_pwm;
+        else return pwm - min_pwm;
+    };
+
+    motor.leftTarget_PWM = static_cast<int16_t>(applyLimits(applyDeadzone(pwmLeft)));
+    motor.rightTarget_PWM = static_cast<int16_t>(applyLimits(applyDeadzone(pwmRight)));
     motor.update();
 }
 
diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp
index 444b211..8cdd14e 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 = 50;
+    int16_t max_step = 10;
 
     leftTarget_PWM  = std::max(-PWM_MAX, std::min(leftTarget_PWM,  PWM_MAX));
     rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_PWM, PWM_MAX));