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

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


lesson10

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

lesson10 [2019/11/07 15:40]
golikov [10 Урок Ориентация Орбикрафт по солнечным датчикам]
lesson10 [2020/03/25 16:28]
Строка 1: Строка 1:
-====== OrbiCraft positioning with solar sensors ====== 
-===== Загрузка программы ​ ===== 
-Откройте файл sun_orient.py и вставьте в массив data_sun = [] все данные из текстового файла с данными,​ и удалите в конце массива пробел и запятую. 
- 
-{{::​10image001.png?​nolink&​200|}} 
- 
-Код на Python. 
-<file python 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() 
-</​file>​ 
- 
-С code. 
-<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; /​*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; 
-} 
-</​file>​ 
- 
- 
- 
- 
-===== 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 · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

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