User Tools

Site Tools


en:lesson10

Differences

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

Link to this comparison view

en:lesson10 [2019/11/07 16:45]
golikov
en:lesson10 [2020/03/25 16:28]
Line 1: Line 1:
-====== Lesson 10. 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. ​ 
- 
  
en/lesson10.txt · Last modified: 2020/03/25 16:28 (external edit)