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-16 19:40:34 +01:00
parent db231d2ae9
commit 8df8c64b0d
2 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Core/Src/modelec.cpp
View File

@@ -60,10 +60,10 @@ void DiffBot::stop(bool stop)
void DiffBot::setup()
{
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);
pidLeft = PID(4.5f, 2.0f, 2.0f, -PWM_MAX, PWM_MAX);
pidRight = PID(4.5f, 2.0f, 2.0f, -PWM_MAX, PWM_MAX);
pidPos = PID(3.0f, 0.0f, 0.0f, -V_MAX, V_MAX);
pidTheta = PID(13.0f, 0.0f, 0.0f, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -74,7 +74,7 @@ void DiffBot::handleStallCondition()
motor.stop(true);
char log[64];
int len = snprintf(log, sizeof(log), "ERR;STALL;WAYPOINT;%d;POS;%.2f;%.2f\n",
int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d;POS;%.2f;%.2f\n",
index, pos.x, pos.y);
CDC_Transmit_FS((uint8_t*)log, len);
@@ -139,6 +139,10 @@ void DiffBot::update(float dt_actual)
no_move = false;
}
if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) {
publishStatus();
}
if (!odo_active || !targets[index].active) {
motor.update();
return;
@@ -208,10 +212,6 @@ void DiffBot::update(float dt_actual)
motor.rightTarget_PWM = (int16_t)computeMotorPWM(vRightReq, vRightAct, pidRight);
motor.update();
if (isDelayPassedFrom(frequencyPublish, publishLastTick)) {
publishStatus();
}
}
void DiffBot::addTarget(int id, int type, float x, float y, float theta)

4
Core/Src/motors.cpp
View File

@@ -16,8 +16,8 @@ void Motor::update() {
int16_t max_step = 10;
leftTarget_PWM = std::max(-PWM_MAX, std::min(leftTarget_PWM, PWM_MAX));
rightTarget_PWM = std::max(-PWM_MAX, std::min(rightTarget_PWM, PWM_MAX));
leftTarget_PWM = std::max(-PWM_MAX, std::min((float)leftTarget_PWM, PWM_MAX));
rightTarget_PWM = std::max(-PWM_MAX, std::min((float)rightTarget_PWM, PWM_MAX));
leftCurrent_PWM = approach(leftCurrent_PWM, leftTarget_PWM, max_step);
rightCurrent_PWM = approach(rightCurrent_PWM, rightTarget_PWM, max_step);