From 25892f0b71ee8d9d1a3388705cdd0e1f918290f9 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 17 Nov 2025 19:49:24 +0100 Subject: [PATCH] wall alignement --- Core/Inc/CommCallbacks.h | 6 +++++ Core/Inc/modelec.h | 4 +++- Core/Src/CommCallbacks.cpp | 36 ++++++++++++++++++++++++++++++ Core/Src/commSTM.cpp | 41 ++++++++++++++++++++++++++++++++-- Core/Src/modelec.cpp | 45 +++++++++++++++++++++++++++++++++++++- 5 files changed, 128 insertions(+), 4 deletions(-) diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h index 5ccb783..b0d7089 100644 --- a/Core/Inc/CommCallbacks.h +++ b/Core/Inc/CommCallbacks.h @@ -47,6 +47,12 @@ float Comm_GetNotMoveTime(); void Comm_SetNotMoveTime(float time); +void Comm_SetAction(uint8_t time); + +uint8_t Comm_GetAction(); + +void Comm_SetAlignement(uint8_t action); + #ifdef __cplusplus } #endif diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index 20fa8d1..abb4f2b 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -52,9 +52,11 @@ public: bool arrive = false; uint32_t publishNotMoved = 0; - uint32_t notMovedMaxTime = 500; + uint32_t notMovedMaxTime = 100; bool no_move = false; + uint8_t action = 0; + uint32_t lastTick = 0; uint32_t publishLastTick = 0; uint32_t frequencyPublish = 100; diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index b086cfa..003cd35 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -133,3 +133,39 @@ float Comm_GetNotMoveTime() { void Comm_SetNotMoveTime(float time) { bot.notMovedMaxTime = time; } + +void Comm_SetAction(uint8_t time) { + bot.action = time; +} + +uint8_t Comm_GetAction() { + return bot.action; +} + +void Comm_SetAlignement(uint8_t action) { + + float x = bot.pos.x; + float y = bot.pos.y; + float theta = bot.pos.theta; + + switch (action) { + case 1: // LEFT + x = 0.0f; + break; + case 2: // TOP + y = 2.0f; + break; + case 3: // RIGHT + x = 3.0f; + break; + case 4: // BOTTOM + y = 0.0f; + break; + default: + break; + } + + bot.addTarget(0, 1, x, y, theta); + + Comm_SetAction(action); +} diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index 109ebe4..b90cd26 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -122,6 +122,12 @@ void USB_Comm_Process(void) { snprintf(response, sizeof(response), "SET;MOTOR;%.2f,%.2f\n", l, r); USB_Comm_Send(response); } + else if (strcmp(token, "ACTION") == 0) { + uint8_t action = Comm_GetAction(); + char response[64]; + snprintf(response, sizeof(response), "SET;ACTION;%d\n", action); + USB_Comm_Send(response); + } else if (strcmp(token, "PRECISE") == 0) { token = strtok(NULL, ";"); @@ -159,7 +165,7 @@ void USB_Comm_Process(void) { token = strtok(NULL, ";"); if (token) { - if (strcmp(token, "NOT") == 0) { + if (strcmp(token, "NO") == 0) { token = strtok(NULL, ";"); if (strcmp(token, "MOVE") == 0) { uint32_t time = Comm_GetNotMoveTime(); @@ -284,6 +290,37 @@ void USB_Comm_Process(void) { snprintf(response, sizeof(response), "OK;FREQUENCY;%ld\n", freq); USB_Comm_Send(response); } + else if (strcmp(token, "ACTION") == 0) { + uint8_t action = atoi(strtok(NULL, ";")); + Comm_SetAction(action); + char response[64]; + snprintf(response, sizeof(response), "OK;ACTION;%d\n", action); + USB_Comm_Send(response); + } + else if (strcmp(token, "ALIGNEMENT") == 0) { + token = strtok(NULL, ";"); + + uint8_t action; + + if (strcmp(token, "LEFT") == 0) { + action = 1; + } + else if (strcmp(token, "TOP") == 0) { + action = 2; + } + else if (strcmp(token, "RIGHT") == 0) { + action = 3; + } + else if (strcmp(token, "BOTTOM") == 0) { + action = 4; + } + + Comm_SetAlignement(action); + + char response[64]; + snprintf(response, sizeof(response), "OK;ACTION;%d\n", action); + USB_Comm_Send(response); + } else if (strcmp(token, "PRECISE") == 0) { token = strtok(NULL, ";"); @@ -325,7 +362,7 @@ void USB_Comm_Process(void) { token = strtok(NULL, ";"); if (token) { - if (strcmp(token, "NOT") == 0) { + if (strcmp(token, "NO") == 0) { token = strtok(NULL, ";"); if (strcmp(token, "MOVE") == 0) { token = strtok(NULL, ";"); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index a6dbc66..5354429 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -65,7 +65,50 @@ void DiffBot::update(float dt) { publishNotMoved = HAL_GetTick(); } else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { - motor.stop(true); + stop(true); + + if (action != 0) { + switch (action) { + case 1: // LEFT + if (cos(pos.theta) > 0) { + pos.x = 0.1f; // dist fron back to center + } + else { + pos.x = 0.1f; // dist fron center to front + } + break; + case 2: // TOP + if (sin(pos.theta) > 0) { + pos.y = 2.0f - 0.1f; // dist fron center to front + } + else { + pos.y = 2.0f - 0.1f; // dist fron back to center + } + break; + case 3: // RIGHT + if (cos(pos.theta) > 0) { + pos.x = 3.0f - 0.1f; // dist fron center to front + } + else { + pos.x = 3.0f - 0.1f; // dist fron back to center + } + break; + case 4: // BOTTOM + if (sin(pos.theta) > 0) { + pos.y = 0.1f; // dist fron back to center + } + else { + pos.y = 0.1f; // dist fron center to front + } + break; + default: + break; + } + + action = 0; + + publishStatus(); + } }