import math time_step = 0.1 # Временной шаг работы алгоритма, с omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. mtr_max_speed = 3500 # Максимально допустимая скорость маховика, об/мин mtr_num = 1 # Номер маховика hyr_num = 1 # Номер ДУС mag_num = 1 # Номер магнитометра alpha_goal = 0 # Целевой угол поворота errora = 0 # Ошибка по углу Pa = 0 # Воздействие пропорционального звена Da = 0 # Воздействие дифференциального звена Ia = 0 # Воздействие интегрального звена Kpa = 20 # Пропорциональный коэфициент ошибки по углу Kda = 0.1 # Дифференциальный коэффициент Kia = 1 # Интегральный коэффициент lastErrora = 0 # Прошлая ошибка по углу Integratora = 0 # Интеграл (сумма всех ошибок по углу) PID_alpha = 0 # Величина управляющего воздействия Integrator_maxa = 10 # Ограничение максимального значения интергатора Integrator_mina = -10 # Ограничение минимального значения интергатора error = 0 # Ошибка по угловой скорости P = 0 # Воздействие пропорционального звена D = 0 # Воздействие дифференциального звена I = 0 # Воздействие интегрального звена Kp = 20 # Пропорциональный коэффициент Kd = 0.1 # Дифференциальный коэффициент Ki = 1 # Интегральный коэффициент lastError = 0 # Прошлая ошибка по угловой скорости Integrator = 0 # Интеграл (сумма всех ошибок по угловой скорости) PID_omega = 0 # Величина управляющего воздействия Integrator_max = 10 # Ограничение максимального значения интергатора Integrator_min = -10 # Ограничение минимального значения интергатора # Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов def mag_calibrated(magx,magy,magz): # коэффициенты после калибровки магнитометра у вас будут другими magx_cal = 1.668867*(magx + -38.326818) + -0.007649*(magy + 20.442007) + 0.113348*(magz + -69.659602) magy_cal = -0.007649*(magx + -38.326818) + 1.749597*(magy + 20.442007) + 0.130999*(magz + -69.659602) magz_cal = 0.113348*(magx + -38.326818) + 0.130999*(magy + 20.442007) + 1.577785*(magz + -69.659602) return magx_cal, magy_cal, magz_cal # Функции для определение новой скорости маховика. # Новая скорость маховика складывается из # текущей скорости маховика mtr_speed и двух приращений скорости PID_omega и PID_alpha # PID_omega - приращение скорости для уменьшения отклонения по угловой скорости # PID_alpha - приращение скорости для уменьшения отклонения по углу # Приращения скорости получены путем сложения трех коэффициентов: # 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, "\t\tI= ", I, "\t\tD= ", D, "\t\tPID_omega= ", PID_omega, "\tmtr_new_speed= ", mtr_new_speed print "Pa= ", Pa, "\tIa= ", Ia, "\t\tDa= ", Da, "\tPID_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) camera_turn_on() #Включаем камеру и передатчик и ждем их загрузки transmitter_turn_on(1) 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) # Выключаем мотор camera_turn_off() #Включаем камеру и передатчик и ждем их загрузки transmitter_turn_off(1) sleep(1) print "Finish program" def angle_transformation(alpha, alpha_goal): if alpha<=(alpha_goal - 180): alpha = alpha + 360 elif alpha>(alpha_goal +180): alpha = alpha - 360 return alpha # Основная функция программы, в которой вызываются остальные функции. def control(): initialize_all() mtr_state = 0 # Инициализируем статус маховика hyro_state = 0 # Инициализируем статус ДУС mag_state = 0 # Инициализируем статус магнитометра alpha_goal = -120 # Целевой угол omega_goal = 0 # Целевая угловая скорость mag_alpha = 0 frame = 0 err = 0 omega = 0 # глобус делает оборот в 360 градусов за 600 секунд. Угловая скорость 0.6 градуса в секунду # по меридианам определяем угол от орбикрафт до точки приема (у меня 40 градусов) # вычисляем задержку 40*0.6=24 сек. И вычитаем 5 сек на поворот спутника 24-5=19 sleep(22) # подбираем вручную время (или вычисляем по формуле в зависимости от угловой скорости глобуса) for i in range(60): # Опрос магнитометра, датчика угловой скорости и маховика 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) mag_alpha = math.atan2(magy_cal, magx_cal)/math.pi*180 mag_alpha = angle_transformation(mag_alpha, alpha_goal) 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: gz_degs = gz_raw * 0.00875 # если ДУС установлен осью z вверх, то угловая скорость # спутника совпадает с показаниями ДУС по оси z, иначе # необходимо изменить знак: omega = - gz_degs omega = gz_degs #если ДУС установлен осью z вверх 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, "\t\ti= ", i, # установка новой скорости маховика mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal) motor_set_speed(mtr_num, mtr_new_speed) if i == 50: # Делаем снимок err_camera = camera_take_photo(frame) if err == 1: print 'Camera access error, check the connection', frame elif err == 2: print 'Camera interface errorr, check the code', frame #Передаем снимок err_transmitter = transmitter_transmit_photo(1, frame) if err_transmitter == 1: print 'Transmition failed' sleep(time_step) switch_off_all()