From 51cdf1c3b6831c20acc09f1aace903e13d7017d2 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 17 Nov 2025 21:07:21 +0100 Subject: [PATCH] wall alignment --- Core/Inc/CommCallbacks.h | 4 ++ Core/Src/CommCallbacks.cpp | 13 +++++ Core/Src/commSTM.cpp | 20 +++++++ Core/Src/modelec.cpp | 112 +++++++++++++++++++------------------ 4 files changed, 95 insertions(+), 54 deletions(-) diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h index 82807fc..b4972ff 100644 --- a/Core/Inc/CommCallbacks.h +++ b/Core/Inc/CommCallbacks.h @@ -53,6 +53,10 @@ uint8_t Comm_GetAction(); void Comm_SetAlignment(uint8_t action); +void Comm_GetWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active); + +void Comm_GetActiveWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active); + #ifdef __cplusplus } #endif diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index 679169b..6268dcc 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -169,3 +169,16 @@ void Comm_SetAlignment(uint8_t action) { Comm_SetAction(action); } + +void Comm_GetWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active) { + type = bot.targets[id].state; + x = bot.targets[id].x; + y = bot.targets[id].y; + theta = bot.targets[id].theta; + active = bot.targets[id].active; +} + +void Comm_GetActiveWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active) { + id = bot.index; + Comm_GetWaypoint(id, type, x, y, theta, active); +} diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index 1d7a970..59ccc26 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -115,6 +115,26 @@ void USB_Comm_Process(void) { snprintf(response, sizeof(response), "SET;FREQUENCY;%ld\n", freq); USB_Comm_Send(response); } + else if (strcmp(token, "WAYPOINT") == 0) { + token = strtok(NULL, ";"); + if (!token) return; + + float x, y, theta; + uint8_t id, type; + bool active; + + if (strcmp(token, "ACTIVE") == 0) { + Comm_GetActiveWaypoint(id, type, x, y, theta, active); + } + else { + id = atoi(token); + Comm_GetWaypoint(id, type, x, y, theta, active); + } + + char response[128]; + snprintf(response, sizeof(response), "SET;WAYPOINT;%d,%d,%.4f,%.4f,%.4f,%d\n", id, type, x, y, theta, active); + USB_Comm_Send(response); + } else if (strcmp(token, "MOTOR") == 0) { float l, r; Comm_GetPWM(l, r); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 5354429..b5fbdce 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -57,63 +57,67 @@ void DiffBot::update(float dt) { float rightVel = readEncoderRight(); float leftVel = readEncoderLeft(); - if (rightVel == 0 && leftVel == 0 && (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0)) { - // TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0 + if (rightVel == 0 && leftVel == 0) { + if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) { + // TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0 + if (!no_move) { + no_move = true; + publishNotMoved = HAL_GetTick(); + } + else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { + // BUG : apres ca le robot ne s'arrete plus quand on le relance + targets[index].active = false; - if (!no_move) { - no_move = true; - publishNotMoved = HAL_GetTick(); + 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; + + addTarget(0, 1, pos.x, pos.y, pos.theta); + + publishStatus(); + } + } } - else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { - 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(); - } + } else { + if (no_move) { + no_move = false; } - - - } else if (no_move) { - no_move = false; } // update pos