Differences

This shows you the differences between two versions of the page.

--- en:lesson10 [2019/11/07 13:54]
golikov created
+++ en:lesson10 [2020/03/25 16:28]
@@ Line 1: / Line 1: @@
-FIXME **This page is not fully translated, yet. Please help completing the translation.**\\ //(remove this paragraph once the translation is finished)//
-====== OrbiCraft positioning with solar sensors ======
-===== Program loading  =====
-Open the file SUN.py and enter to the array data_sun = [] all data from the data text file; delete the space and comma characters at the end of array.
-{{::10image001.png?nolink&200|}}
-Python code.
-<file python sun_orient.py>
-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()
-</file>
-Код на С.
-<file c sun_orient.c>
-#include "libschsat.h"
-#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 = -100; /*К-т пропорциональной обратной связи.
-Если текущий угол больше целевого,
-то спутник надо вращать против часовой стрелки,
-соответственно маховик надо разгонять против часовой стрелки.*/
-const float time_step = 0.05; //Временной шаг работы системы ориентации и стабилизации
-const int mtr_max_speed = 3000; 		// Максимально допустимая скорость маховика, об/мин
-const uint16_t mtr_num = 1;			// Номер маховика
-const uint16_t hyr_num = 1; 		        // Номер ДУС
-const uint16_t mag_num = 1;			// Номер магнитометра
-const int sun_max = 25000;//Максимальное значение солнечного датчика больше которого считаем ошибкой
-const int sun_min = 50;//Минимальное значение солнечного датчика меньше которого считаем ошибкой
-int16_t mtr_new_speed;
-int angle_transformation(int alpha, int alpha_goal){
-	/* Для определения команды управления для маховика важно знать не угол разворота спутника относительно магнитного поля, а угловую ошибку - разницу между текущим углом, и целевым углом alpha_goal
-// По умолчанию угол определяется в диапазоне от -180 до 180 градусов
-// Функция angle_transformation изменяет диапазона измерения угла. Для решения
-задачи ориентации угол необходимо измерять в диапазоне от (alpha_goal-180) до
-(alpha_goal+180), где alpha_goal - это целевой угол, между осью x магнитометра
-и проекцией вектора магнитного поля на горизонтальную плоскость.*/
-	if (alpha <= (alpha_goal - 180)){
-		alpha = alpha + 360;
-	}
-	else if (alpha>(alpha_goal +180)){
-		alpha = alpha - 360;
-	}
-	return alpha;
-}
-int motor_new_speed_PD(int mtr_speed,int alpha,int alpha_goal, float omega, int omega_goal){
-	/* Определение новой скорости маховика
-// Новая скорость маховика равна текущей скорости маховика + приращение скорости
-// Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости*/
-	int mtr_new_speed = mtr_speed + kp*(alpha-alpha_goal) + 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 Sun sensors 1-4\n");
-	sun_sensor_turn_on(1);
-	sun_sensor_turn_on(2);
-	sun_sensor_turn_on(3);
-	sun_sensor_turn_on(4);
-	Sleep(1);
-}
-void switch_off_all(void){/* Функция отключает все приборы,
- которые будут использоваться в основной программе.*/
-	printf("Finishing...\n");
-	int16_t new_speed = mtr_new_speed;
-	hyro_turn_off(hyr_num); // Выключаем ДУС
-	magnetometer_turn_off(mag_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, &new_speed);
-	Sleep(1);
-	motor_turn_off(mtr_num);
-	printf("Finish program\n");
-}
-int check_sun(int sun11, int sun12, int sun21, int sun22, int sun31, int sun32, int sun41, int sun42){
-	int delta1 = 0;
-	int delta2 = 0;
-	int delta3 = 0;
-	int delta4 = 0;
-	int delta5 = 0;
-	int delta6 = 0;
-	int delta7 = 0;
-	int delta8 = 0;
-	int delta_sum = 36000;
-	int delta_min = 35000;
-	int delta_i = 0;
-	float data_sun[] = {-47.329501295977, 239, 102, -48.1458278265892}; //вставить данные из текстового файла с данными
-	int i;
-	for (i = 0; i < 4; i++){
-		delta1 = abs(sun11 - data_sun[i]);
-		delta2 = abs(sun12 - data_sun[i]);
-		delta3 = abs(sun21 - data_sun[i]);
-		delta4 = abs(sun22 - data_sun[i]);
-		delta5 = abs(sun31 - data_sun[i]);
-		delta6 = abs(sun32 - data_sun[i]);
-		delta7 = abs(sun41 - data_sun[i]);
-		delta8 = abs(sun42 - data_sun[i]);
-		delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8;
-		printf("%d, %f\n", i, data_sun[i]);
-		printf("Sun_sens: %d, %d, %d, %d, %d, %d, %d, %d\n", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42);
-		printf("deltas= %d, %d, %d, %d, %d, %d, %d, %d, %d\n", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum);
-		if (delta_min > delta_sum) {
-			delta_min = delta_sum;
-			delta_i = data_sun[i];
-			printf("delta_min= %d\n", delta_min);
-			printf("delta_i= %d\n", delta_i);
-		}
-	}
-	return delta_i;
-}
-int sun_range(uint16_t *sun_1, uint16_t *sun_2, uint16_t sun_old_1, uint16_t sun_old_2){
-	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 0;
-}
-int control(){ // основная функция программы
-	initialize_all(); // Инициализируем статус маховика
-	int mtr_state = 0;  // Инициализируем статус ДУС
-	int hyro_state = 0;
-	//int mag_state = 0; //Инициализируем статус магнитометра
-	uint16_t sun_result_1[] = {0,0,0}; // Инициализируем sun_result_1
-	uint16_t sun_result_2[] = {0,0,0}; // Инициализируем sun_result_2
-	uint16_t sun_result_3[] = {0,0,0}; // Инициализируем sun_result_3
-	uint16_t sun_result_4[] = {0,0,0}; // Инициализируем sun_result_4
-	uint16_t sun_result_1_Old[] = {0,0,0}; // Инициализируем sun_result_1
-	uint16_t sun_result_2_Old[] = {0,0,0}; // Инициализируем sun_result_2
-	uint16_t sun_result_3_Old[] = {0,0,0}; // Инициализируем sun_result_3
-	uint16_t sun_result_4_Old[] = {0,0,0}; // Инициализируем sun_result_4
-	int16_t mtr_speed;
-	//данные ДУС
-	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 mgx;
-	int16_t mgy;
-	int16_t mgz;
-	int16_t *magx_raw = &mgx;
-	int16_t *magy_raw = &mgy;
-	int16_t *magz_raw = &mgz;*/
-	int16_t omega;
-	int omega_goal = 0;		// omega_goal - целевая угловая скорость спутника, град/с
-	int alpha_goal = 30;		// Целевой угол поворота
-	int i;
-	int j;
-	for (i = 0; i < 300; i++){
-		// Запоминаем старые значения
-		for(j = 0; j < 3; j++){
-		sun_result_1_Old[j] = sun_result_1[j];
-		sun_result_2_Old[j] = sun_result_2[j];
-		sun_result_3_Old[j] = sun_result_3[j];
-		sun_result_4_Old[j] = sun_result_4[j];
-		}
-		// опрос датчиков и маховика
-		sun_result_1[0] = sun_sensor_request_raw(1,&sun_result_1[1],&sun_result_1[2]);
-		sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]);
-		sun_result_3[0] = sun_sensor_request_raw(3,&sun_result_3[1],&sun_result_3[2]);
-		sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]);
-		//print sun_result_1, sun_result_2, sun_result_3, sun_result_4
-		// Проверяем чтобы новое измеренное значение не выходило за пределы диапазона
-		sun_result_1[0] = sun_range( & sun_result_1[1], & sun_result_1[2], sun_result_1_Old[1], sun_result_1_Old[2]);
-		sun_result_2[0] = sun_range( & sun_result_2[1], & sun_result_2[2], sun_result_2_Old[1], sun_result_2_Old[2]);
-		sun_result_3[0] = sun_range( & sun_result_3[1], & sun_result_3[2], sun_result_3_Old[1], sun_result_3_Old[2]);
-		sun_result_4[0] = sun_range( & sun_result_4[1], & sun_result_4[2], sun_result_4_Old[1], sun_result_4_Old[2]);
-		int 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);
-		printf("alpha_sun = %d\n",alpha_sun);
-		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);
-		//mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);
-		mtr_speed = motor_request_speed(mtr_num, &mtr_speed);
-		if (!hyro_state){ // если ДУС вернул код ошибки 0, т.е. ошибки нет
-			float gx_degs = gx_raw * 0.00875;
-			float gy_degs = gy_raw * 0.00875;
-			float gz_degs = gz_raw * 0.00875;
-			omega = (int16_t)gz_degs;	// если ДУС установлен осью z вверх, то угловая скорость спутника совпадает с показаниями ДУС по оси z
-			printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Выводим данные
-		}
-		else if (hyro_state == 1){ // если датчик вернул сообщение об ошибке 1
-			printf("Fail because of access error, check the connection");
-		}
-		else if (hyro_state == 2){ // если датчик вернул сообщение об ошибке 2
-			printf("Fail because of interface error, check your code");
-		}
-		if (!mtr_state)	{ // если код ошибки 0, т.е. ошибки нет
-			int16_t mtr_speed = 0;
-			motor_request_speed(mtr_num, &mtr_speed);
-			printf("Motor_speed: %d\n", mtr_speed);
-			// установка новой скорости маховика
-			mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun, alpha_goal,omega,omega_goal );
-			motor_set_speed(mtr_num, mtr_new_speed, &omega);
-		}
-		Sleep(time_step);
-	}
-	switch_off_all();
-	return 0;
-}
-</file>
-===== Принцип работы программы =====
-В огромном массиве с данными хранятся значения, измеренные при калибровке солнечных датчиков и значения углов. Функция check_sun получает 8 значений, считанных с солнечных датчиков (по два с каждого), и вычисляет 8 значений отклонений delta1…delta8, как модуль разности между измеренными значениями и значениями из таблицы. Затем вычисляется суммарное отклонение delta_sum. Программа проходит по всему массиву значений и ищет минимальное отклонение delta_sum и соответствующее значение угла. В результате функция check_sun возвращает значение угла, на который повернут Орбикрафт.
-Функция sun_range отбраковывает ошибочные значения, полученные от солнечного датчика, и если измеренное значение больше 25000 или меньше 50 (больше sun_max  или меньше sun_min), то  она вернет старое значение из нормального диапазона. Применение этой функции позволяет избавиться от ошибок измерения, которые иногда возникают.
-Основная функция программы control 300 раз в цикле считывает значения с солнечных датчиков и проверяет чтобы значения не выходили за пределы диапазона с помощью функции sun_range. Корректные значения передаются в функцию check_sun, которая возвращает значение угла, на который повернут Орбикрафт.
-Затем с помощью функции angle_transformation происходит трансформация угла к диапазону -180…180, и с помощью функции motor_new_speed_PD вычисляется новая скорость с которой необходимо вращаться маховику.
-Затем работа программы приостанавливается на время time_step.

Конструктор спутника "ОрбиКрафт"

User Tools

Site Tools

Differences

Page Tools