Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Это старая версия документа!
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.
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.
#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; }
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.