import time import math kd = 200 # Incremental feedback coefficient. If the satellite’s angular velocity is positive, than satellite must be spun clock-wise (i.e. the flywheel must rotated clock-wise. kp = -100 # К-т пропорциональной обратной связи. Если текущий угол больше целевого, то спутник надо вращать против часовой стрелки, соответственно маховик надо разгонять против часовой стрелки. time_step = 0.05 # Временной шаг работы системы ориентации и стабилизации mtr_num = 1 # Номер маховика hyr_num = 1 # Номер ДУС mag_num = 1 # Номер магнитометра # Максимально допустимая скорость маховика, об/мин mtr_max_speed = 3000 sun_max = 25000 # Максимальное значение солнечного датчика больше которого считаем ошибкой sun_min = 50 # Минимальное значение солнечного датчика меньше которого считаем ошибкой # Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal # По умолчанию угол определяется в диапазоне от -180 до 180 градусов # Функция 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 # Определение новой скорости маховика # Новая скорость маховика равна текущей скорости маховика + приращение скорости # Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости 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)) 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 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 Sun sensors 1-4" sun_sensor_turn_on(1) sun_sensor_turn_on(2) sun_sensor_turn_on(3) sun_sensor_turn_on(4) sleep(1) def switch_off_all(): print "Finishing..." hyro_turn_off(hyr_num) # Выключаем ДУС sun_sensor_turn_off(1) sun_sensor_turn_off(2) sun_sensor_turn_off(3) sun_sensor_turn_off(4) motor_set_speed(mtr_num, 0) sleep (1) motor_turn_off(mtr_num) print "Finish program" def check_sun(sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42): delta1 = 0 delta2 = 0 delta3 = 0 delta4 = 0 delta5 = 0 delta6 = 0 delta7 = 0 delta8 = 0 delta_sum = 36000 delta_min = 35000 delta_i = 0 data_sun = [] for i in range(0, len(data_sun), 9): # len(data_sun) - длина массива delta1 = abs(sun11 - data_sun[i+0]) delta2 = abs(sun12 - data_sun[i+1]) delta3 = abs(sun21 - data_sun[i+2]) delta4 = abs(sun22 - data_sun[i+3]) delta5 = abs(sun31 - data_sun[i+4]) delta6 = abs(sun32 - data_sun[i+5]) delta7 = abs(sun41 - data_sun[i+6]) delta8 = abs(sun42 - data_sun[i+7]) delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8 #print i, data_sun[i+7] #print "Sun_sens", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42 #print "deltas= ", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum if delta_min > delta_sum : delta_min = delta_sum delta_i = data_sun[i+8] #print "delta_min= ", delta_min, "delta_i= ", delta_i return delta_i def sun_range(sun, sun_old): if sun[1] > sun_max: sun[1] = sun_old[1] if sun[1] < sun_min: sun[1] = sun_old[1] if sun[2] > sun_max: sun[2] = sun_old[2] if sun[2] < sun_min: sun[2] = sun_old[2] return sun def control(): # основная функция программы initialize_all() # Инициализируем статус маховика mtr_state = 0 # Инициализируем статус ДУС hyro_state = 0 sun_sensor_num = 0 # Инициализируем переменную для номера солнечного датчика sun_result_1 = [0,0,0] # Инициализируем sun_result_1 sun_result_2 = [0,0,0] # Инициализируем sun_result_2 sun_result_3 = [0,0,0] # Инициализируем sun_result_3 sun_result_4 = [0,0,0] # Инициализируем sun_result_4 sun_result_1_Old = [0,0,0] # Инициализируем sun_result_1 sun_result_2_Old = [0,0,0] # Инициализируем sun_result_2 sun_result_3_Old = [0,0,0] # Инициализируем sun_result_3 sun_result_4_Old = [0,0,0] # Инициализируем sun_result_4 omega_goal = 0 # Целевая угловая скорость alpha_goal = 30 # Целевой угол поворота for i in range(300): # Запоминаем старые значения sun_result_1_Old = sun_result_1 sun_result_2_Old = sun_result_2 sun_result_3_Old = sun_result_3 sun_result_4_Old = sun_result_4 # опрос датчиков и маховика sun_result_1 = sun_sensor_request_raw(1) sun_result_2 = sun_sensor_request_raw(2) sun_result_3 = sun_sensor_request_raw(3) sun_result_4 = sun_sensor_request_raw(4) #print sun_result_1, sun_result_2, sun_result_3, sun_result_4 # Проверяем чтобы новое измеренное значение не выходило за пределы диапазона sun_result_1 = sun_range(sun_result_1, sun_result_1_Old) sun_result_2 = sun_range(sun_result_2, sun_result_2_Old) sun_result_3 = sun_range(sun_result_3, sun_result_3_Old) sun_result_4 = sun_range(sun_result_4, sun_result_4_Old) alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2]) alpha_sun = angle_transformation(alpha_sun, alpha_goal) print alpha_sun hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) if not hyro_state: # если ДУС вернул код ошибки 0, т.е. ошибки нет gx_degs = gx_raw * 0.00875 gy_degs = gy_raw * 0.00875 gz_degs = gz_raw * 0.00875 omega = gz_degs # если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные elif hyro_state == 1: # если датчик вернул сообщение об ошибке 1 print "Fail because of access error, check the connection" elif hyro_state == 2: # если датчик вернул сообщение об ошибке 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,alpha_sun,alpha_goal,gz_degs,omega_goal) # установка новой скорости маховика motor_set_speed(mtr_num, mtr_new_speed) sleep(time_step) switch_off_all()