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(); float readEncoderLeft();
DiffBot(Point pos, float dt); DiffBot(Point pos);
void stop(bool stop); void stop(bool stop);

2
Core/Inc/setup.h
View File

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

2
Core/Src/main.c
View File

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

16
Core/Src/modelec.cpp
View File

@@ -48,7 +48,7 @@ float DiffBot::readEncoderRight()
return distance / dt; 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; this->dt = dt_actual;
if (!isDelayPassed(dt * 1000)) return; if (!isDelayPassed(dt * 1000))
{
motor.update();
return;
}
float rightVel = readEncoderRight(); float rightVel = readEncoderRight();
float leftVel = readEncoderLeft(); float leftVel = readEncoderLeft();
@@ -88,8 +92,8 @@ void DiffBot::update(float dt_actual)
pos.y += dDistance * sinf(avgTheta); pos.y += dDistance * sinf(avgTheta);
pos.theta = normalizeAngle(pos.theta + dTheta); pos.theta = normalizeAngle(pos.theta + dTheta);
if (std::abs(rightVel) < 0.001f && std::abs(leftVel) < 0.001f && if (std::abs(rightVel) < 0.0005f && std::abs(leftVel) < 0.0005f &&
(std::abs(motor.rightTarget_PWM) > 50 || std::abs(motor.leftTarget_PWM) > 50)) (std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
{ {
if (!no_move) if (!no_move)
{ {
@@ -154,6 +158,10 @@ void DiffBot::update(float dt_actual)
wRef = pidTheta.compute(0, -finalAngleErr, dt); wRef = pidTheta.compute(0, -finalAngleErr, dt);
} else { } 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; targets[index].active = false;
index = (index + 1) % MAX_WAYPOINTS; index = (index + 1) % MAX_WAYPOINTS;
resetPID(); resetPID();

15
Core/Src/setup.cpp
View File

@@ -9,13 +9,22 @@
#include <modelec.h> #include <modelec.h>
#include "commSTM.h" #include "commSTM.h"
DiffBot bot(Point(), 0.01f); DiffBot bot = DiffBot(Point());
void ModelecOdometrySetup() { void ModelecOdometrySetup() {
bot.setup(); 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(); USB_Comm_Process();
bot.update(dt); bot.update(actualDt);
lastTick = currentTick;
} }