modelec/odo_STM32
1
0
Fork 0
mirror of https://github.com/modelec/odo_STM32.git synced 2026-03-18 21:30:38 +01:00
Code Issues Packages Projects Releases Wiki Activity

read multiple waypoints from USB

Browse Source
This commit is contained in:
acki
2025-09-20 19:14:48 +02:00
parent 830cc89558
commit 40bc40d491
2 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
Core/Src/commSTM.cpp
View File

@@ -116,12 +116,27 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;PID\n"); USB_Comm_Send("OK;PID\n");
} }
else if (strcmp(token, "WAYPOINT") == 0) { else if (strcmp(token, "WAYPOINT") == 0) {
int id = atoi(strtok(NULL, ";"));
int type = atoi(strtok(NULL, ";")); while (true) {
float x = atof(strtok(NULL, ";"));
float y = atof(strtok(NULL, ";")); char* idTok = strtok(nullptr, ";");
float t = atof(strtok(NULL, ";")); char* typeTok = strtok(nullptr, ";");
Comm_AddWaypoint(id, type, x, y, t); 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"); USB_Comm_Send("OK;WAYPOINT\n");
} }
else if (strcmp(token, "START") == 0) { else if (strcmp(token, "START") == 0) {

14
Core/Src/modelec.cpp
View File

@@ -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;
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()) { switch (targets[index].getState()) {
case StatePoint::FINAL: case StatePoint::FINAL:
if (fabs(dx) < 0.005 && fabs(dy) < 0.005 && fabs(angleError) < 0.08 /* 5deg */) { if (fabs(dx) < 0.005 && fabs(dy) < 0.005 && fabs(angleError) < 0.08 /* 5deg */) {
stop(true); stop(true);
char log[128]; char log[32];
sprintf(log, "SET;WAYPOINT;%d\n", index); sprintf(log, "SET;WAYPOINT;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, strlen(log)); CDC_Transmit_FS((uint8_t*)log, strlen(log));
return;
} }
break; 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;
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); sprintf(log, "SET;WAYPOINT;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, strlen(log)); 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); targets[id].setState(type == 1 ? StatePoint::FINAL : StatePoint::INTERMEDIAIRE);
// if (type == StatePoint::FINAL) index = 0; // if (type == StatePoint::FINAL) index = 0;
index = 0; if (id <= index) index = 0;
arrive = false; arrive = false;
} }