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

add more serial comm

Browse Source
This commit is contained in:
acki
2025-11-13 10:31:24 +01:00
parent 61a70aea5a
commit 97076b13eb
5 changed files with 120 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
Core/Inc/CommCallbacks.h
View File

@@ -29,6 +29,18 @@ void Comm_SetPublishFrequency(uint32_t frequencyPublish = 20);
void Comm_GetPublishFrequency(uint32_t &frequencyPublish);
float Comm_GetPreciseAngle();
float Comm_GetPrecisePos();
float Comm_GetPrecisePosFinal();
void Comm_SetPreciseAngle(float d);
void Comm_SetPrecisePos(float d);
void Comm_SetPrecisePosFinal(float d);
#ifdef __cplusplus
}
#endif

9
Core/Inc/modelec.h
View File

@@ -22,9 +22,6 @@
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
#define WHEEL_BASE 0.287f
#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
#define PRECISE_ANGLE 0.017f // radians
#define PRECISE_POS_FINAL 0.01f // meters
#define PRECISE_POS 0.1f // meters
#define V_MAX 0.643f // m/s
extern TIM_HandleTypeDef htim3;
@@ -56,7 +53,11 @@ public:
uint32_t lastTick = 0;
uint32_t publishLastTick = 0;
uint32_t frequencyPublish = 20;
uint32_t frequencyPublish = 200;
float preciseAngle = 0.017f;
float precisePosFinal = 0.01f;
float precisePos = 0.1f;
static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick);

24
Core/Src/CommCallbacks.cpp
View File

@@ -95,3 +95,27 @@ void Comm_SetPublishFrequency(uint32_t frequencyPublish) {
void Comm_GetPublishFrequency(uint32_t &frequencyPublish) {
frequencyPublish = bot.frequencyPublish;
}
float Comm_GetPreciseAngle() {
return bot.preciseAngle;
}
float Comm_GetPrecisePos() {
return bot.precisePos;
}
float Comm_GetPrecisePosFinal() {
return bot.precisePosFinal;
}
void Comm_SetPreciseAngle(float d) {
bot.preciseAngle = d;
}
void Comm_SetPrecisePos(float d) {
bot.precisePos = d;
}
void Comm_SetPrecisePosFinal(float d) {
bot.precisePosFinal = d;
}

76
Core/Src/commSTM.cpp
View File

@@ -106,7 +106,7 @@ 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")) {
else if (strcmp(token, "FREQUENCY") == 0) {
token = strtok(NULL, ";");
if (!token) return;
uint32_t freq;
@@ -115,6 +115,39 @@ void USB_Comm_Process(void) {
snprintf(response, sizeof(response), "SET;FREQUENCY;%ld\n", freq);
USB_Comm_Send(response);
}
else if (strcmp(token, "PRECISE") == 0) {
token = strtok(NULL, ";");
if (!token) {
USB_Comm_Send("KO;UNKNOWN;NEED_DATA");
}
else {
float d;
if (strcmp(token, "POS") == 0) {
token = strtok(NULL, ";");
if (strcmp(token, "FINAL") == 0) {
d = Comm_GetPrecisePosFinal();
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;POS;FINAL;%.4f\n", d);
USB_Comm_Send(response);
}
else {
d = Comm_GetPrecisePos();
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;POS;%.4f\n", d);
USB_Comm_Send(response);
}
}
else if (strcmp(token, "ANGLE") == 0) {
d = Comm_GetPreciseAngle();
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;ANGLE;%.4f\n", d);
USB_Comm_Send(response);
}
}
}
else {
USB_Comm_Send("KO;UNKNOWN\n");
}
@@ -222,13 +255,52 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;MOTOR\n");
}
else if (strcmp(token, "FREQUENCY")) {
else if (strcmp(token, "FREQUENCY") == 0) {
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 if (strcmp(token, "PRECISE") == 0) {
token = strtok(NULL, ";");
if (!token) {
USB_Comm_Send("KO;UNKNOWN;NEED_DATA");
}
else {
float d;
if (strcmp(token, "POS") == 0) {
token = strtok(NULL, ";");
if (strcmp(token, "FINAL") == 0) {
float d = atof(strtok(NULL, ";"));
Comm_SetPrecisePosFinal(d);
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;POS;FINAL;%.4f\n", d);
USB_Comm_Send(response);
}
else {
float d = atof(token);
Comm_SetPrecisePos(d);
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;POS;%.4f\n", d);
USB_Comm_Send(response);
}
}
else if (strcmp(token, "ANGLE") == 0) {
float d = atof(strtok(NULL, ";"));
Comm_SetPreciseAngle(d);
char response[64];
snprintf(response, sizeof(response), "SET;PRECISE;ANGLE;%.4f\n", d);
USB_Comm_Send(response);
}
}
}
else {
char response[268];
snprintf(response, sizeof(response), "KO;UNKNOWN;%s\n", parse_buffer);

10
Core/Src/modelec.cpp
View File

@@ -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 && isDelayPassedFrom(dt*1000*frequencyPublish, publishLastTick)) {
if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) {
publishStatus();
}
@@ -87,7 +87,7 @@ void DiffBot::update(float dt) {
switch (targets[index].state) {
case FINAL:
if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pos.theta) < PRECISE_ANGLE) {
if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs(targets[index].theta - pos.theta) < preciseAngle) {
targets[index].active = false;
motor.stop(true);
@@ -103,7 +103,7 @@ void DiffBot::update(float dt) {
break;
case INTERMEDIAIRE:
if (std::fabs(dx) < PRECISE_POS && std::fabs(dy) < PRECISE_POS) {
if (std::fabs(dx) < precisePos && std::fabs(dy) < precisePos) {
char log[32];
sprintf(log, "SET;WAYPOINT;%d\n", index);
@@ -143,7 +143,7 @@ void DiffBot::update(float dt) {
angleError > 0 ? angleError -= M_PI : angleError += M_PI;
}
if (std::fabs(angleError) <= PRECISE_POS_FINAL) angleError = 0;
if (std::fabs(angleError) <= precisePosFinal) angleError = 0;
float distError = dist * cosf(angleError);
@@ -152,7 +152,7 @@ void DiffBot::update(float dt) {
float vRef = pidPos.compute(0.0, -distError, dt);
float wRef;
if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) {
if (targets[index].state == FINAL && std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal) {
wRef = pidTheta.compute(targets[index].theta, pos.theta, dt);
vRef = 0;
}