From fdcd541ae895dcec96e8cbe881bf45fb54fc44a6 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Mon, 16 Mar 2026 19:52:24 +0100
Subject: [PATCH] PID

---
 Core/Src/modelec.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 839a0d2..82d5700 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -99,7 +99,7 @@ void DiffBot::notifyWaypointReached(int reachedIndex)
     resetPID();
 
     int nextIndex = (reachedIndex + 1) % MAX_WAYPOINTS;
-    if (targets[nextIndex].active && targets[nextIndex].state != FINAL) {
+    if (targets[nextIndex].active && targets[reachedIndex].state != FINAL) {
         index = nextIndex;
     } else {
         motor.stop(true);