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);