Инструменты пользователя

Инструменты сайта


stabilization

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

Предыдущая версия справа и слева Предыдущая версия
Следующая версия
Предыдущая версия
stabilization [2018/12/18 10:47]
golikov
stabilization [2020/03/25 16:28] (текущий)
Строка 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 c stab.c>
 +
 +#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>​
 ===== Задание для самостоятельной работы ===== ===== Задание для самостоятельной работы =====
  
stabilization.1545119265.txt.gz · Последние изменения: 2020/03/25 16:29 (внешнее изменение)

Инструменты страницы