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

wall alignement

Browse Source
This commit is contained in:
acki
2025-11-17 19:49:24 +01:00
parent 327ec5c183
commit 25892f0b71
5 changed files with 128 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Core/Inc/CommCallbacks.h
View File

@@ -47,6 +47,12 @@ float Comm_GetNotMoveTime();
void Comm_SetNotMoveTime(float time);
void Comm_SetAction(uint8_t time);
uint8_t Comm_GetAction();
void Comm_SetAlignement(uint8_t action);
#ifdef __cplusplus
}
#endif

4
Core/Inc/modelec.h
View File

@@ -52,9 +52,11 @@ public:
bool arrive = false;
uint32_t publishNotMoved = 0;
uint32_t notMovedMaxTime = 500;
uint32_t notMovedMaxTime = 100;
bool no_move = false;
uint8_t action = 0;
uint32_t lastTick = 0;
uint32_t publishLastTick = 0;
uint32_t frequencyPublish = 100;

36
Core/Src/CommCallbacks.cpp
View File

@@ -133,3 +133,39 @@ float Comm_GetNotMoveTime() {
void Comm_SetNotMoveTime(float time) {
bot.notMovedMaxTime = time;
}
void Comm_SetAction(uint8_t time) {
bot.action = time;
}
uint8_t Comm_GetAction() {
return bot.action;
}
void Comm_SetAlignement(uint8_t action) {
float x = bot.pos.x;
float y = bot.pos.y;
float theta = bot.pos.theta;
switch (action) {
case 1: // LEFT
x = 0.0f;
break;
case 2: // TOP
y = 2.0f;
break;
case 3: // RIGHT
x = 3.0f;
break;
case 4: // BOTTOM
y = 0.0f;
break;
default:
break;
}
bot.addTarget(0, 1, x, y, theta);
Comm_SetAction(action);
}

41
Core/Src/commSTM.cpp
View File

@@ -122,6 +122,12 @@ void USB_Comm_Process(void) {
snprintf(response, sizeof(response), "SET;MOTOR;%.2f,%.2f\n", l, r);
USB_Comm_Send(response);
}
else if (strcmp(token, "ACTION") == 0) {
uint8_t action = Comm_GetAction();
char response[64];
snprintf(response, sizeof(response), "SET;ACTION;%d\n", action);
USB_Comm_Send(response);
}
else if (strcmp(token, "PRECISE") == 0) {
token = strtok(NULL, ";");
@@ -159,7 +165,7 @@ void USB_Comm_Process(void) {
token = strtok(NULL, ";");
if (token) {
if (strcmp(token, "NOT") == 0) {
if (strcmp(token, "NO") == 0) {
token = strtok(NULL, ";");
if (strcmp(token, "MOVE") == 0) {
uint32_t time = Comm_GetNotMoveTime();
@@ -284,6 +290,37 @@ void USB_Comm_Process(void) {
snprintf(response, sizeof(response), "OK;FREQUENCY;%ld\n", freq);
USB_Comm_Send(response);
}
else if (strcmp(token, "ACTION") == 0) {
uint8_t action = atoi(strtok(NULL, ";"));
Comm_SetAction(action);
char response[64];
snprintf(response, sizeof(response), "OK;ACTION;%d\n", action);
USB_Comm_Send(response);
}
else if (strcmp(token, "ALIGNEMENT") == 0) {
token = strtok(NULL, ";");
uint8_t action;
if (strcmp(token, "LEFT") == 0) {
action = 1;
}
else if (strcmp(token, "TOP") == 0) {
action = 2;
}
else if (strcmp(token, "RIGHT") == 0) {
action = 3;
}
else if (strcmp(token, "BOTTOM") == 0) {
action = 4;
}
Comm_SetAlignement(action);
char response[64];
snprintf(response, sizeof(response), "OK;ACTION;%d\n", action);
USB_Comm_Send(response);
}
else if (strcmp(token, "PRECISE") == 0) {
token = strtok(NULL, ";");
@@ -325,7 +362,7 @@ void USB_Comm_Process(void) {
token = strtok(NULL, ";");
if (token) {
if (strcmp(token, "NOT") == 0) {
if (strcmp(token, "NO") == 0) {
token = strtok(NULL, ";");
if (strcmp(token, "MOVE") == 0) {
token = strtok(NULL, ";");

45
Core/Src/modelec.cpp
View File

@@ -65,7 +65,50 @@ void DiffBot::update(float dt) {
publishNotMoved = HAL_GetTick();
}
else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
motor.stop(true);
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();
}
}