Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия | Следующая версия Следующая версия справа и слева | ||
stabilization_pid [2020/02/13 15:49] golikov |
stabilization_pid [2020/02/14 16:20] golikov |
||
---|---|---|---|
Строка 51: | Строка 51: | ||
hyr_num = 1 | hyr_num = 1 | ||
- | error = 0 # Ошибка | + | error = 0 # Ошибка |
- | P = 0 # Воздействие пропорционального звена | + | P = 0 # Воздействие пропорционального звена |
- | D = 0 # Воздействие дифференциального звена | + | D = 0 # Воздействие дифференциального звена |
- | I = 0 # Воздействие нтегрального звена | + | I = 0 # Воздействие нтегрального звена |
- | Kp = 100 # Пропорциональный коэффициент | + | Kp = 100 # Пропорциональный коэффициент |
- | Kd = 0.02 # Дифференциальный коэффициент | + | Kd = 0.02 # Дифференциальный коэффициент |
- | Ki = 0.9 # Интегральный коэффициент | + | Ki = 0.9 # Интегральный коэффициент |
- | lastError = 0 # Прошлая ошибка | + | lastError = 0 # Прошлая ошибка |
- | Integrator = 0 # Интеграл (сумма всех ошибок) | + | Integrator = 0 # Интеграл (сумма всех ошибок) |
- | PID = 0 # Величина управляющего воздействия | + | PID = 0 # Величина управляющего воздействия |
- | Integrator_max = 10 # | + | Integrator_max = 10 # |
- | Integrator_min = -10 # Ограничение минимального | + | Integrator_min = -10 # Ограничение минимального |
# Функции для определение новой скорости маховика. | # Функции для определение новой скорости маховика. | ||
Строка 78: | Строка 78: | ||
error = omega - omega_goal # Вычисление ошибки | error = omega - omega_goal # Вычисление ошибки | ||
P = Kp * error # Вычисление воздействия пропорционального звена | P = Kp * error # Вычисление воздействия пропорционального звена | ||
- | D = Kd * ( error - lastError) / time_step # Вычисление воздействия дифференциального звена | + | D = Kd * ( error - lastError) / time_step # Вычисление воздействия дифференциального звена |
lastError = error # Запоминаем ошибку | lastError = error # Запоминаем ошибку | ||
- | Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку | + | Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку |
if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | ||
Integrator = Integrator_max | Integrator = Integrator_max | ||
Строка 87: | Строка 87: | ||
I = Integrator * Ki # Вычисление воздействия интегрального звена | I = Integrator * Ki # Вычисление воздействия интегрального звена | ||
- | PID = P + I + D # Вычисление суммарного управляющего воздействия | + | PID = P + I + D # Вычисление суммарного управляющего воздействия |
mtr_new_speed = int(mtr_speed + PID) | mtr_new_speed = int(mtr_speed + PID) |