mirror of
https://github.com/modelec/odo_STM32.git
synced 2026-03-18 21:30:38 +01:00
48 lines
843 B
C++
48 lines
843 B
C++
/*
|
|
* setup.cpp
|
|
*
|
|
* Created on: Sep 18, 2025
|
|
* Author: Modelec
|
|
*/
|
|
|
|
#include <setup.h>
|
|
#include <modelec.h>
|
|
#include "commSTM.h"
|
|
|
|
extern TIM_HandleTypeDef htim6;
|
|
|
|
DiffBot bot = DiffBot(Point());
|
|
|
|
uint32_t lastUs = 0;
|
|
|
|
void ModelecOdometrySetup() {
|
|
HAL_TIM_Base_Start(&htim6);
|
|
|
|
lastUs = __HAL_TIM_GET_COUNTER(&htim6);
|
|
|
|
bot.setup();
|
|
}
|
|
|
|
void ModelecOdometryLoop() {
|
|
uint32_t currentUs = __HAL_TIM_GET_COUNTER(&htim6);
|
|
uint32_t diffUs;
|
|
|
|
if (currentUs >= lastUs) {
|
|
diffUs = currentUs - lastUs;
|
|
} else {
|
|
diffUs = (65535 - lastUs) + currentUs + 1;
|
|
}
|
|
|
|
float actualDt = diffUs / 1000000.0f;
|
|
|
|
if (actualDt <= 0.0001f || actualDt > 0.1f) {
|
|
if (actualDt > 0.1f) lastUs = currentUs;
|
|
return;
|
|
}
|
|
|
|
USB_Comm_Process();
|
|
bot.update(actualDt);
|
|
|
|
lastUs = currentUs;
|
|
}
|