10 Урок. Ориентация Орбикрафт по солнечным датчикам

Загрузка программы

Откройте файл SUN.py и введите в массив данных data_sun = [] все данные из текстового файла данных; удалите пробелы и запятые в конце массива.

Код программы на Python

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()

Код программы на 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;
}

Процедура программы заключается в следующем:

В большом массиве данных хранятся значения, измеренные во время калибровки солнечных данных и угловые значения. Функция 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.