Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия | ||
stabilization_pid [2020/03/10 13:31] golikov |
stabilization_pid [2020/03/10 13:56] golikov |
||
---|---|---|---|
Строка 36: | Строка 36: | ||
ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора: Wпид(s) = K1 + K0 / s + K2 s. | ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора: Wпид(s) = K1 + K0 / s + K2 s. | ||
+ | ====Код программы стабилизации на языке Python==== | ||
<code python stabilization_PID.py> | <code python stabilization_PID.py> | ||
- | # Временной шаг работы алгоритма, с | + | |
- | time_step = 0.1 | + | time_step = 0.1 # Временной шаг работы алгоритма, с |
- | # Целевая угловая скорость спутника, град/с. | + | omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. |
- | # Для режима стабилизации равна 0.0. | + | mtr_num = 1 # Номер маховика |
- | omega_goal = 0.0 | + | mtr_max_speed = 3000 # Максимально допустимая скорость маховика, об/мин |
- | # Номер маховика | + | hyr_num = 1 # Номер ДУС (датчика угловой скорости) |
- | mtr_num = 1 | + | |
- | # Максимально допустимая скорость маховика, об/мин | + | |
- | mtr_max_speed = 3000 | + | |
- | # Номер ДУС (датчика угловой скорости) | + | |
- | hyr_num = 1 | + | |
error = 0 # Ошибка | error = 0 # Ошибка | ||
Строка 61: | Строка 57: | ||
Integrator = 0 # Интеграл (сумма всех ошибок) | Integrator = 0 # Интеграл (сумма всех ошибок) | ||
PID = 0 # Величина управляющего воздействия | PID = 0 # Величина управляющего воздействия | ||
- | Integrator_max = 10 # | + | Integrator_max = 10 # Ограничение максимального значения интеграла |
- | Integrator_min = -10 # Ограничение минимального | + | Integrator_min = -10 # Ограничение минимального значения интеграла |
# Функции для определение новой скорости маховика. | # Функции для определение новой скорости маховика. | ||
- | # Новая скорость маховика складывается из | ||
- | # текущей скорости маховика и приращения скорости. | ||
- | # Приращение скорости пропорционально ошибке по углу | ||
- | # и ошибке по угловой скорости. | ||
# mtr_speed - текущая угловая скорость маховика, об/мин | # mtr_speed - текущая угловая скорость маховика, об/мин | ||
# omega - текущая угловая скорость спутника, град/с | # omega - текущая угловая скорость спутника, град/с | ||
Строка 97: | Строка 89: | ||
return mtr_new_speed | return mtr_new_speed | ||
- | # Функция включает все приборы, | + | # Функция включает все приборы, которые будут использоваться в основной программе. |
- | # которые будут использоваться в основной программе. | + | |
def initialize_all(): | def initialize_all(): | ||
print "Enable motor №", mtr_num | print "Enable motor №", mtr_num | ||
Строка 120: | Строка 111: | ||
# Основная функция программы, в которой вызываются остальные функции. | # Основная функция программы, в которой вызываются остальные функции. | ||
def control(): | def control(): | ||
- | initialize_all() | + | initialize_all() # Инициализируем все устройства |
- | # Инициализируем статус маховика | + | mtr_state = 0 # Инициализируем статус маховика |
- | mtr_state = 0 | + | hyro_state = 0 # Инициализируем статус ДУС |
- | # Инициализируем статус ДУС | + | omega_goal = 0.0 # Целевой угол поворота |
- | hyro_state = 0 | + | |
- | omega_goal = 0.0 | + | |
for i in range(100): | for i in range(100): | ||
Строка 163: | Строка 152: | ||
</code> | </code> | ||
- | <code с stabilization_PID.с> | + | ====Код программы стабилизации на языке Си==== |
+ | |||
+ | <code C stabilization_PID.с> | ||
#include <stdio.h> | #include <stdio.h> | ||
#include <stdint.h> | #include <stdint.h> | ||
Строка 173: | Строка 164: | ||
const float time_step = 100; // Временной шаг работы алгоритма, мс | const float time_step = 100; // Временной шаг работы алгоритма, мс | ||
- | //const float omega_goal = 0.0; // Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. | ||
- | //const float alpha_goal = 0; // Целевой угол поворота | ||
const int16_t mtr_max_speed = 3000; // Максимально допустимая скорость маховика, об/мин | const int16_t mtr_max_speed = 3000; // Максимально допустимая скорость маховика, об/мин | ||
- | const uint16_t mtr_num = 1; // Номер маховика | + | const uint16_t mtr_num = 1; // Номер маховика |
- | const uint16_t hyr_num = 1; // Номер ДУС | + | const uint16_t hyr_num = 1; // Номер ДУС |
- | const uint16_t mag_num = 1; // Номер магнитометра | + | const uint16_t mag_num = 1; // Номер магнитометра |
- | //float mag_alpha = 0; | + | |
- | //float omega = 0; | + | |
int16_t confirm = 0; //Переменная для функции motor_set_speed | int16_t confirm = 0; //Переменная для функции motor_set_speed | ||
float errora = 0; // Ошибка по углу | float errora = 0; // Ошибка по углу | ||
- | float Pa = 0; // Воздействие пропорционального звена | + | float Pa = 0; // Воздействие пропорционального звена |
- | float Da = 0; // Воздействие дифференциального звена | + | float Da = 0; // Воздействие дифференциального звена |
- | float Ia = 0; // Воздействие интегрального звена | + | float Ia = 0; // Воздействие интегрального звена |
- | float Kpa = 20; // Пропорциональный коэфициент ошибки по углу | + | float Kpa = 20; // Пропорциональный коэффициент ошибки по углу |
float Kda = 0.1; // Дифференциальный коэффициент | float Kda = 0.1; // Дифференциальный коэффициент | ||
float Kia = 1; // Интегральный коэффициент | float Kia = 1; // Интегральный коэффициент | ||
Строка 193: | Строка 180: | ||
float Integratora = 0; // Интеграл (сумма всех ошибок по углу) | float Integratora = 0; // Интеграл (сумма всех ошибок по углу) | ||
float PID_alpha = 0; // Величина управляющего воздействия | float PID_alpha = 0; // Величина управляющего воздействия | ||
- | float Integrator_maxa = 10; // Ограничение максимального значения интергатора | + | float Integrator_maxa = 10; // Ограничение максимального значения интеграла |
- | float Integrator_mina = -10; // Ограничение минимального значения интергатора | + | float Integrator_mina = -10; // Ограничение минимального значения интеграла |
- | float error = 0; // Ошибка по угловой скорости | + | float error = 0; // Ошибка по угловой скорости |
- | float P = 0; // Воздействие пропорционального звена | + | float P = 0; // Воздействие пропорционального звена |
- | float D = 0; // Воздействие дифференциального звена | + | float D = 0; // Воздействие дифференциального звена |
- | float I = 0; // Воздействие интегрального звена | + | float I = 0; // Воздействие интегрального звена |
- | float Kp = 20; // Пропорциональный коэффициент | + | float Kp = 20; // Пропорциональный коэффициент |
- | float Kd = 0.1; // Дифференциальный коэффициент | + | float Kd = 0.1; // Дифференциальный коэффициент |
- | float Ki = 1; // Интегральный коэффициент | + | float Ki = 1; // Интегральный коэффициент |
- | float lastError = 0; // Прошлая ошибка по угловой скорости | + | float lastError = 0; // Прошлая ошибка по угловой скорости |
- | float Integrator = 0; // Интеграл (сумма всех ошибок по угловой скорости) | + | float Integrator = 0; // Интеграл (сумма всех ошибок по угловой скорости) |
- | float PID_omega = 0; // Величина управляющего воздействия | + | float PID_omega = 0; // Величина управляющего воздействия |
- | float Integrator_max = 10; // Ограничение максимального значения интергатора | + | float Integrator_max = 10; // Ограничение максимального значения интеграла |
- | float Integrator_min = -10; // Ограничение минимального значения интергатора | + | float Integrator_min = -10; // Ограничение минимального значения интеграла |
int16_t mtr_new_speed; | int16_t mtr_new_speed; | ||
Строка 213: | Строка 200: | ||
int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ | int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ | ||
//Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | //Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | ||
- | float magx_cal; // коэффициенты для орбикрафт 1 собраны у моего стола для SCHSAT_SX01 | + | float magx_cal; |
magx_cal = 1.668867*(*magx + -38.326818) + -0.007649*(*magy + 20.442007) + 0.113348*(*magz + -69.659602); | magx_cal = 1.668867*(*magx + -38.326818) + -0.007649*(*magy + 20.442007) + 0.113348*(*magz + -69.659602); | ||
float magy_cal; | float magy_cal; | ||
Строка 226: | Строка 213: | ||
int motor_new_speed_PD(int mtr_speed, int alpha, int alpha_goal, int omega, int omega_goal){ | int motor_new_speed_PD(int mtr_speed, int alpha, int alpha_goal, int omega, int omega_goal){ | ||
- | /* Функция для определения новой скорости маховика. | + | /* Функция для определения новой скорости маховика. |
- | Новая скорость маховика складывается из | + | |
- | текущей скорости маховика и приращения скорости. | + | |
- | Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. | + | |
mtr_speed - текущая угловая скорость маховика, об/мин | mtr_speed - текущая угловая скорость маховика, об/мин | ||
omega - текущая угловая скорость спутника, град/с | omega - текущая угловая скорость спутника, град/с | ||
omega_goal - целевая угловая скорость спутника, град/с | omega_goal - целевая угловая скорость спутника, град/с | ||
mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ | mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ | ||
- | + | ||
- | //mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal)); | + | error = omega - omega_goal; // Вычисление ошибки |
- | + | P = Kp * error; // Вычисление воздействия пропорционального звена | |
- | error = omega - omega_goal; // Вычисление ошибки | + | D = Kd * (error - lastError) / time_step; // Вычисление воздействия дифференциального звена |
- | P = Kp * error; // Вычисление воздействия пропорционального звена | + | lastError = error; // Запоминаем ошибку |
- | D = Kd * (error - lastError) / time_step; // Вычисление воздействия дифференциального звена | + | |
- | 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; | ||
} | } | ||
Строка 248: | Строка 230: | ||
Integrator = Integrator_min; | Integrator = Integrator_min; | ||
} | } | ||
- | I = Integrator * Ki; // Вычисление воздействия интегрального звена | + | I = Integrator * Ki; // Вычисление воздействия интегрального звена |
- | PID_omega = P + I + D; // Вычисление суммарного управляющего воздействия | + | PID_omega = P + I + D; // Вычисление суммарного управляющего воздействия |
- | errora = alpha - alpha_goal; // Вычисление ошибки | + | errora = alpha - alpha_goal; // Вычисление ошибки |
- | Pa = Kpa * errora; // Вычисление воздействия пропорционального звена | + | Pa = Kpa * errora; // Вычисление воздействия пропорционального звена |
Da = Kda * (errora - lastErrora) / time_step; // Вычисление воздействия дифференциального звена | Da = Kda * (errora - lastErrora) / time_step; // Вычисление воздействия дифференциального звена | ||
- | lastErrora = errora; // Запоминаем ошибку | + | lastErrora = errora; // Запоминаем ошибку |
- | Integratora = Integratora + errora * time_step; // Накапливаем суммарную ошибку | + | Integratora = Integratora + errora * time_step; // Накапливаем суммарную ошибку |
if (Integratora > Integrator_maxa){ // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | if (Integratora > Integrator_maxa){ // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | ||
Integratora = Integrator_maxa; | Integratora = Integrator_maxa; | ||
Строка 262: | Строка 244: | ||
Integratora = Integrator_mina; | Integratora = Integrator_mina; | ||
} | } | ||
- | Ia = Integratora * Kia; // Вычисление воздействия интегрального звена | + | Ia = Integratora * Kia; // Вычисление воздействия интегрального звена |
- | PID_alpha = Pa + Ia + Da; // Вычисление суммарного управляющего воздействия | + | PID_alpha = Pa + Ia + Da; // Вычисление суммарного управляющего воздействия |
mtr_new_speed = (int16_t)(mtr_speed + PID_omega + PID_alpha); | mtr_new_speed = (int16_t)(mtr_speed + PID_omega + PID_alpha); | ||
Строка 289: | Строка 271: | ||
void initialize_all(void){ | void initialize_all(void){ | ||
- | /* Функция инициализации всех систем, | + | /* Функция инициализации всех систем, |
включает все приборы,которые будут использоваться в основной программе.*/ | включает все приборы,которые будут использоваться в основной программе.*/ | ||
printf ("Enable motor №%d\n", mtr_num); | printf ("Enable motor №%d\n", mtr_num); | ||
Строка 303: | Строка 285: | ||
int switch_off_all(){ | int switch_off_all(){ | ||
- | /*Функция отключает все приборы,которые будут использоваться в основной программе.*/ | + | //Функция отключает все приборы,которые будут использоваться в основной программе. |
printf("Finishing...\n"); | printf("Finishing...\n"); | ||
printf("Disable angular velocity sensor №%d\n", hyr_num); | printf("Disable angular velocity sensor №%d\n", hyr_num); | ||
Строка 317: | Строка 299: | ||
int control(){ | int control(){ | ||
- | float mtr_new_speed; | ||
//данные магнитометра | //данные магнитометра | ||
int16_t mgx_cal=0; | int16_t mgx_cal=0; | ||
Строка 338: | Строка 319: | ||
initialize_all(); | initialize_all(); | ||
- | int mtr_state = 0; // Инициализируем статус маховика | + | int mtr_state = 0; // Инициализируем статус маховика |
int hyro_state = 0; // Инициализируем статус ДУС | int hyro_state = 0; // Инициализируем статус ДУС | ||
- | int mag_state = 0; // Инициализируем статус магнитометра | + | int mag_state = 0; // Инициализируем статус магнитометра |
int alpha_goal = 90; // Целевой угол | int alpha_goal = 90; // Целевой угол | ||
double mag_alpha = 0; | double mag_alpha = 0; |