Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Создайте следующую программу. Коэффициенты для функции def mag_calibrated возьмите из программы Magneto.
Код на Python.
import math time_step = 0.1 # Временной шаг работы алгоритма, с omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. alpha_goal = 0 # Целевой угол поворота mtr_max_speed = 5000 # Максимально допустимая скорость маховика, об/мин mtr_num = 1 # Номер маховика hyr_num = 1 # Номер ДУС mag_num = 1 # Номер магнитометра errora = 0 # Ошибка по углу Pa = 0 # Воздействие пропорционального звена Da = 0 # Воздействие дифференциального звена Ia = 0 # Воздействие интегрального звена Kpa = 200 # Пропорциональный коэфициент ошибки по углу Kda = 0.02 # Дифференциальный коэффициент Kia = 0.5 # Интегральный коэффициент lastErrora = 0 # Прошлая ошибка по углу Integratora = 0 # Интеграл (сумма всех ошибок по углу) PID_alpha = 0 # Величина управляющего воздействия Integrator_maxa = 10 # Ограничение максимального значения интергатора Integrator_mina = -10 # Ограничение минимального значения интергатора error = 0 # Ошибка по угловой скорости P = 0 # Воздействие пропорционального звена D = 0 # Воздействие дифференциального звена I = 0 # Воздействие интегрального звена Kp = 200 # Пропорциональный коэффициент Kd = 0.02 # Дифференциальный коэффициент Ki = 0.5 # Интегральный коэффициент lastError = 0 # Прошлая ошибка по угловой скорости Integrator = 0 # Интеграл (сумма всех ошибок по угловой скорости) PID_omega = 0 # Величина управляющего воздействия Integrator_max = 10 # Ограничение максимального значения интергатора Integrator_min = -10 # Ограничение минимального значения интергатора # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов def mag_calibrated(magx,magy,magz): magx_cal = 1.451626*(magx + 47.687257) + 0.036309*(magy + -1468.312497) + 0.008968*(magz + 48.449050) magy_cal = 0.036309*(magx + 47.687257) + 0.408031*(magy + -1468.312497) + 0.074774*(magz + 48.449050) magz_cal = 0.008968*(magx + 47.687257) + 0.074774*(magy + -1468.312497) + 1.429262*(magz + 48.449050) return magx_cal, magy_cal, magz_cal # Функции для определение новой скорости маховика. # Новая скорость маховика складывается из # текущей скорости маховика mtr_speed и приращения скорости PID # Приращение скорости получено путем сложения трех коэффициентов # P - пропорциональный коэффициент регулятора # I - интегральный коэффициент регулятора # D - дифференциальный коэффициент регулятора # Integrator накапливает суммарную ошибку # Сатуратор ограничивает максимальную величину суммарной ошибки # mtr_speed - текущая угловая скорость маховика, об/мин # omega - текущая угловая скорость спутника, град/с # omega_goal - целевая угловая скорость спутника, град/с # mtr_new_speed - требуемая угловая скорость маховика, об/мин def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal): global Integrator global lastError global Integratora global lastErrora error = omega - omega_goal # Вычисление ошибки P = Kp * error # Вычисление воздействия пропорционального звена D = Kd * (error - lastError) / time_step # Вычисление воздействия дифференциального звена lastError = error # Запоминаем ошибку Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) Integrator = Integrator_max elif Integrator < Integrator_min: Integrator = Integrator_min I = Integrator * Ki # Вычисление воздействия интегрального звена PID_omega = P + I + D # Вычисление суммарного управляющего воздействия errora = alpha - alpha_goal # Вычисление ошибки Pa = Kpa * errora # Вычисление воздействия пропорционального звена Da = Kda * (errora - lastErrora) / time_step # Вычисление воздействия дифференциального звена lastErrora = errora # Запоминаем ошибку Integratora = Integratora + errora * time_step # Накапливаем суммарную ошибку if Integratora > Integrator_maxa: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) Integratora = Integrator_maxa elif Integratora < Integrator_mina: Integratora = Integrator_mina Ia = Integratora * Kia # Вычисление воздействия интегрального звена PID_alpha = Pa + Ia + Da # Вычисление суммарного управляющего воздействия mtr_new_speed = int(mtr_speed + PID_omega + PID_alpha) if mtr_new_speed > mtr_max_speed: mtr_new_speed = mtr_max_speed elif mtr_new_speed < -mtr_max_speed: mtr_new_speed = -mtr_max_speed print "P= ", P, "I= ", I, "D= ", D, "PID_omega= ", PID_omega, "PID_alpha=", PID_alpha, "mtr_new_speed= ", mtr_new_speed print "Pa= ", Pa, "Ia= ", Ia, "Da= ", Da, "PID_alpha=", PID_alpha return mtr_new_speed # Функция включает все приборы, # которые будут использоваться в основной программе. def initialize_all(): # Функция инициализации всех систем print "Enable motor №", mtr_num motor_turn_on(mtr_num) sleep(1) print "Enable angular velocity sensor №", hyr_num hyro_turn_on(hyr_num) # Включаем ДУС sleep(1) print "Enable magnetometer", mag_num magnetometer_turn_on(mag_num) sleep(1) def switch_off_all(): # Функция выключения всех систем print "Finishing..." print "Disable angular velocity sensor №", hyr_num hyro_turn_off(hyr_num) # Выключаем ДУС print "Disable magnetometer", mag_num magnetometer_turn_off(mag_num) motor_set_speed(mtr_num, 0) sleep (1) motor_turn_off(mtr_num) print "Finish program" # Основная функция программы, в которой вызываются остальные функции. def control(): initialize_all() mtr_state = 0 # Инициализируем статус маховика hyro_state = 0 # Инициализируем статус ДУС mag_state = 0 # Инициализируем статус магнитометра alpha_goal = 90 # Целевой угол omega_goal = 0 # Целевая угловая скорость mag_alpha = 0 for i in range(200): #print "i = ", i # Опрос датчика угловой скорости и маховика. mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw) magy_cal = - magy_cal # переходим из левой системы координат, которая изображена на магнитометре в правую mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180 #mag_alpha = angle_transformation(mag_alpha, alpha_goal) #print "magx_cal =", magx_cal, "magy_cal =", magy_cal, "magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра print "mag_alpha atan2= ", mag_alpha elif mag_state == 1: print "Fail because of access error, check the connection" elif mag_state == 2: print "Fail because of interface error, check your code" # Обработка показаний датчика угловой скорости, # вычисление угловой скорости спутника по показаниям ДУС. # Если код ошибки ДУС равен 0, т.е. ошибки нет if not hyro_state: gx_degs = gx_raw * 0.00875 gy_degs = gy_raw * 0.00875 gz_degs = gz_raw * 0.00875 # если ДУС установлен осью z вверх, то угловая скорость # спутника совпадает с показаниями ДУС по оси z, иначе # необходимо изменить знак: omega = - gz_degs omega = gz_degs #print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs elif hyro_state == 1: print "Fail because of access error, check the connection" elif hyro_state == 2: print "Fail because of interface error, check your code" #Обработка показаний маховика и установка требуемой угловой скорости. if not mtr_state: # если код ошибки 0, т.е. ошибки нет print "Motor_speed: ", mtr_speed # установка новой скорости маховика mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal) motor_set_speed(mtr_num, mtr_new_speed) sleep(time_step) switch_off_all()
Код на С.
#include <stdio.h> #include <stdint.h> #include "libschsat.h" #define LSS_OK 0 #define LSS_ERROR 1 #define LSS_BREAK 2 #include <math.h> const int kd = 200; /* К-т дифференциальной обратной связи. Если угловая скорость спутника положительна, то спутник надо раскручивать по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке.*/ const int kp = -50; /* К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки.*/ const float time_step = 0.05; // Временной шаг работы const uint16_t mtr_num = 1; // Номер маховика const uint16_t hyr_num = 1; // Номер ДУС const uint16_t mag_num = 1; // Номер магнитометра // Максимально допустимая скорость маховика, об/мин const int16_t mtr_max_speed = 5000; int16_t mtr_new_speed; int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ //Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов float magx_cal; magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24); float magy_cal; magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24); float magz_cal; magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24); *magx = (int16_t) magx_cal; *magy = (int16_t) magy_cal; *magz = (int16_t) magz_cal; return 0; } int motor_new_speed_PD(int mtr_speed, int omega, int 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); printf("Enable magnetometer %d\n", mag_num); magnetometer_turn_on(mag_num); Sleep(1); } int switch_off_all(){ /*Функция отключает все приборы,которые будут использоваться в основной программе.*/ printf("Finishing..."); int16_t new_speed = mtr_new_speed; printf("\nDisable angular velocity sensor №%d\n", hyr_num); hyro_turn_off(hyr_num); printf("Disable magnetometer %d\n", mag_num); magnetometer_turn_off(mag_num); motor_set_speed(mtr_num, 0, &new_speed); Sleep (1); motor_turn_off(mtr_num); printf("Finish program\n"); return 0; } int control(){ float mtr_new_speed; //данные магнитометра int16_t mgx_cal=0; int16_t mgy_cal=0; int16_t mgz_cal=0; int16_t *magx_raw = &mgx_cal; int16_t *magy_raw = &mgy_cal; int16_t *magz_raw = &mgz_cal; //данные ДУС int16_t gx_raw; int16_t gy_raw; int16_t gz_raw; int16_t *hyrox_raw=&gx_raw; int16_t *hyroy_raw= &gy_raw; int16_t *hyroz_raw = &gz_raw; int16_t mtr_speed; int16_t omega; int16_t omega_goal=0; // Целевая угловая скорость initialize_all(); int mtr_state = 0; // Инициализируем статус маховика int hyro_state = 0; // Инициализируем статус ДУС int mag_state = 0; // Инициализируем статус магнитометра // int alpha_goal = 0; // Целевой угол double mag_alpha = 0; int i; for (i = 0; i < 500; i++) { mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw); hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); gx_raw = *hyrox_raw; gy_raw = *hyroy_raw; gz_raw = *hyroz_raw; mtr_state = motor_request_speed(mtr_num, &mtr_speed); if (!mag_state){ mag_calibrated(magx_raw,magy_raw,magz_raw); *magy_raw = - *magy_raw;/* переходим из левой системы координат, которая изображена на магнитометре в правую, для того чтобы положительное направление угла было против часовой стрелки*/ mag_alpha = atan2(mgy_cal, mgx_cal)/M_PI*180; printf("magx_cal = %d", mgx_cal); printf(" magy_cal = %d", mgy_cal); printf(" magz_cal = %d", mgz_cal); printf(" mag_alpha atan2 = %f\n", mag_alpha); } else if (mag_state == 1){ printf("Fail because of access error, check the connection\n"); } else if (mag_state == 2){ printf("Fail because of interface error, check your code\n"); } 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("\ngx_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"); } //Обработка показаний маховика и установка требуемой угловой скорости. if (!mtr_state) { // если код ошибки 0, т.е. ошибки нет int16_t mtr_speed=0; motor_request_speed(mtr_num, &mtr_speed); printf("\nMotor_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(); return 0; }
В программе используются следующие функции для работы с Орбикрафт.
magnetometer_turn_on(num)
– функция включения магнитометра, где num – это номер магнитометра.
magnetometer_turn_off(num)
– функция выключения магнитометра, где num – это номер магнитометра.
magnetometer_request_raw(num)
– функция возвращающая сырые данные измеренные магнитометром с номером num, представляющие собой список из 4 числовых значений.
hyro_turn_on(hyr_num)
– функция включения ДУС, где hyr_num – это номер ДУС.
hyro_turn_off(hyr_num)
– функция выключения ДУС, где hyr_num – это номер ДУС.
hyro_request_raw(hyr_num)
– функция возвращающая сырые данные измеренные ДУС с номером hyr_num, представляющие собой список из 4 числовых значений.
motor_turn_on(num)
– функция включения маховика, где num – это номер маховика.
motor_turn_off(num)
– функция выключения маховика, где num – это номер маховика.
motor_set_speed(mtr_num, speed)
– функция устанавливающая скорость вращения маховика с номером mtr_num, в значение speed.
Протестируйте работу программы при следующих значениях целевого угла alpha_goal. 0, 90, -90, 180. При повороте на целевой угол в 180 градусов программа будет работать неправильно. Для устранения бага в работе в программу необходимо добавить функцию трансформации угла angle_transformation. Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal. По умолчанию угол определяется в диапазоне от -180 до 180 градусов с помощью функции atan2. Функция angle_transformation изменяет диапазона измерения угла. Для решения задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до (alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра и проекцией вектора магнитного поля на горизонтальную плоскость.
def angle_transformation(alpha, alpha_goal): if alpha<=(alpha_goal - 180): alpha = alpha + 360 elif alpha>(alpha_goal +180): alpha = alpha - 360 return alpha
Протестируйте работу программы при следующих значениях целевого угла alpha_goal: 0, 90, -90, 180. Теперь при повороте на целевой угол в 180 градусов программа будет работать правильно.