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

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

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


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

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

Как с этим работать

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

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

Уроки

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

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

Новости

lesson10

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.

Python code.

sun_orient.py
import time
import math
 
kd = 200			# Differential 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			# Proportional feedback coefficient. If the current angle is more than target angle, than satellite must be spun counter-clock-wise (i.e. the flywheel must rotated counter-clock-wise).
time_step = 0.05		# working time-step for the positioning and stabilization system
 
mtr_num = 1			# flywheel number
hyr_num = 1			# angular velocity sensor number
mag_num = 1			# magnetometer number
# Maximum allowed flywheel speed, rev/min
mtr_max_speed = 3000
sun_max = 25000		# Maximum value of the solar sensor (higher values are deemed as errors)
sun_min = 50		# Minimum value of the solar sensor (lower values are deemed as errors)
 
# To define the steering command for the flywheel it is critical to know not the angular orientation of the satellite as related to magnetic field but the angular error – the difference between the current angle and target angle alpha_goal
# By default the angle will be defined in range from -180 to 180 degree 
# angle_transformation function will change the angle measurement range. For the task of positioning the angle must be measured in the range from (alpha_goal-180) to (alpha_goal+180), where alpha_goal is the target angle between magnetometer’s x-axis and projection of magnetic field vector to horizontal plane.
 
def angle_transformation(alpha, alpha_goal):
	if alpha<=(alpha_goal - 180):
		alpha = alpha + 360
	elif alpha>(alpha_goal +180):
		alpha = alpha - 360
	return alpha
 
# Specifying the flywheel new speed 
# flywheel’s new speed equals to flywheel’s current speed + speed increment
# Speed increment is proportioned by angle error and angular velocity error
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) # Switch on the angular velocity sensor
	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)   # Switch off the angular velocity sensor
	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) - array length
		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(): # Program’s main function
	initialize_all()
	# Initialize flywheel status
	mtr_state = 0
	# Initialize angular velocity sensor status		
	hyro_state = 0
	sun_sensor_num = 0 	   # Initialize variable for solar sensor number 
	sun_result_1 = [0,0,0] # Initialize sun_result_1
	sun_result_2 = [0,0,0] # Initialize sun_result_2
	sun_result_3 = [0,0,0] # Initialize sun_result_3
	sun_result_4 = [0,0,0] # Initialize sun_result_4
	sun_result_1_Old = [0,0,0] # Initialize sun_result_1_Old
	sun_result_2_Old = [0,0,0] # Initialize sun_result_2_Old
	sun_result_3_Old = [0,0,0] # Initialize sun_result_3_Old
	sun_result_4_Old = [0,0,0] # Initialize sun_result_4_Old	
 
	omega_goal = 0 		# Target angular velocity
	alpha_goal = 30		# Target turning angle
 
	for i in range(300):
		# Put into memory the old values
		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		
		# sensors and flywheel data request
		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
		# Checking the new measured value for going out of range 
		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: # if angular velocity sensor returned error code 0 (no error)
			gx_degs = gx_raw * 0.00875
			gy_degs = gy_raw * 0.00875
			gz_degs = gz_raw * 0.00875
			omega = gz_degs	# if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis
			print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Выводим данные
		elif hyro_state == 1: # if the sensor returned error code 1
			print "Fail because of access error, check the connection"
		elif hyro_state == 2: # if the sensor returned error code 2
			print "Fail because of interface error, check your code"
 
		if not mtr_state:	# if the flywheel returned error code 0 (no error)
			print "Motor_speed: ", mtr_speed
			mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun,alpha_goal,gz_degs,omega_goal)	# set the flywheel’s new speed 
			motor_set_speed(mtr_num, mtr_new_speed)
 
		sleep(time_step)
	switch_off_all()

С code.

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; /*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.*/
const int kp = -100; /*Proportional feedback coefficient. If the current angle is more than target angle, than satellite must be spun counter-clock-wise (i.e. the flywheel must rotated counter-clock-wise).*/
const float time_step = 0.05; //working time-step for the positioning and stabilization system 
const int mtr_max_speed = 3000; 		// Maximum allowed flywheel speed, rev/min
const uint16_t mtr_num = 1;			// flywheel number
const uint16_t hyr_num = 1; 		        // angular velocity sensor number
const uint16_t mag_num = 1;			// magnetometer number
 
const int sun_max = 25000;//Maximum value of the solar sensor (higher values are deemed as errors)
const int sun_min = 50;//Minimum value of the solar sensor (lower values are deemed as errors)
int16_t mtr_new_speed;
 
int angle_transformation(int alpha, int alpha_goal){
/* To define the steering command for the flywheel it is critical to know not the angular orientation of the satellite as related to magnetic field but the angular error – the difference between the current angle and target angle alpha_goal
By default the angle will be defined in range from -180 to 180 degree angle_transformation function will change the angle measurement range. For the task of positioning the angle must be measured in the range from (alpha_goal-180) to (alpha_goal+180), where alpha_goal is the target angle between magnetometer’s x-axis and projection of magnetic field vector to horizontal plane.
*/
	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){
/* Specifying the flywheel new speed 
flywheel’s new speed equals to flywheel’s current speed + speed increment
Speed increment is proportioned by angle error and angular velocity error
*/
	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){/* All systems’ initialization function*/
	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){/* All systems’ switching off function*/
	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}; //insert data from text data file
	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(){ // Program’s main function
	initialize_all(); // Initialize flywheel status
	int mtr_state = 0;  // Initialize angular velocity sensor status
	int hyro_state = 0;
	//int mag_state = 0; // Initialize magnetometer status
	uint16_t sun_result_1[] = {0,0,0}; // Initialize sun_result_1
	uint16_t sun_result_2[] = {0,0,0}; // Initialize sun_result_2
	uint16_t sun_result_3[] = {0,0,0}; // Initialize sun_result_3
	uint16_t sun_result_4[] = {0,0,0}; // Initialize sun_result_4
	uint16_t sun_result_1_Old[] = {0,0,0}; // Initialize sun_result_1_Old
	uint16_t sun_result_2_Old[] = {0,0,0}; // Initialize sun_result_2_Old
	uint16_t sun_result_3_Old[] = {0,0,0}; // Initialize sun_result_3_Old
	uint16_t sun_result_4_Old[] = {0,0,0}; // Initialize sun_result_4_Old
	int16_t mtr_speed;
	//angular velocity sensor data
	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;
	//magnetometer data
	/*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 - Target angular velocity
	int alpha_goal = 30;		// Target turning angle
	int i;
	int j;
	for (i = 0; i < 300; i++){
		// Put into memory the old values
		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];
		}		
		// sensors and flywheel data request
		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
		// Checking the new measured value for going out of range 
		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){ // if angular velocity sensor returned error code 0 (no error)
			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;	// if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis
			printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Data output
		}
		else if (hyro_state == 1){ // if the sensor returned error code 1
			printf("Fail because of access error, check the connection");
		}
		else if (hyro_state == 2){ // if the sensor returned error code 2
			printf("Fail because of interface error, check your code");
		}
 
		if (!mtr_state)	{ // if the flywheel returned error code 0 (no error)
			int16_t mtr_speed = 0;
			motor_request_speed(mtr_num, &mtr_speed);
			printf("Motor_speed: %d\n", mtr_speed);	
			// set the flywheel’s new 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;
}

Program’s method of procedure is:

At the large data array there stored the values measured during solar data calibration and angle values. check_sun function receives 8 values read from solar sensors (two from each sensor), calculates 8 deviation values delta1…delta8 as absolute value of remainder between measured values and values from the table. Then there calculated total deviation delta_sum. The program processes all values from the array searching the minimum deviation delta_sum with corresponding angle value. As a result the check_sun function returns angle value at which ОrbiCraft turned. sun_range function rejects erroneous values received from solar sensor; if the measured value is higher than 25000 or lower than 50 (higher than sun_max or lower than sun_min), than the program will return the old value from allowed range. The use of this function allows to amend the measurement errors that arises sometimes. The program’s main function control reads 300 times in cycle the readings of solar sensors checking that the values are not out of range via sun_range function. The correct values are transferred to the check_sun function that returns the angle value at which ОrbiCraft turned. Than via angle_transformation function there performed the transformation of the angle value to the range of -180…180, and via motor_new_speed_PD function there are calculated the new speed which is required for flywheels rotation. Than program operation is paused for the time_step time.

lesson10.txt · Последние изменения: 2019/11/07 15:41 — golikov

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