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

working odometry

Browse Source
This commit is contained in:
acki
2025-09-30 18:56:46 +02:00
parent 70c48a25a4
commit 8b4db1c18c
13 changed files with 208 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
Core/Src/commSTM.cpp
View File

@@ -5,9 +5,11 @@
* Author: maxch
*/
#include <CommCallbacks.h>
#include "CommCallbacks.h"
#include "commSTM.h"
#include "usbd_cdc.h"
#include "modelec.h"
#include "usbd_cdc_if.h"
// fourni par CubeMX dans usb_device.c
extern USBD_HandleTypeDef hUsbDeviceFS;
@@ -75,11 +77,16 @@ void USB_Comm_Process(void) {
USB_Comm_Send(response);
}
else if (strcmp(token, "PID") == 0) {
char* pid = strtok(NULL, ";");
float p, i, d;
Comm_GetPID(p, i, d);
char response[64];
snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d);
USB_Comm_Send(response);
if (Comm_GetPID(pid, p, i, d)) {
char response[64];
snprintf(response, sizeof(response), "SET;PID;%.4f;%.4f;%.4f\n", p, i, d);
USB_Comm_Send(response);
}
else {
USB_Comm_Send("KO;PID;UNKNOWN\n");
}
}
else if (strcmp(token, "DIST") == 0) {
token = strtok(NULL, ";");
@@ -106,11 +113,17 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;POS\n");
}
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, ";"));
Comm_SetPID(p, i, d);
USB_Comm_Send("OK;PID\n");
if (Comm_SetPID(pid, p, i, d)) {
USB_Comm_Send("OK;PID\n");
}
else {
USB_Comm_Send("KO;PID;UNKNOWN\n");
}
}
else if (strcmp(token, "WAYPOINT") == 0) {
@@ -142,6 +155,19 @@ void USB_Comm_Process(void) {
USB_Comm_Send("OK;START\n");
}
else if (strcmp(token, "MOTOR") == 0) {
float left = atof(strtok(NULL, ";"));
float right = atof(strtok(NULL, ";"));
if (left < -PWM_MAX || left > PWM_MAX || right < -PWM_MAX || right > PWM_MAX) {
USB_Comm_Send("KO;MOTOR;RANGE\n");
return;
}
Comm_SetPWM(left, right);
USB_Comm_Send("OK;MOTOR\n");
}
else {
USB_Comm_Send("KO;UNKNOWN\n");
}