From 727b422166c87b7abfea72d7e7834e1afe3e729c Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Thu, 12 Mar 2026 16:56:09 +0100
Subject: [PATCH] PID

---
 Core/Inc/modelec.h   | 16 ++++++++++------
 Core/Src/modelec.cpp | 24 +++++++++++++-----------
 2 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index df0cade..dde7296 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -15,14 +15,15 @@
 #include "pid.h"
 #include "point.h"
 
+#define PWM_MAX 640.0f
+#define V_MAX 0.643f // m/s
+
 #define MAX_WAYPOINTS 16
-#define PWM_MAX 626.0f
 #define ENCODER_RES 2400.0f
-#define WHEEL_DIAMETER 0.082f
+#define WHEEL_DIAMETER 0.081f
 #define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
 #define WHEEL_BASE 0.29f
 #define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
-#define V_MAX 0.643f // m/s
 
 extern TIM_HandleTypeDef htim3;
 extern TIM_HandleTypeDef htim2;
@@ -52,7 +53,7 @@ public:
 	bool arrive = false;
 
 	uint32_t publishNotMoved = 0;
-	uint32_t notMovedMaxTime = 100;
+	uint32_t notMovedMaxTime = 300;
 	bool no_move = false;
 
 	uint8_t action = 0;
@@ -61,11 +62,14 @@ public:
 	uint32_t publishLastTick = 0;
 	uint32_t frequencyPublish = 100;
 
-	float preciseAngleFinal = 0.017f;
+	float preciseAngleFinal = 0.005f;
 	float preciseAngle = 0.39f;
 	float precisePosFinal = 0.001f;
 	float precisePos2 = 0.02f;
-	float precisePos = 0.1f;
+	float precisePos = 0.15f;
+
+	float currentV = 0;
+	const float maxAccel = 0.5f;
 
 	static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick);
 
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index ca7ce50..ccaec80 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -60,10 +60,10 @@ void DiffBot::stop(bool stop)
 
 void DiffBot::setup()
 {
-    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);
+    pidLeft = PID(5.f, 0.5f, 0.5f, -PWM_MAX, PWM_MAX);
+    pidRight = PID(5.f, 0.5f, 0.5f, -PWM_MAX, PWM_MAX);
+    pidPos = PID(4.0f, 0.0f, 0.0f, -V_MAX, V_MAX);
+    pidTheta = PID(5.0f, 1.0f, 0.0f, -M_PI, M_PI);
 
     prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
     prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -73,12 +73,6 @@ void DiffBot::update(float dt_actual)
 {
     this->dt = dt_actual;
 
-    if (!isDelayPassed(dt * 1000))
-    {
-        motor.update();
-        return;
-    }
-
     float rightVel = readEncoderRight();
     float leftVel = readEncoderLeft();
 
@@ -92,6 +86,7 @@ void DiffBot::update(float dt_actual)
     pos.y += dDistance * sinf(avgTheta);
     pos.theta = normalizeAngle(pos.theta + dTheta);
 
+    // TODO : review this part cause do not work.
     if (std::abs(rightVel) == 0.0f && std::abs(leftVel) == 0.0f &&
         (std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
     {
@@ -186,7 +181,13 @@ void DiffBot::update(float dt_actual)
         float alignmentScale = cosf(angleError);
         if (alignmentScale < 0) alignmentScale = 0;
 
-        vRef = pidPos.compute(0, -dist * direction, dt) * alignmentScale;
+        float targetV = pidPos.compute(0, -dist * direction, dt) * alignmentScale;
+
+        if (targetV > currentV + (maxAccel * dt)) currentV += maxAccel * dt;
+        else if (targetV < currentV - (maxAccel * dt)) currentV -= maxAccel * dt;
+        else currentV = targetV;
+
+        vRef = currentV;
     }
 
     float vLeftReq = vRef - (WHEEL_BASE_2 * wRef);
@@ -224,6 +225,7 @@ void DiffBot::addTarget(int id, int type, float x, float y, float theta)
     if (id <= index) index = 0;
 
     arrive = false;
+    no_move = false;
 }
 
 void DiffBot::resetPID()