From 61a70aea5a99b17643251cc70d5b076aa99f8e0a Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 10 Nov 2025 18:18:48 +0100 Subject: [PATCH] update serial comm --- Core/Inc/CommCallbacks.h | 10 +++- Core/Inc/modelec.h | 2 + Core/Inc/pid.h | 8 ++++ Core/Src/CommCallbacks.cpp | 84 ++++++++++++++++----------------- Core/Src/commSTM.cpp | 97 +++++++++++++++++++++++++++++++------- Core/Src/modelec.cpp | 4 +- 6 files changed, 143 insertions(+), 62 deletions(-) diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h index 8514b3d..3d75abd 100644 --- a/Core/Inc/CommCallbacks.h +++ b/Core/Inc/CommCallbacks.h @@ -1,6 +1,8 @@ #ifndef COMM_CALLBACKS_HPP #define COMM_CALLBACKS_HPP +#include + #ifdef __cplusplus extern "C" { #endif @@ -11,9 +13,9 @@ void Comm_SetPos(float x, float y, float t); void Comm_GetSpeed(float& vx, float& vy, float& omega); -bool Comm_GetPID(char *pid, float &p, float &i, float &d); +bool Comm_GetPID(char *pid_name, float &p, float &i, float &d, float &v_min, float &v_max); -bool Comm_SetPID(char *pid, float p, float i, float d); +bool Comm_SetPID(char *pid_name, float p, float i, float d, float out_min, float out_max); void Comm_StartOdometry(bool start); @@ -23,6 +25,10 @@ float Comm_GetDistance(int sensorId); void Comm_SetPWM(float left, float right); +void Comm_SetPublishFrequency(uint32_t frequencyPublish = 20); + +void Comm_GetPublishFrequency(uint32_t &frequencyPublish); + #ifdef __cplusplus } #endif diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index 81b6472..2a942b9 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -55,6 +55,8 @@ public: bool arrive = false; uint32_t lastTick = 0; + uint32_t publishLastTick = 0; + uint32_t frequencyPublish = 20; static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick); diff --git a/Core/Inc/pid.h b/Core/Inc/pid.h index 356b9ca..c3689fa 100644 --- a/Core/Inc/pid.h +++ b/Core/Inc/pid.h @@ -29,9 +29,17 @@ public: this->kd = kd; } + void setLimits(float out_min, float out_max) { + this->outMin = out_min; + this->outMax = out_max; + } + float getKp() const { return kp; } float getKi() const { return ki; } float getKd() const { return kd; } + float getOutMin() const { return outMin; } + float getOutMax() const { return outMax; } + }; diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp index a0f0b8a..6ad218f 100644 --- a/Core/Src/CommCallbacks.cpp +++ b/Core/Src/CommCallbacks.cpp @@ -12,6 +12,7 @@ #include "modelec.h" #include "motors.h" #include "pid.h" +#include extern DiffBot bot; @@ -33,50 +34,41 @@ void Comm_GetSpeed(float& vx, float& vy, float& omega) { omega = bot.vtheta; } -bool Comm_GetPID(char *pid, float &p, float &i, float &d) { - if (strcmp(pid, "LEFT") == 0) { - p = bot.pidLeft.getKp(); - i = bot.pidLeft.getKi(); - d = bot.pidLeft.getKd(); - } - else if (strcmp(pid, "RIGHT") == 0) { - p = bot.pidRight.getKp(); - i = bot.pidRight.getKi(); - d = bot.pidRight.getKd(); - } - else if (strcmp(pid, "POS") == 0) { - p = bot.pidPos.getKp(); - i = bot.pidPos.getKi(); - d = bot.pidPos.getKd(); - } - else if (strcmp(pid, "THETA") == 0) { - p = bot.pidTheta.getKp(); - i = bot.pidTheta.getKi(); - d = bot.pidTheta.getKd(); - } - else { - return false; - } - return true; +bool Comm_GetPID(char *pid_name, float &p, float &i, float &d, float &v_min, float &v_max) { + PID* pid = nullptr; + + if (strcmp(pid_name, "LEFT") == 0) pid = &bot.pidLeft; + else if (strcmp(pid_name, "RIGHT") == 0) pid = &bot.pidRight; + else if (strcmp(pid_name, "POS") == 0) pid = &bot.pidPos; + else if (strcmp(pid_name, "THETA") == 0) pid = &bot.pidTheta; + else return false; + + p = pid->getKp(); + i = pid->getKi(); + d = pid->getKd(); + v_min = pid->getOutMin(); + v_max = pid->getOutMax(); + + return true; } -bool Comm_SetPID(char *pid, float p, float i, float d) { - if (strcmp(pid, "LEFT") == 0) { - bot.pidLeft.setTunings(p, i, d); - } - else if (strcmp(pid, "RIGHT") == 0) { - bot.pidRight.setTunings(p, i, d); - } - else if (strcmp(pid, "POS") == 0) { - bot.pidPos.setTunings(p, i, d); - } - else if (strcmp(pid, "THETA") == 0) { - bot.pidTheta.setTunings(p, i, d); - } - else { - return false; - } - return true; +bool Comm_SetPID(char *pid_name, float p, float i, float d, float out_min, float out_max) { + PID* pid = nullptr; + + if (strcmp(pid_name, "LEFT") == 0) pid = &bot.pidLeft; + else if (strcmp(pid_name, "RIGHT") == 0) pid = &bot.pidRight; + else if (strcmp(pid_name, "POS") == 0) pid = &bot.pidPos; + else if (strcmp(pid_name, "THETA") == 0) pid = &bot.pidTheta; + else return false; + + pid->setTunings(p, i, d); + + // only set limits if user provided them + if (!std::isnan(out_min) && !std::isnan(out_max)) { + pid->setLimits(out_min, out_max); + } + + return true; } void Comm_StartOdometry(bool on) { @@ -95,3 +87,11 @@ void Comm_SetPWM(float left, float right) { bot.motor.leftTarget_PWM = left; bot.motor.rightTarget_PWM = right; } + +void Comm_SetPublishFrequency(uint32_t frequencyPublish) { + bot.frequencyPublish = frequencyPublish; +} + +void Comm_GetPublishFrequency(uint32_t &frequencyPublish) { + frequencyPublish = bot.frequencyPublish; +} diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp index 4f3569c..7ace5bf 100644 --- a/Core/Src/commSTM.cpp +++ b/Core/Src/commSTM.cpp @@ -11,6 +11,8 @@ #include "modelec.h" #include "usbd_cdc_if.h" +#include + // fourni par CubeMX dans usb_device.c extern USBD_HandleTypeDef hUsbDeviceFS; @@ -31,17 +33,24 @@ void USB_Comm_OnReceive(uint8_t* Buf, uint32_t Len) { for (uint32_t i = 0; i < Len; i++) { char c = Buf[i]; + if (c == '\n' || c == '\r') { + if (usb_rx_index == 0) continue; // ignore empty lines + usb_rx_buffer[usb_rx_index] = '\0'; memcpy(parse_buffer, usb_rx_buffer, usb_rx_index + 1); usb_rx_index = 0; data_ready = 1; - return; + + USB_Comm_Process(); } else { - usb_rx_buffer[usb_rx_index++] = c; + if (usb_rx_index < RX_BUFFER_SIZE - 1) { + usb_rx_buffer[usb_rx_index++] = c; + } else { + usb_rx_index = 0; + } } - } -} + }} // Répondre via USB static void USB_Comm_Send(const char* message) { @@ -78,10 +87,10 @@ void USB_Comm_Process(void) { } else if (strcmp(token, "PID") == 0) { char* pid = strtok(NULL, ";"); - float p, i, d; - if (Comm_GetPID(pid, p, i, d)) { + float p, i, d, v_min, v_max; + if (Comm_GetPID(pid, p, i, d, v_min, v_max)) { char response[64]; - snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d); + snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f;%.4f,%.4f\n", p, i, d, v_min, v_max); USB_Comm_Send(response); } else { @@ -97,6 +106,15 @@ void USB_Comm_Process(void) { snprintf(response, sizeof(response), "SET;DIST;%d;%.2f\n", n, dist); USB_Comm_Send(response); } + else if (strcmp(token, "FREQUENCY")) { + token = strtok(NULL, ";"); + if (!token) return; + uint32_t freq; + Comm_GetPublishFrequency(freq); + char response[64]; + snprintf(response, sizeof(response), "SET;FREQUENCY;%ld\n", freq); + USB_Comm_Send(response); + } else { USB_Comm_Send("KO;UNKNOWN\n"); } @@ -114,16 +132,52 @@ void USB_Comm_Process(void) { } else if (strcmp(token, "PID") == 0) { char* pid = strtok(NULL, ";"); - float p = atof(strtok(NULL, ";")); - float i = atof(strtok(NULL, ";")); - float d = atof(strtok(NULL, ";")); + if (!pid) { + USB_Comm_Send("KO;PID;MISSING_NAME\n"); + return; + } - if (Comm_SetPID(pid, p, i, d)) { - USB_Comm_Send("OK;PID\n"); + char* p_str = strtok(NULL, ";"); + char* i_str = strtok(NULL, ";"); + char* d_str = strtok(NULL, ";"); + + if (!p_str || !i_str || !d_str) { + char msg[64]; + snprintf(msg, sizeof(msg), "KO;PID;%s;MISSING_VALUES\n", pid); + USB_Comm_Send(msg); + return; } - else { - USB_Comm_Send("KO;PID;UNKNOWN\n"); + + float p = atof(p_str); + float i = atof(i_str); + float d = atof(d_str); + + // Optional parameters + char* out_min_str = strtok(NULL, ";"); + char* out_max_str = strtok(NULL, ";"); + + bool has_out_min = (out_min_str != nullptr); + bool has_out_max = (out_max_str != nullptr); + + float out_min = has_out_min ? atof(out_min_str) : 0.0f; + float out_max = has_out_max ? atof(out_max_str) : 0.0f; + + bool success = false; + + // Call with or without output limits + if (has_out_min && has_out_max) { + success = Comm_SetPID(pid, p, i, d, out_min, out_max); + } else { + success = Comm_SetPID(pid, p, i, d, NAN, NAN); } + + char msg[64]; + if (success) { + snprintf(msg, sizeof(msg), "OK;PID;%s\n", pid); + } else { + snprintf(msg, sizeof(msg), "KO;PID;%s;UNKNOWN\n", pid); + } + USB_Comm_Send(msg); } else if (strcmp(token, "WAYPOINT") == 0) { @@ -168,11 +222,22 @@ void USB_Comm_Process(void) { USB_Comm_Send("OK;MOTOR\n"); } + else if (strcmp(token, "FREQUENCY")) { + uint32_t freq = atoi(strtok(NULL, ";")); + Comm_SetPublishFrequency(freq); + char response[64]; + snprintf(response, sizeof(response), "OK;FREQUENCY;%ld\n", freq); + USB_Comm_Send(response); + } else { - USB_Comm_Send("KO;UNKNOWN\n"); + char response[268]; + snprintf(response, sizeof(response), "KO;UNKNOWN;%s\n", parse_buffer); + USB_Comm_Send(response); } } else { - USB_Comm_Send("KO;UNKNOWN\n"); + char response[268]; + snprintf(response, sizeof(response), "KO;UNKNOWN;%s\n", parse_buffer); + USB_Comm_Send(response); } } diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 8b70302..8e2887e 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -44,7 +44,7 @@ void DiffBot::setup() { pidLeft = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidRight = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX); pidPos = PID(3, 0.0, 0.0, -V_MAX, V_MAX); - pidTheta = PID(4, 0.0, 0.0, -2.0f, 2); + pidTheta = PID(4, 0.0, 0.0, -M_PI, M_PI); prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); @@ -71,7 +71,7 @@ void DiffBot::update(float dt) { while (pos.theta > M_PI) pos.theta -= 2*M_PI; while (pos.theta < -M_PI) pos.theta += 2*M_PI; - if (odo_active) { + if (odo_active && isDelayPassedFrom(dt*1000*frequencyPublish, publishLastTick)) { publishStatus(); }