Здесь показаны различия между двумя версиями данной страницы.
Предыдущая версия справа и слева Предыдущая версия Следующая версия | Предыдущая версия | ||
lesson7 [2019/05/14 11:25] golikov [Программа ориентации по магнитометру] |
lesson7 [2020/02/17 15:14] golikov |
||
---|---|---|---|
Строка 10: | Строка 10: | ||
import math | import math | ||
- | kd = 200 # К-т дифференциальной обратной связи. Если угловая скорость спутника положительна, то спутник надо раскручивать по часовой стрелки, т.е. маховик надо разгонять по часовой стрелке. | + | time_step = 0.1 # Временной шаг работы алгоритма, с |
- | kp = -50 # К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки. | + | omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. |
- | time_step = 0.05 # Временной шаг работы | + | alpha_goal = 0 # Целевой угол поворота |
+ | mtr_max_speed = 5000 # Максимально допустимая скорость маховика, об/мин | ||
+ | mtr_num = 1 # Номер маховика | ||
+ | hyr_num = 1 # Номер ДУС | ||
+ | mag_num = 1 # Номер магнитометра | ||
- | mtr_num = 1 # Номер маховика | + | |
- | hyr_num = 1 # Номер ДУС | + | errora = 0 # Ошибка по углу |
- | mag_num = 1 # Номер магнитометра | + | 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 вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | ||
def mag_calibrated(magx,magy,magz): | def mag_calibrated(magx,magy,magz): | ||
- | magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24) | + | magx_cal = 1.451626*(magx + 47.687257) + 0.036309*(magy + -1468.312497) + 0.008968*(magz + 48.449050) |
- | magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24) | + | magy_cal = 0.036309*(magx + 47.687257) + 0.408031*(magy + -1468.312497) + 0.074774*(magz + 48.449050) |
- | magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24) | + | magz_cal = 0.008968*(magx + 47.687257) + 0.074774*(magy + -1468.312497) + 1.429262*(magz + 48.449050) |
return magx_cal, magy_cal, magz_cal | return magx_cal, magy_cal, magz_cal | ||
- | + | ||
- | # Функция motor_new_speed_PD определяет новую скорость маховика | + | # Функции для определение новой скорости маховика. |
- | # Новая скорость маховика равна текущей скорости маховика + приращение скорости | + | # Новая скорость маховика складывается из |
- | # Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости | + | # текущей скорости маховика 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): | def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal): | ||
- | mtr_new_speed = int(mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal)) | + | global Integrator |
- | return mtr_new_speed | + | global lastError |
- | # return 0 | + | 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(): # Функция инициализации всех систем | def initialize_all(): # Функция инициализации всех систем | ||
print "Enable motor №", mtr_num | print "Enable motor №", mtr_num | ||
motor_turn_on(mtr_num) | motor_turn_on(mtr_num) | ||
sleep(1) | sleep(1) | ||
- | |||
print "Enable angular velocity sensor №", hyr_num | print "Enable angular velocity sensor №", hyr_num | ||
hyro_turn_on(hyr_num) # Включаем ДУС | hyro_turn_on(hyr_num) # Включаем ДУС | ||
sleep(1) | sleep(1) | ||
- | |||
print "Enable magnetometer", mag_num | print "Enable magnetometer", mag_num | ||
magnetometer_turn_on(mag_num) | magnetometer_turn_on(mag_num) | ||
Строка 56: | Строка 128: | ||
motor_turn_off(mtr_num) | motor_turn_off(mtr_num) | ||
print "Finish program" | print "Finish program" | ||
- | + | ||
- | def control(): # основная функция программы | + | |
+ | # Основная функция программы, в которой вызываются остальные функции. | ||
+ | def control(): | ||
initialize_all() | initialize_all() | ||
mtr_state = 0 # Инициализируем статус маховика | mtr_state = 0 # Инициализируем статус маховика | ||
hyro_state = 0 # Инициализируем статус ДУС | hyro_state = 0 # Инициализируем статус ДУС | ||
mag_state = 0 # Инициализируем статус магнитометра | mag_state = 0 # Инициализируем статус магнитометра | ||
- | alpha_goal = 0 # Целевой угол | + | alpha_goal = 90 # Целевой угол |
omega_goal = 0 # Целевая угловая скорость | omega_goal = 0 # Целевая угловая скорость | ||
mag_alpha = 0 | mag_alpha = 0 | ||
- | + | ||
- | for i in range(500): | + | for i in range(200): |
- | # опрос датчиков и маховика | + | #print "i = ", i |
+ | |||
+ | # Опрос датчика угловой скорости и маховика. | ||
mag_state, magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) | 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) | hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) | ||
- | mtr_state, mtr_speed = motor_request_speed(mtr_num) | + | mtr_state, mtr_speed = motor_request_speed(mtr_num) |
if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет | if not mag_state: # если магнитометр вернул код ошибки 0, т.е. ошибки нет | ||
magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw) | magx_cal, magy_cal, magz_cal = mag_calibrated(magx_raw,magy_raw,magz_raw) | ||
- | magy_cal = - magy_cal # переходим из левой системы координат, которая изображена на магнитометре в правую, для того чтобы | + | magy_cal = - magy_cal # переходим из левой системы координат, которая изображена на магнитометре в правую |
- | положительное направление угла было против часовой стрелки | + | |
mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180 | mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180 | ||
- | print "magx_cal =", magx_cal, "magy_cal =", magy_cal, "magz_cal =", magz_cal # Вывод откалиброванных значений магнитометра | + | #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 | print "mag_alpha atan2= ", mag_alpha | ||
elif mag_state == 1: | elif mag_state == 1: | ||
Строка 83: | Строка 159: | ||
elif mag_state == 2: | elif mag_state == 2: | ||
print "Fail because of interface error, check your code" | print "Fail because of interface error, check your code" | ||
- | + | | |
- | if not hyro_state: # если ДУС вернул код ошибки 0, т.е. ошибки нет | + | # Обработка показаний датчика угловой скорости, |
+ | # вычисление угловой скорости спутника по показаниям ДУС. | ||
+ | # Если код ошибки ДУС равен 0, т.е. ошибки нет | ||
+ | if not hyro_state: | ||
gx_degs = gx_raw * 0.00875 | gx_degs = gx_raw * 0.00875 | ||
gy_degs = gy_raw * 0.00875 | gy_degs = gy_raw * 0.00875 | ||
gz_degs = gz_raw * 0.00875 | gz_degs = gz_raw * 0.00875 | ||
- | omega = gz_degs # если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z | + | # если ДУС установлен осью z вверх, то угловая скорость |
- | print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные | + | # спутника совпадает с показаниями ДУС по оси z, иначе |
- | elif hyro_state == 1: # если датчик вернул сообщение об ошибке 1 | + | # необходимо изменить знак: 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" | print "Fail because of access error, check the connection" | ||
- | elif hyro_state == 2: # если датчик вернул сообщение об ошибке 2 | + | elif hyro_state == 2: |
print "Fail because of interface error, check your code" | print "Fail because of interface error, check your code" | ||
- | + | ||
- | if not mtr_state: # если маховик вернул код ошибки 0, т.е. ошибки нет | + | #Обработка показаний маховика и установка требуемой угловой скорости. |
+ | if not mtr_state: # если код ошибки 0, т.е. ошибки нет | ||
print "Motor_speed: ", mtr_speed | print "Motor_speed: ", mtr_speed | ||
- | mtr_new_speed = motor_new_speed_PD(mtr_speed,mag_alpha,alpha_goal,gz_degs,omega_goal) # установка новой скорости маховика | + | # установка новой скорости маховика |
+ | mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal) | ||
motor_set_speed(mtr_num, mtr_new_speed) | motor_set_speed(mtr_num, mtr_new_speed) | ||
- | + | ||
sleep(time_step) | sleep(time_step) | ||
+ | |||
switch_off_all() | switch_off_all() | ||
</file> | </file> | ||
Строка 123: | Строка 208: | ||
const float time_step = 0.05; // Временной шаг работы | const float time_step = 0.05; // Временной шаг работы | ||
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; // Номер магнитометра |
// Максимально допустимая скорость маховика, об/мин | // Максимально допустимая скорость маховика, об/мин | ||
const int16_t mtr_max_speed = 5000; | const int16_t mtr_max_speed = 5000; | ||
Строка 168: | Строка 253: | ||
/* Функция инициализации всех систем, | /* Функция инициализации всех систем, | ||
включает все приборы,которые будут использоваться в основной программе.*/ | включает все приборы,которые будут использоваться в основной программе.*/ | ||
- | printf ("Enable motor №%d\n", mtr_num); //а нужнали ссылка?? | + | printf ("Enable motor №%d\n", mtr_num); |
motor_turn_on(mtr_num); | motor_turn_on(mtr_num); | ||
Sleep(1); | Sleep(1); |