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

Перевод этой страницы:

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


Боковая панель

Для чего нужен ОрбиКрафт

Подсистемы конструктора

Инструкции по работе с ОрбиКрафт

Уроки

Лабораторная оснастка

Знакомство с Arduino

Полезная нагрузка на базе Arduino

Обратная связь

Новости

terra_02

Миссия дистанционного зондирования Земли

Запустите ПО ЦУП (рисунок 1)

1. Нажмите на кнопку Подключить для подключения ПО ЦУП к глобусу по USB

2. Нажмите на кнопку Исходная чтобы установить глобус в исходное положение

3. Нажмите на кнопку Старт непосредственно перед запуском программы в Орбикрафт

Рисунок 1. ПО ЦУП

Определите положение ОрбиКрафт относительно станции приема после установки глобуса в исходное положение.

Это нужно для определения времени задержки, которое необходимо прописать в программе Орбикрафт.

Например, если точка фотографирования находится на меридиане 80, а орбикрафт подвешен напротив меридиана 40, то расстояние между ними равно 40 градусов. Скорость вращения глобуса - один оборот за 5 минут (360 градусов за 600 секунд, или 0,6 градусов в секунду). Следовательно на угол в 40 градусов глобус повернется за 24 секунды. Используйте эту величину в программе Орбикрафт.

Загрузите в Орбикрафт следующий код Код на языке Python для стабилизации Орбикрафт («спутника») и получения фотографий глобуса («Земли»)

Earth_remote_sensing.py
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()

Код на языке Python для стабилизации Орбикрафт («спутника»), получения фотографий глобуса («Земли») и передачи телеметрии по УКВ

Earth_remote_sensing_UHF.py
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" 
	hyro_turn_on(hyr_num) 			# Включаем ДУС
	sleep(1)
	print "Enable magnetometer", mag_num
	magnetometer_turn_on(mag_num)		# Включаем магнитометр
	sleep(1)
	camera_turn_on()			# Включаем камеру 
	sleep(1) 
	transmitter_turn_on(1)
	sleep(1)
	transceiver_turn_on(2) 			# Бортовой УКВ имеет номер 2
	sleep(1) 
 
def switch_off_all(): # Функция выключения всех систем
	print "Finishing..."
	print "Disable angular velocity sensor"
	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)			# Выключаем мотор
	sleep (1)
	camera_turn_off()			# Выключаем камеру
	sleep(1) 
	transmitter_turn_off(1)
	sleep(1) 
	transceiver_turn_off(2)
	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 uhf(msg):
	error = transceiver_send(2, 1, msg)
 
	if not error:
		print "data has been transmitted"
	elif error == 1: # если датчик вернул сообщение об ошибке 1
		print "Fail because of access error, check the connection" 
	elif error == 2: # если датчик вернул сообщение об ошибке 2
		print "Fail because of interface error, check your code"
 
 
# Основная функция программы, в которой вызываются остальные функции.
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
 
	bus_setup();
	#глобус делает оборот в 360 градусов за 600 секунд. Угловая скорость 0.6 градуса в секунду
	# по меридианам определяем угол от орбикрафт до точки приема (у меня 40 градусов)
	# вычисляем задержку 40*0.6=24 сек. И вычитаем 5 сек на поворот спутника 24-5=19
 
	sleep(22) # подбираем вручную время (или вычисляем по формуле в зависимости от угловой скорости глобуса) 
 
	message = "Start mission\n"	
	uhf(message)
 
	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'
 
			message = "Photo\n"
			uhf(message)
 
			message = "Omega = " +str(omega) + '\n'
			uhf(message)
 
			message = "Finish mission\n"
			uhf(message)
 
		sleep(time_step)
 
	switch_off_all()
terra_02.txt · Последние изменения: 2022/03/15 18:04 — ekaterina.manucharova

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