From cc42b9815e59689aa84a1467bcf04cfe42b80b1a Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Fri, 6 Mar 2026 19:36:18 +0100
Subject: [PATCH] PID test

---
 Core/Inc/modelec.h         |  3 ++-
 Core/Src/CommCallbacks.cpp |  4 ++--
 Core/Src/modelec.cpp       | 21 ++++++++++++++-------
 3 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index 6554fa3..7842be7 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -61,7 +61,8 @@ public:
 	uint32_t publishLastTick = 0;
 	uint32_t frequencyPublish = 100;
 
-	float preciseAngle = 0.017f;
+	float preciseAngleFinal = 0.017f;
+	float preciseAngle = 0.39f;
 	float precisePosFinal = 0.01f;
 	float precisePos = 0.1f;
 
diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp
index 38223e1..1d2d1f7 100644
--- a/Core/Src/CommCallbacks.cpp
+++ b/Core/Src/CommCallbacks.cpp
@@ -107,7 +107,7 @@ void Comm_GetPublishFrequency(uint32_t &frequencyPublish) {
 }
 
 float Comm_GetPreciseAngle() {
-	return bot.preciseAngle;
+	return bot.preciseAngleFinal;
 }
 
 float Comm_GetPrecisePos() {
@@ -119,7 +119,7 @@ float Comm_GetPrecisePosFinal() {
 }
 
 void Comm_SetPreciseAngle(float d) {
-	bot.preciseAngle = d;
+	bot.preciseAngleFinal = d;
 }
 
 void Comm_SetPrecisePos(float d) {
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index ba1b25a..0fe447b 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
 }
 
 void DiffBot::setup() {
-	pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
-	pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
-	pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX);
-	pidTheta = PID(2, 0.0, 0.0, -M_PI, M_PI);
+	pidLeft = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX);
+	pidRight = PID(2, 0, 0.5, -PWM_MAX, PWM_MAX);
+	pidPos = PID(3, -0.2, 1.0, -V_MAX, V_MAX);
+	pidTheta = PID(6, 0.5, 0.5, -M_PI, M_PI);
 
 	prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
 	prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -66,7 +66,14 @@ void DiffBot::update(float dt) {
         	else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
         		motor.stop(true);
 
-        		targets[index].active = false;
+        		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;
+        		}
 
     			no_move = false;
 
@@ -146,7 +153,7 @@ void DiffBot::update(float dt) {
     switch (targets[index].state) {
     case FINAL:
 
-    	if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngle) {
+    	if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngleFinal) {
     		targets[index].active = false;
     		motor.stop(true);
 
@@ -202,7 +209,7 @@ void DiffBot::update(float dt) {
 		angleError > 0 ? angleError -= M_PI : angleError += M_PI;
 	}
 
-	if (std::fabs(angleError) <= precisePosFinal) angleError = 0;
+	if (std::fabs(angleError) <= preciseAngle) angleError = 0;
 
 	float distError = dist * cosf(angleError);