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

rewrite thing due to usage of tim6

Browse Source
This commit is contained in:
acki
2026-03-17 22:25:33 +01:00
parent bef9d4d723
commit a6520682bb
2 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
Core/Src/modelec.cpp
View File

@@ -21,11 +21,6 @@ bool DiffBot::isDelayPassedFrom(uint32_t delay, uint32_t& lastTick)
return false;
}
bool DiffBot::isDelayPassed(uint32_t delay)
{
return isDelayPassedFrom(delay, lastTick);
}
float DiffBot::readEncoderLeft()
{
int16_t count = (int16_t)__HAL_TIM_GET_COUNTER(&htim2);
@@ -60,10 +55,12 @@ void DiffBot::stop(bool stop)
void DiffBot::setup()
{
pidLeft = PID(7.0f, 0.0f, 0.0f, -PWM_MAX, PWM_MAX);
pidRight = PID(7.0f, 0.0f, 0.0f, -PWM_MAX, PWM_MAX);
pidPos = PID(6.0f, 0.2f, 0.02f, -V_MAX, V_MAX);
pidTheta = PID(15.0f, 0.2f, 0.02f, -M_PI, M_PI);
pidLeft = PID(45.0f, 2.0f, 0.5f, -PWM_MAX, PWM_MAX);
pidRight = PID(45.0f, 2.0f, 0.5f, -PWM_MAX, PWM_MAX);
pidPos = PID(40.0f, 0.5f, 0.0f, -V_MAX, V_MAX);
pidTheta = PID(120.0f, 0.0f, 0.1f, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -165,7 +162,7 @@ void DiffBot::update(float dt_actual)
notifyWaypointReached(index);
return;
}
wRef = pidTheta.compute(0, -angleErr, dt); // Pivot in place
wRef = pidTheta.compute(0, -angleErr, dt);
} else {
notifyWaypointReached(index);
return;
@@ -184,7 +181,9 @@ void DiffBot::update(float dt_actual)
float alignScale = std::max(0.0f, cosf(angleError));
float targetV = pidPos.compute(0, -dist * direction, dt) * alignScale;
float maxStep = maxAccel * dt;
float accelLimit = (std::abs(targetV) < std::abs(currentV)) ? maxAccel * 2.0f : maxAccel;
float maxStep = accelLimit * dt;
currentV = std::clamp(targetV, currentV - maxStep, currentV + maxStep);
vRef = currentV;
}
@@ -199,11 +198,11 @@ void DiffBot::update(float dt_actual)
float fb = pid.compute(vTarget, vActual, dt);
float total = ff + fb;
const float deadzone = 35.0f;
const float deadzone = 40.0f;
if (std::abs(total) > 1.0f) {
total += (total > 0) ? deadzone : -deadzone;
}
return std::clamp(total, -PWM_MAX, PWM_MAX);
return std::clamp(total, (float)-PWM_MAX, (float)PWM_MAX);
};
motor.leftTarget_PWM = (int16_t)computeMotorPWM(vLeftReq, vLeftAct, pidLeft);

3
Core/Src/setup.cpp
View File

@@ -8,7 +8,8 @@
#include <setup.h>
#include <modelec.h>
#include "commSTM.h"
#include "tim.h"
extern TIM_HandleTypeDef htim6;
DiffBot bot = DiffBot(Point());