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

dt

Browse Source
This commit is contained in:
acki
2026-03-07 14:08:15 +01:00
parent eea5bf7df0
commit e2938857fb
5 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Core/Inc/modelec.h
View File

@@ -75,7 +75,7 @@ public:
float readEncoderLeft();
DiffBot(Point pos, float dt);
DiffBot(Point pos);
void stop(bool stop);

2
Core/Inc/setup.h
View File

@@ -13,7 +13,7 @@ extern "C" {
#endif
void ModelecOdometrySetup();
void ModelecOdometryLoop(float dt);
void ModelecOdometryLoop();
#ifdef __cplusplus
}

2
Core/Src/main.c
View File

@@ -111,7 +111,7 @@ int main(void)
while (1)
{
ModelecOdometryLoop(0.01f);
ModelecOdometryLoop();
/* USER CODE END WHILE */

16
Core/Src/modelec.cpp
View File

@@ -48,7 +48,7 @@ float DiffBot::readEncoderRight()
return distance / dt;
}
DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt)
DiffBot::DiffBot(Point pos) : pos(pos)
{
};
@@ -73,7 +73,11 @@ void DiffBot::update(float dt_actual)
{
this->dt = dt_actual;
if (!isDelayPassed(dt * 1000)) return;
if (!isDelayPassed(dt * 1000))
{
motor.update();
return;
}
float rightVel = readEncoderRight();
float leftVel = readEncoderLeft();
@@ -88,8 +92,8 @@ void DiffBot::update(float dt_actual)
pos.y += dDistance * sinf(avgTheta);
pos.theta = normalizeAngle(pos.theta + dTheta);
if (std::abs(rightVel) < 0.001f && std::abs(leftVel) < 0.001f &&
(std::abs(motor.rightTarget_PWM) > 50 || std::abs(motor.leftTarget_PWM) > 50))
if (std::abs(rightVel) < 0.0005f && std::abs(leftVel) < 0.0005f &&
(std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
{
if (!no_move)
{
@@ -154,6 +158,10 @@ void DiffBot::update(float dt_actual)
wRef = pidTheta.compute(0, -finalAngleErr, dt);
} else {
char log[64];
int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index);
CDC_Transmit_FS((uint8_t*)log, len);
targets[index].active = false;
index = (index + 1) % MAX_WAYPOINTS;
resetPID();

15
Core/Src/setup.cpp
View File

@@ -9,13 +9,22 @@
#include <modelec.h>
#include "commSTM.h"
DiffBot bot(Point(), 0.01f);
DiffBot bot = DiffBot(Point());
void ModelecOdometrySetup() {
bot.setup();
}
void ModelecOdometryLoop(float dt) {
uint32_t lastTick = 0;
void ModelecOdometryLoop() {
uint32_t currentTick = HAL_GetTick();
float actualDt = (currentTick - lastTick) / 1000.0f;
if (actualDt <= 0.0f) return;
USB_Comm_Process();
bot.update(dt);
bot.update(actualDt);
lastTick = currentTick;
}