What is Orbicraft for?
How to work with it
Orbicraft Subsystems
Arduino-Based payload
Lessons
Laboratory equipment
Feedback
News
What is Orbicraft for?
How to work with it
Orbicraft Subsystems
Arduino-Based payload
Lessons
Laboratory equipment
Feedback
News
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 # 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()
Код на С.
#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; }
В огромном массиве с данными хранятся значения, измеренные при калибровке солнечных датчиков и значения углов. Функция 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.