From 40bc40d49199b7faa8b896eb5261dfa9896ae90f Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Sat, 20 Sep 2025 19:14:48 +0200
Subject: [PATCH] read multiple waypoints from USB

---
 Core/Src/commSTM.cpp | 27 +++++++++++++++++++++------
 Core/Src/modelec.cpp | 14 +++++---------
 2 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp
index 2657884..92432eb 100644
--- a/Core/Src/commSTM.cpp
+++ b/Core/Src/commSTM.cpp
@@ -116,12 +116,27 @@ void USB_Comm_Process(void) {
             USB_Comm_Send("OK;PID\n");
         }
         else if (strcmp(token, "WAYPOINT") == 0) {
-            int id = atoi(strtok(NULL, ";"));
-            int type = atoi(strtok(NULL, ";"));
-            float x = atof(strtok(NULL, ";"));
-            float y = atof(strtok(NULL, ";"));
-            float t = atof(strtok(NULL, ";"));
-            Comm_AddWaypoint(id, type, x, y, t);
+
+            while (true) {
+
+                char* idTok = strtok(nullptr, ";");
+                char* typeTok = strtok(nullptr, ";");
+                char* xTok = strtok(nullptr, ";");
+                char* yTok = strtok(nullptr, ";");
+                char* tTok = strtok(nullptr, ";");
+
+                if (!idTok || !typeTok || !xTok || !yTok || !tTok) {
+                    break;
+                }
+
+                int id = atoi(idTok);
+                int type = atoi(typeTok);
+                float x = atof(xTok);
+                float y = atof(yTok);
+                float theta = atof(tTok);
+
+                Comm_AddWaypoint(id, type, x, y, theta);
+            }
             USB_Comm_Send("OK;WAYPOINT\n");
         }
         else if (strcmp(token, "START") == 0) {
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 5d1ac6f..af8f01d 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -78,21 +78,17 @@ void DiffBot::update(float dt) {
     while (angleError >  M_PI) angleError -= 2*M_PI;
     while (angleError < -M_PI) angleError += 2*M_PI;
 
-
-    /* CHECK IF ON POS THERE
-     * IF final target check if every things is on purpose like x, y, theta
-     * IF not final target check if x AND y are close and if so index++
-     */
-
     switch (targets[index].getState()) {
     case StatePoint::FINAL:
 
     	if (fabs(dx) < 0.005 && fabs(dy) < 0.005 && fabs(angleError) < 0.08 /* 5deg */) {
     		stop(true);
 
-    		char log[128];
+    		char log[32];
     		sprintf(log, "SET;WAYPOINT;%d\n", index);
     		CDC_Transmit_FS((uint8_t*)log, strlen(log));
+
+    		return;
     	}
 
     	break;
@@ -110,7 +106,7 @@ void DiffBot::update(float dt) {
     	    while (angleError >  M_PI) angleError -= 2*M_PI;
     	    while (angleError < -M_PI) angleError += 2*M_PI;
 
-    		char log[128];
+    		char log[32];
     		sprintf(log, "SET;WAYPOINT;%d\n", index);
     		CDC_Transmit_FS((uint8_t*)log, strlen(log));
     	}
@@ -146,7 +142,7 @@ void DiffBot::addTarget(int id, int type, float x, float y, float theta) {
 	targets[id].setState(type == 1 ? StatePoint::FINAL : StatePoint::INTERMEDIAIRE);
 
 	// if (type == StatePoint::FINAL) index = 0;
-	index = 0;
+	if (id <= index) index = 0;
 
     arrive = false;
 }