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 alignment

Browse Source
This commit is contained in:
acki
2025-11-17 21:07:21 +01:00
parent b8626d5b93
commit 51cdf1c3b6
4 changed files with 95 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Inc/CommCallbacks.h
View File

@@ -53,6 +53,10 @@ uint8_t Comm_GetAction();
void Comm_SetAlignment(uint8_t action);
void Comm_GetWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active);
void Comm_GetActiveWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active);
#ifdef __cplusplus
}
#endif

13
Core/Src/CommCallbacks.cpp
View File

@@ -169,3 +169,16 @@ void Comm_SetAlignment(uint8_t action) {
Comm_SetAction(action);
}
void Comm_GetWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active) {
type = bot.targets[id].state;
x = bot.targets[id].x;
y = bot.targets[id].y;
theta = bot.targets[id].theta;
active = bot.targets[id].active;
}
void Comm_GetActiveWaypoint(uint8_t &id, uint8_t &type, float &x, float &y, float &theta, bool &active) {
id = bot.index;
Comm_GetWaypoint(id, type, x, y, theta, active);
}

20
Core/Src/commSTM.cpp
View File

@@ -115,6 +115,26 @@ void USB_Comm_Process(void) {
snprintf(response, sizeof(response), "SET;FREQUENCY;%ld\n", freq);
USB_Comm_Send(response);
}
else if (strcmp(token, "WAYPOINT") == 0) {
token = strtok(NULL, ";");
if (!token) return;
float x, y, theta;
uint8_t id, type;
bool active;
if (strcmp(token, "ACTIVE") == 0) {
Comm_GetActiveWaypoint(id, type, x, y, theta, active);
}
else {
id = atoi(token);
Comm_GetWaypoint(id, type, x, y, theta, active);
}
char response[128];
snprintf(response, sizeof(response), "SET;WAYPOINT;%d,%d,%.4f,%.4f,%.4f,%d\n", id, type, x, y, theta, active);
USB_Comm_Send(response);
}
else if (strcmp(token, "MOTOR") == 0) {
float l, r;
Comm_GetPWM(l, r);

112
Core/Src/modelec.cpp
View File

@@ -57,63 +57,67 @@ void DiffBot::update(float dt) {
float rightVel = readEncoderRight();
float leftVel = readEncoderLeft();
if (rightVel == 0 && leftVel == 0 && (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0)) {
// TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0
if (rightVel == 0 && leftVel == 0) {
if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) {
// TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0
if (!no_move) {
no_move = true;
publishNotMoved = HAL_GetTick();
}
else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
// BUG : apres ca le robot ne s'arrete plus quand on le relance
targets[index].active = false;
if (!no_move) {
no_move = true;
publishNotMoved = HAL_GetTick();
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;
addTarget(0, 1, pos.x, pos.y, pos.theta);
publishStatus();
}
}
}
else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
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();
}
} else {
if (no_move) {
no_move = false;
}
} else if (no_move) {
no_move = false;
}
// update pos