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

update serial comm

Browse Source
This commit is contained in:
acki
2025-11-10 18:18:48 +01:00
parent efb7e3e855
commit 61a70aea5a
6 changed files with 143 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Core/Inc/CommCallbacks.h
View File

@@ -1,6 +1,8 @@
#ifndef COMM_CALLBACKS_HPP
#define COMM_CALLBACKS_HPP
#include <stdint.h>
#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

2
Core/Inc/modelec.h
View File

@@ -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);

8
Core/Inc/pid.h
View File

@@ -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; }
};

84
Core/Src/CommCallbacks.cpp
View File

@@ -12,6 +12,7 @@
#include "modelec.h"
#include "motors.h"
#include "pid.h"
#include <cmath>
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;
}

97
Core/Src/commSTM.cpp
View File

@@ -11,6 +11,8 @@
#include "modelec.h"
#include "usbd_cdc_if.h"
#include <cmath>
// 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);
}
}

4
Core/Src/modelec.cpp
View File

@@ -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();
}