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

PID

Browse Source
This commit is contained in:
acki
2026-03-12 16:56:09 +01:00
parent e47964b8af
commit 727b422166
2 changed files with 23 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
Core/Inc/modelec.h
View File

@@ -15,14 +15,15 @@
#include "pid.h"
#include "point.h"
#define PWM_MAX 640.0f
#define V_MAX 0.643f // m/s
#define MAX_WAYPOINTS 16
#define PWM_MAX 626.0f
#define ENCODER_RES 2400.0f
#define WHEEL_DIAMETER 0.082f
#define WHEEL_DIAMETER 0.081f
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
#define WHEEL_BASE 0.29f
#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
#define V_MAX 0.643f // m/s
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim2;
@@ -52,7 +53,7 @@ public:
bool arrive = false;
uint32_t publishNotMoved = 0;
uint32_t notMovedMaxTime = 100;
uint32_t notMovedMaxTime = 300;
bool no_move = false;
uint8_t action = 0;
@@ -61,11 +62,14 @@ public:
uint32_t publishLastTick = 0;
uint32_t frequencyPublish = 100;
float preciseAngleFinal = 0.017f;
float preciseAngleFinal = 0.005f;
float preciseAngle = 0.39f;
float precisePosFinal = 0.001f;
float precisePos2 = 0.02f;
float precisePos = 0.1f;
float precisePos = 0.15f;
float currentV = 0;
const float maxAccel = 0.5f;
static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick);

24
Core/Src/modelec.cpp
View File

@@ -60,10 +60,10 @@ void DiffBot::stop(bool stop)
void DiffBot::setup()
{
pidLeft = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
pidRight = PID(4.f, 0.1f, 0.f, -PWM_MAX, PWM_MAX);
pidPos = PID(6.0f, 0.0f, 0.1f, -V_MAX, V_MAX);
pidTheta = PID(10.0f, 0.0f, 0.1f, -M_PI, M_PI);
pidLeft = PID(5.f, 0.5f, 0.5f, -PWM_MAX, PWM_MAX);
pidRight = PID(5.f, 0.5f, 0.5f, -PWM_MAX, PWM_MAX);
pidPos = PID(4.0f, 0.0f, 0.0f, -V_MAX, V_MAX);
pidTheta = PID(5.0f, 1.0f, 0.0f, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -73,12 +73,6 @@ void DiffBot::update(float dt_actual)
{
this->dt = dt_actual;
if (!isDelayPassed(dt * 1000))
{
motor.update();
return;
}
float rightVel = readEncoderRight();
float leftVel = readEncoderLeft();
@@ -92,6 +86,7 @@ void DiffBot::update(float dt_actual)
pos.y += dDistance * sinf(avgTheta);
pos.theta = normalizeAngle(pos.theta + dTheta);
// TODO : review this part cause do not work.
if (std::abs(rightVel) == 0.0f && std::abs(leftVel) == 0.0f &&
(std::abs(motor.rightTarget_PWM) > 0 || std::abs(motor.leftTarget_PWM) > 0))
{
@@ -186,7 +181,13 @@ void DiffBot::update(float dt_actual)
float alignmentScale = cosf(angleError);
if (alignmentScale < 0) alignmentScale = 0;
vRef = pidPos.compute(0, -dist * direction, dt) * alignmentScale;
float targetV = pidPos.compute(0, -dist * direction, dt) * alignmentScale;
if (targetV > currentV + (maxAccel * dt)) currentV += maxAccel * dt;
else if (targetV < currentV - (maxAccel * dt)) currentV -= maxAccel * dt;
else currentV = targetV;
vRef = currentV;
}
float vLeftReq = vRef - (WHEEL_BASE_2 * wRef);
@@ -224,6 +225,7 @@ void DiffBot::addTarget(int id, int type, float x, float y, float theta)
if (id <= index) index = 0;
arrive = false;
no_move = false;
}
void DiffBot::resetPID()