Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия Следующая версия Следующая версия справа и слева | ||
stabilization [2018/12/18 10:47] golikov |
stabilization [2019/05/13 16:34] golikov [Пример полного кода программы стабилизации спутника на языке Python:] |
||
---|---|---|---|
Строка 1: | Строка 1: | ||
- | ===== 04 Урок. Стабилизация ===== | + | ===== 04 Урок. Стабилизация спутника===== |
Режим стабилизации спутника означает поддержание нулевой угловой скорости. Такой режим необходим, например для получения четких снимков или их передачи на наземный пункт приема, когда время передачи данных продолжительно и не допустимо отклонение антенны спутника от направления на наземный пункт приема данных. Также описанная в этом уроке теория подходит для режима поддержания любой требуемой угловой скорости, а не только нулевой, для таких задач как отслеживание подвижного объекта. | Режим стабилизации спутника означает поддержание нулевой угловой скорости. Такой режим необходим, например для получения четких снимков или их передачи на наземный пункт приема, когда время передачи данных продолжительно и не допустимо отклонение антенны спутника от направления на наземный пункт приема данных. Также описанная в этом уроке теория подходит для режима поддержания любой требуемой угловой скорости, а не только нулевой, для таких задач как отслеживание подвижного объекта. | ||
Строка 90: | Строка 90: | ||
==== Пример полного кода программы стабилизации спутника на языке Python: ==== | ==== Пример полного кода программы стабилизации спутника на языке Python: ==== | ||
- | <code python> | + | <file python stab.py> |
# Коэффициент дифференциальной обратной связи. | # Коэффициент дифференциальной обратной связи. | ||
# Коэффициент положительный, если маховик расположен осью z вверх | # Коэффициент положительный, если маховик расположен осью z вверх | ||
Строка 108: | Строка 108: | ||
# Номер ДУС (датчика угловой скорости) | # Номер ДУС (датчика угловой скорости) | ||
hyr_num = 1 | hyr_num = 1 | ||
- | |||
# Функции для определение новой скорости маховика. | # Функции для определение новой скорости маховика. | ||
Строка 194: | Строка 193: | ||
switch_off_all() | switch_off_all() | ||
- | </code> | + | </file> |
+ | ==== Пример полного кода программы стабилизации спутника на языке С: ==== | ||
+ | |||
+ | <file с stab.с> | ||
+ | #include "libschsat.h" | ||
+ | const float kd = 200.0; | ||
+ | |||
+ | // Временной шаг работы алгоритма, с | ||
+ | const float time_step = 0.2; | ||
+ | |||
+ | // Целевая угловая скорость спутника, град/с. | ||
+ | // Для режима стабилизации равна 0.0. | ||
+ | const float omega_goal = 0.0; | ||
+ | |||
+ | // Номер маховика | ||
+ | const int mtr_num = 1; | ||
+ | |||
+ | // Максимально допустимая скорость маховика, об/мин | ||
+ | const int mtr_max_speed = 5000; | ||
+ | |||
+ | // Номер ДУС (датчика угловой скорости) | ||
+ | const uint16_t hyr_num = 1; | ||
+ | int16_t mtr_new_speed; | ||
+ | int motor_new_speed_PD(int mtr_speed, int omega, float omega_goal){ | ||
+ | /* Функция для определения новой скорости маховика. | ||
+ | Новая скорость маховика складывается из | ||
+ | текущей скорости маховика и приращения скорости. | ||
+ | Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. | ||
+ | mtr_speed - текущая угловая скорость маховика, об/мин | ||
+ | omega - текущая угловая скорость спутника, град/с | ||
+ | omega_goal - целевая угловая скорость спутника, град/с | ||
+ | mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ | ||
+ | |||
+ | mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal)); | ||
+ | |||
+ | if (mtr_new_speed > mtr_max_speed) | ||
+ | { | ||
+ | mtr_new_speed = mtr_max_speed; | ||
+ | } | ||
+ | else if (mtr_new_speed < -mtr_max_speed) | ||
+ | { | ||
+ | mtr_new_speed = -mtr_max_speed; | ||
+ | } | ||
+ | return mtr_new_speed; | ||
+ | } | ||
+ | |||
+ | void initialize_all(void){ | ||
+ | /*Функция включает все приборы,которые будут использоваться в основной программе.*/ | ||
+ | printf("Enable motor №%d\n", mtr_num); | ||
+ | motor_turn_on(mtr_num); | ||
+ | Sleep(1); | ||
+ | printf("Enable angular velocity sensor №%d\n", hyr_num); | ||
+ | hyro_turn_on(hyr_num); | ||
+ | Sleep(1); | ||
+ | } | ||
+ | |||
+ | void switch_off_all(void){ | ||
+ | /* Функция отключает все приборы,которые будут использоваться в основной программе.*/ | ||
+ | printf("Finishing..."); | ||
+ | int16_t new_speed = mtr_new_speed; // | ||
+ | printf("\nDisable angular velocity sensor №%d\n", hyr_num); | ||
+ | hyro_turn_off(hyr_num); | ||
+ | motor_set_speed(mtr_num, 0, &new_speed); | ||
+ | Sleep (1); | ||
+ | motor_turn_off(mtr_num); | ||
+ | printf("Finish program\n"); | ||
+ | } | ||
+ | |||
+ | void control(void){ | ||
+ | initialize_all(); | ||
+ | // Инициализируем статус маховика | ||
+ | int16_t mtr_state; | ||
+ | // Инициализируем статус ДУС | ||
+ | int16_t hyro_state = 0; | ||
+ | int16_t pRAW_dataX = 0; | ||
+ | int16_t pRAW_dataY = 0; | ||
+ | int16_t pRAW_dataZ = 0; | ||
+ | int16_t *gx_raw = &pRAW_dataX; | ||
+ | int16_t *gy_raw = &pRAW_dataY; | ||
+ | int16_t *gz_raw = &pRAW_dataZ; | ||
+ | int16_t speed = 0; | ||
+ | int16_t *mtr_speed = &speed; | ||
+ | int16_t omega; | ||
+ | int i; | ||
+ | for(i = 0; i < 1000; i++){ | ||
+ | printf("i = %d\n", i); | ||
+ | // Опрос датчика угловой скорости и маховика. | ||
+ | hyro_state = hyro_request_raw(hyr_num, gx_raw, gy_raw, gz_raw); | ||
+ | mtr_state = motor_request_speed(mtr_num, mtr_speed); | ||
+ | |||
+ | // Обработка показаний датчика угловой скорости, | ||
+ | // вычисление угловой скорости спутника по показаниям ДУС. | ||
+ | // Если код ошибки ДУС равен 0, т.е. ошибки нет | ||
+ | if (!hyro_state){ | ||
+ | float gx_degs = *gx_raw * 0.00875; | ||
+ | float gy_degs = *gy_raw * 0.00875; | ||
+ | float gz_degs = *gz_raw * 0.00875; | ||
+ | // если ДУС установлен осью z вверх, то угловая скорость | ||
+ | // спутника совпадает с показаниями ДУС по оси z, иначе | ||
+ | // необходимо изменить знак: omega = - gz_degs | ||
+ | omega = gz_degs; | ||
+ | printf("gx_degs = %f ", gx_degs); | ||
+ | printf(" gy_degs = %f ", gy_degs); | ||
+ | printf(" gz_degs = %f\n", gz_degs); | ||
+ | } | ||
+ | else if (hyro_state == 1){ | ||
+ | printf("Fail because of access error, check the connection\n"); | ||
+ | } | ||
+ | else if (hyro_state == 2){ | ||
+ | printf("Fail because of interface error, check your code\n"); | ||
+ | } | ||
+ | |||
+ | int mtr_new_speed; | ||
+ | //Обработка показаний маховика и установка требуемой угловой скорости. | ||
+ | if (!mtr_state) { | ||
+ | // если код ошибки 0, т.е. ошибки нет | ||
+ | int16_t mtr_speed=0; | ||
+ | motor_request_speed(mtr_num, &mtr_speed); | ||
+ | printf("Motor_speed: %d\n", mtr_speed); | ||
+ | // установка новой скорости маховика | ||
+ | mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal); | ||
+ | motor_set_speed(mtr_num, mtr_new_speed, &omega); | ||
+ | } | ||
+ | } | ||
+ | Sleep(time_step); | ||
+ | switch_off_all(); | ||
+ | } | ||
+ | </file> | ||
===== Задание для самостоятельной работы ===== | ===== Задание для самостоятельной работы ===== | ||